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

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

本章定位：本章是力控与柔顺控制系列（F01-F10，28 周）的起点。从"为什么位置控制不够"出发，建立力控的第一性原理——Hogan 1985 的阻抗/导纳二分法，然后通过二端口网络模型统一理解所有力控范式，最终给出工业应用场景地图和与足式方向（Ch53 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$ 的物理含义？	控制理论基础

本章目标¶

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

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

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 位置控制失败的物理分析¶

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

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

\[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.3 从三个认知跨越理解力控的必要性¶

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

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

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

1.4 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^{cart} = 500$ N/m）让末端跟踪一条经过刚性桌面内部 2mm 的轨迹。环境刚度 $K_e = 10^5$ N/m。计算稳态接触力约为多少？如果标定误差是 1mm 呢？
⭐ 约束分析：画出以下三个任务的约束空间分解和选择矩阵 $S$：(a) 拧瓶盖（6D），(b) 插 USB（6D），(c) 按电梯按钮（3D）。
⭐⭐ 位控极限：为什么不能通过降低位置控制增益 $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 阻抗控制与导纳控制——因果性对偶¶

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.5 "控制关系"的深层含义¶

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

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

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

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

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

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

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

2.6 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 的阻抗控制是刚度控制的完整版。

练习¶

⭐ 频率响应：对阻抗传递函数 $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 图（幅频和相频）。标注自然频率和阻尼比。
⭐⭐ 导纳实现：写出导纳控制器的离散化伪代码：从读取力传感器到输出位置修正 $\Delta x$。考虑采样周期 $T = 1$ ms。
⭐⭐ 哲学辨析：用你自己的话解释"控制阻抗"与"控制力"的根本区别。举一个日常生活中你实际使用"阻抗控制"的例子（提示：想想你如何拧开一个不确定松紧程度的瓶盖）。

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 离散化与采样对无源性的破坏¶

在连续时间中，标准阻抗控制器是无源的。但当控制器以离散时间实现（采样周期 $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.6 接触稳定性的频域直觉¶

为了对接触稳定性建立更深的直觉，我们画出机器人阻抗 $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$，连续时间阻抗控制就是无源的。离散化带来的问题本质上是"虚拟阻尼"在采样间隔内的"缺失"。

练习¶

⭐⭐ 二端口推导：对一个简单的 1-DOF 阻抗控制系统（$Z_r = Ms + D + K/s$），推导其与弹簧环境（$Z_e = K_e/s$）耦合后的闭环特征方程。分析稳定性条件。
⭐⭐ 无源性验证：证明标准阻抗 $Z(s) = Ms + D + K/s$（$M, D, K > 0$）是正实的（即 $\text{Re}[Z(j\omega)] \geq 0, \forall \omega$）。提示：代入 $s = j\omega$ 并取实部。
⭐⭐⭐ 离散化影响：取 $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	±0.5 N	$3000-15000	所有通用平台
关节力矩传感器	应变片（谐波减速器输出侧）	~3 kHz	±0.1 Nm	内置	Franka, KUKA iiwa
电流反推	电机电流→力矩（$\tau = K_t i$）	~10 kHz	±5%（含摩擦）	$0	MIT Cheetah, QDD 臂
动量观测器	广义动量偏差→外力估计	~500 Hz	±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 出厂标定后的精度约为 ±0.5 N，但随使用磨损可能退化。

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
天然范式	阻抗控制	阻抗控制	导纳控制
力估计精度	±0.5 N（关节反推）	±0.5 N（关节反推）	±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 做力控至关重要。

练习¶

⭐ 重力补偿：一个末端安装了 0.5 kg 工具的 F/T 传感器。机器人当前姿态使工具重心在传感器坐标系 z 轴正下方。写出重力补偿公式，计算补偿前后 z 方向力读数的差异。
⭐⭐ 观测器调参：动量观测器增益 $K_I$ 选大了会怎样？选小了呢？如果关节摩擦辨识误差为 0.5 Nm，$K_I = 50$ 时外力估计的稳态偏差约为多少？
⭐⭐ 方案选型：你要为一个 UR5e 搭建一个打磨系统（法向力精度要求 ±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)

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 策略"。

练习¶

⭐ 分类练习：将以下算法归入分类树的正确位置：(a) libfranka cartesian_impedance_control，(b) ros2_controllers admittance_controller，(c) MIT Cheetah 的 WBIC，(d) SERL，(e) Whitney 的 RCC。
⭐⭐ 频率选择：一个接触任务的接触动力学带宽约为 200 Hz。根据 Nyquist 定理，力控环的最低采样率应为多少？如果你的控制器只能跑在 500 Hz，这会对哪些类型的接触任务产生影响？
⭐⭐ 三层设计：为一个 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.7 质量检测——精密力测量¶

任务描述：以极低接触力（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.8 力控应用与硬件平台的匹配¶

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

打磨/抛光（恒力 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

练习¶

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

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

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

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

回顾 Ch53（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（当代主流，对应 Ch53 的 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 统一视角：力控是交互控制的核心¶

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

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

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

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

练习¶

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

练习¶

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

本章小结¶

知识点	核心内容	难度	后续章节
位置控制的局限	刚性环境→力不可控→振荡/损坏	⭐	F03 对比
Mason 约束空间	自然/人工约束→选择矩阵 $S$	⭐	F03 混合控制
Hogan 阻抗控制	$M_d\ddot{\tilde{x}} + D_d\dot{\tilde{x}} + K_d\tilde{x} = f_{ext}$	⭐⭐	F02 数学, F03 算法
阻抗 vs 导纳	因果性对偶, 硬件接口决定选型	⭐⭐	F04 阻抗, F05 导纳
二端口网络	$Z, Y$ 矩阵, 力-速度对偶	⭐⭐⭐	D05 遥操作
接触稳定性	无源性→耦合稳定（Colgate-Hogan）	⭐⭐⭐	F02 无源性理论
力传感器选型	F/T vs 关节力矩 vs 动量观测器	⭐⭐	F04, F05 工程
应用场景地图	打磨/装配/协作→方案+平台匹配	⭐⭐	F03-F05 实现
与足式对偶	阻抗控制↔WBC-QP 的统一视角	⭐⭐⭐	F07 WBC, F08 MPC

累积项目：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 插件）

延伸阅读¶

资料	类型	难度	推荐理由
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"	论文	⭐⭐⭐	耦合稳定性定理——力控稳定性的理论基石
Chiaverini et al. 1999, "Survey of Robot Interaction Control", IEEE/ASME T-Mech 4(3)	综述	⭐⭐	经典力控方案的分类与实验对比
libfranka `cartesian_impedance_control.cpp`	代码	⭐⭐	最简洁的笛卡尔阻抗实现（~130 行）

故障排查手册¶

症状	可能原因	排查步骤	相关章节
接触时机械臂剧烈振荡	阻抗刚度 $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. 检查重力补偿 $g(q)$ 是否正确 2. 适当提高 $K_d$ 3. 加前馈项	F01 2.3, F03

机器人力控	电路网络	物理意义
力 \(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\)	能量传输速率

环境特性	\(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\) 时振荡风险小

编号	问题	答不出时回顾
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\) 的物理含义？	控制理论基础

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

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

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

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

控制范式	端口行为	因果方向	\(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_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\)	精密装配	极高

装配类型	间隙 (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

异常状态	力/力矩特征	检测条件	动作
楔紧（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}	$ 持续偏大

参数	典型值	理由
目标力 \(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\)（过阻尼）	绝对禁止力过冲

决策因素	打磨	装配	协作	检测	学习型
力精度要求	\(\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\) 过高 + 环境刚度大	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. 检查重力补偿 \(g(q)\) 是否正确 2. 适当提高 \(K_d\) 3. 加前馈项	F01 2.3, F03