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

F03 经典力控算法——从 Raibert-Craig 力位混合到笛卡尔阻抗控制¶

本章定位：本章是力控基础三部曲的终章。F01 建立了直觉（"是什么和为什么"），F02 提供了数学工具（"用什么工具"），本章展示"怎么做"——将 F02 的工具应用于四大经典力控算法的完整推导、实现和稳定性分析。每个算法都从动机出发，经过完整推导，到达可实现的控制律，并分析其适用场景和局限。

前置依赖：F01（力控导论/阻抗导纳概念）、F02（操作空间动力学/无源性理论/参数约束）、M01（Pinocchio 动力学——关节空间 PD/计算力矩法基础）

下游章节：F04（笛卡尔阻抗工程/libfranka）、F05（导纳控制工程/ros2_control）、F07（WBC）

建议用时：2 周（算法推导 4 天 + MuJoCo 实现 5 天 + 对比实验 5 天）

前置自测 ⭐¶

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

编号	问题	答不出时回顾
1	写出操作空间动力学方程 \(\Lambda\ddot{x} + \mu + p = F\)。\(\Lambda, \mu, p\) 各自是什么？	F02 第 4 节
2	动力学一致伪逆 \(\bar{J} = M^{-1}J^T\Lambda\) 与 Moore-Penrose 伪逆 \(J^+\) 有什么区别？力控中为什么必须用 \(\bar{J}\)？	F02 第 4.3 节
3	什么是无源性？阻抗控制器的存储函数是什么？耗散不等式怎么写？	F02 第 3 节
4	Mason 的选择矩阵 \(S\) 如何将操作空间分解为力控和位控子空间？	F01 第 1.4 节
5	什么是接触过渡策略？为什么接触瞬间需要降低刚度 \(K_d\)？	F02 第 8 节

本章目标¶

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

完整推导 Raibert-Craig 力位混合控制从选择矩阵到关节力矩的全链条
**实现**直接力控制器（PI 力跟踪），理解积分漂移问题和抗饱和策略
**推导**关节空间和笛卡尔空间阻抗控制律，并证明其闭环稳定性
**对比**阻抗控制与 Ch53 WBC 中 QP 力控的数学结构差异
**分析**每个算法的适用场景、硬件要求和工程局限性
**在 MuJoCo 中实现**至少两种力控算法并做定量对比

1. Raibert-Craig 力位混合控制（1981） ⭐⭐¶

1.1 动机——当力和位置需要分管不同方向¶

回顾 F01 第 1.4 节：Mason 的约束空间分析将 6D 操作空间分解为力控子空间和位控子空间。Raibert & Craig 1981 给出了实现这种分解的第一个完整控制律。

核心问题：在擦黑板任务中，z 方向需要力控（维持法向力），xy 方向需要位控（跟踪轨迹）。如何在**一个控制律**中同时实现两者？

历史背景：Raibert & Craig 1981 年在 ASME Journal of Dynamic Systems, Measurement, and Control 上发表 "Hybrid Position/Force Control of Manipulators"（累计引用约 3800 次）。这是第一个系统化地将力控和位控统一在一个框架中的工作。

1.2 选择矩阵的定义与构造¶

回顾 F01：选择矩阵 \(S = \text{diag}(s_1, ..., s_6)\), \(s_i \in \{0, 1\}\)。\(s_i = 1\) 位控，\(s_i = 0\) 力控。\(\bar{S} = I - S\)。

**约束帧（Constraint Frame）**的概念：

选择矩阵必须在一个特定的坐标系中定义——这个坐标系的各轴与约束方向对齐。De Schutter 1988 将其称为**任务帧（Task Frame）或**柔顺帧（Compliance Frame）。

例: 擦桌子（桌面法线 = z 轴）

约束帧 = 桌面法线坐标系
  z: 法向（力控）     s_z = 0
  x: 切向 1（位控）   s_x = 1
  y: 切向 2（位控）   s_y = 1
  Rx: 绕 x 旋转（力矩控制 = 0）  s_Rx = 0
  Ry: 绕 y 旋转（力矩控制 = 0）  s_Ry = 0
  Rz: 绕 z 旋转（位控）          s_Rz = 1

S = diag(1, 1, 0, 0, 0, 1)
S_bar = diag(0, 0, 1, 1, 1, 0)

非对角约束帧：当约束方向不与世界坐标系对齐时（如斜面上的打磨），需要将选择矩阵旋转到约束帧：

\[S_{world} = T_{cf}^{world} \cdot S_{cf} \cdot (T_{cf}^{world})^T, \quad T_{cf}^{world} = \text{blkdiag}(R_{cf}^{world},\; R_{cf}^{world})\]

其中 \(R_{cf}^{world}\) 是约束帧到世界帧的 3×3 旋转矩阵，\(T_{cf}^{world}\) 是对应的 6×6 块对角变换（力和力矩方向需同步旋转）。在斜面场景中，\(R_{cf}^{world}\) 由表面法向量确定。

类比：选择矩阵 \(S\) 就像一个"交通分流器"——它把 6 维操作空间的"车流"（控制力）分配到两条"车道"（力控和位控）。约束帧就是分流器的"朝向"——它决定了哪个方向是力控车道、哪个是位控车道。

1.3 完整推导——从任务描述到关节力矩¶

Step 1：定义任务空间控制力

位控子空间的控制力（PD 控制器）：

\[F_{pos} = K_p e_x + K_d \dot{e}_x\]

其中 \(e_x = x_d - x\) 是位置误差。

力控子空间的控制力（PI 控制器）：

\[F_{force} = K_{fp} e_f + K_{fi} \int_0^t e_f \, d\tau\]

其中 \(e_f = f_d - f_{measured}\) 是力误差。本节采用**wrench 接口**：PI 的输出就是任务空间 wrench，因此 \(K_{fp}\) 无量纲，\(K_{fi}\) 的单位为 \(1/s\)。如果 PI 输出的是位置偏置 \(\Delta x\)，则 \(K_{fp}\) 才是 m/N、\(K_{fi}\) 才是 m/(N·s)，并且必须送入位置/导纳内环，不能再直接并入 \(J^T F\)。

Step 2：用选择矩阵组合

\[F_{task} = S \cdot F_{pos} + \bar{S} \cdot F_{force}\]

这一步的数学含义是：对于 \(s_i = 1\) 的方向（位控），取 \(F_{pos}\) 的对应分量；对于 \(s_i = 0\) 的方向（力控），取 \(F_{force}\) 的对应分量。

Step 3：映射到关节力矩

\[\tau = J^T F_{task} + g(q)\]

或含完整动力学补偿的版本：

\[\tau = J^T(S \cdot F_{pos} + \bar{S} \cdot F_{force}) + M(q)\ddot{q}_{ff} + C(q,\dot{q})\dot{q} + g(q)\]

完整控制律（展开形式）：

\[\boxed{\tau = J^T \Big[ S \cdot (K_p(x_d - x) + K_d(\dot{x}_d - \dot{x})) + \bar{S} \cdot (K_{fp}(f_d - f) + K_{fi}\int(f_d - f)dt) \Big] + C\dot{q} + g}\]

数值示例（擦桌子）：

S = diag(1, 1, 0, 0, 0, 1)

位控参数 (x, y, Rz 方向):
  K_p = diag(1000, 1000, -, -, -, 100) N/m
  K_d = diag(63, 63, -, -, -, 20) Ns/m

力控参数 (z, Rx, Ry 方向):
  K_fp = diag(-, -, 0.2, 0.2, 0.2, -) dimensionless
  K_fi = diag(-, -, 5.0, 5.0, 5.0, -) 1/s
  f_d = [0, 0, 10, 0, 0, 0]^T N  (法向 10N)

Raibert-Craig 控制律的稳定性分析：

对位控子空间（\(s_i = 1\)），闭环动力学为标准二阶系统：

\[\Lambda_{ii}\ddot{\tilde{x}}_i + K_{d,i}\dot{\tilde{x}}_i + K_{p,i}\tilde{x}_i = 0\]

渐近稳定当 \(K_{p,i}, K_{d,i} > 0\)（标准 PD 稳定性条件）。

对力控子空间（\(s_i = 0\)），分析更复杂。将 PI 力控与弹性环境 \(f = K_e \tilde{x}\) 联立：

\[M_d \ddot{x} + D_{eff}\dot{x} + K_e x = K_e \cdot x_{ref}\]

其中 \(x_{ref}\) 由力 PI 控制器输出。闭环特征方程为三阶的：

\[M_d s^3 + D_{eff} s^2 + K_e(1 + K_{fp}) s + K_e K_{fi} = 0\]

由 Routh-Hurwitz 判据，稳定性要求：

\[D_{eff}(1 + K_{fp}) > M_d K_{fi} / K_e\]

这里的 \(K_{fp}, K_{fi}\) 指的是**位置偏置接口**下的外环增益，不是上面 wrench 接口的无量纲/1/s 增益。两种实现的稳定性模型不同：wrench 接口直接输出 \(F_{force}\) 并经 \(J^T\) 映射；位置偏置接口输出 \(x_{ref}\) 或 \(\Delta x\) 并由内环产生接触力。推导和代码必须先选定接口，再写单位和闭环模型，不能把 m/N 增益塞进 \(J^T\) wrench 控制律。

关键洞察：稳定性条件依赖于环境刚度 \(K_e\)！当 \(K_e\) 极大（刚性环境）时，条件容易满足；当 \(K_e\) 较小（软环境）时，\(K_{fi}\) 不能太大——否则积分项会导致不稳定。

"不是X而是Y"：Raibert-Craig 的稳定性**不是**无条件的（不像阻抗控制有无源性保证），而是**取决于环境刚度和力控增益的匹配**。这是混合控制方案的固有弱点——它需要对环境有一定了解才能保证稳定。

1.4 选择矩阵的局限性分析¶

Raibert-Craig 方案虽然概念清晰，但有几个根本性的局限：

局限	原因	后果
需要已知约束几何	\(S\) 矩阵需要精确的约束帧方向	曲面/未知表面不适用
不连续切换	\(s_i \in \{0, 1\}\)，无中间状态	接触/脱离接触时控制律跳变
力和位置耦合	实际中力控方向的位移影响位控方向	交叉耦合导致性能退化
无阻抗行为	力控方向无弹簧/阻尼	接触瞬间力不连续

反事实推理：如果约束帧方向估计错误 5 度会怎样？力控方向的"投影"会泄漏到位控方向——本应跟踪力参考的分量被位控器"拉走"，导致力跟踪偏差。5 度误差在 10N 法向力下可产生约 \(10 \times \sin(5°) \approx 0.87\) N 的侧向力干扰——对精密装配可能不可接受。

"不是X而是Y"：选择矩阵 \(S\) **不是**工程师随意选择的设计参数，而是由**约束几何决定**的物理量。错误的 \(S\) 不仅性能差，还可能不稳定。Mason 1981 强调：约束分析必须先于控制器设计。

练习¶

⭐ 选择矩阵构造：为以下任务写出选择矩阵 \(S\) 和约束帧定义：(a) 在 30 度斜面上打磨；(b) 拧一个 M6 螺栓（绕 z 轴旋转力矩控制 + z 向力控 + 其余位控）。
⭐⭐ 完整推导：对 3-DOF 平面机械臂（\(n_q = 3, n_x = 2\)），手推 Raibert-Craig 控制律从任务空间到关节空间的完整形式。假设 \(S = \text{diag}(1, 0)\)（x 位控，y 力控）。
⭐⭐ 约束帧旋转：斜面法向量为 \(n = [\sin(30°), 0, \cos(30°)]^T\)。构造约束帧旋转矩阵 \(R_{cf}^{world}\)，并写出世界坐标系中的选择矩阵 \(S_{world}\)。

2. 直接力控制 ⭐⭐¶

2.1 动机——当需要精确跟踪力参考¶

有些任务不需要分解约束空间，只需要精确跟踪一个力参考 \(f_d\)——例如接触力检测、恒力抛光。这时最简单的方案是**直接力控**：用力误差驱动一个 PI 控制器。

直接力控是 F01 分类树中最"直接"的力控方案——它不试图控制阻抗或位置，而是直接让接触力跟踪参考值。

2.2 内环位置/速度 + 外环力控——级联结构¶

在大多数实际系统中，直接力控不是单独使用的，而是作为外环包裹在内环位置/速度控制器上：

力控外环：
  f_d --> [+] --> [PI 力控] --> delta_x --> [+] --> [位置内环] --> tau --> 机器人
          ^(-)                                ^(+)                        |
          |                               x_ref (基准轨迹)               |
          |                                                              |
          +-------------- [F/T 传感器] <---------------------------------+
                          f_measured

PI 力控制律（外环）：

\[\Delta x = K_{fp}(f_d - f_{measured}) + K_{fi}\int_0^t(f_d - f_{measured})d\tau\]

这里的直接力控采用**位置偏置接口**，所以 \(K_{fp}\) 单位是 m/N，\(K_{fi}\) 单位是 m/(N·s)。若改成直接输出 wrench，则控制律应写成 \(F_{cmd}=K_{fp}e_f+K_{fi}\int e_fdt\)，且 \(K_{fp}\) 无量纲、\(K_{fi}\) 为 \(1/s\)。

位置控制器（内环）：

\[\tau = K_p(x_{ref} + \Delta x - x) + K_d(\dot{x}_{ref} - \dot{x}) + g(q)\]

2.3 积分漂移问题¶

PI 力控的积分项 \(K_{fi}\int e_f \, dt\) 在**非接触状态**下会持续积累——因为 \(f_{measured} = 0\) 而 \(f_d \neq 0\) 时，积分项无限增长。当机器人重新接触时，积累的积分导致巨大的力冲击。

三种危险场景：

场景	积分行为	后果
接触 → 脱离 → 再接触	脱离期间积分持续增长	再接触时力冲击（可达数百 N）
力参考阶跃变化	积分快速积累	严重超调和振荡
传感器零偏漂移	积分缓慢积累	长期力偏差增长

2.4 抗饱和策略——三种方案对比¶

方案 1：积分限幅（Anti-windup clamp）

\[I_f = \text{clamp}\Big(\int_0^t e_f \, d\tau, \; -I_{max}, \; I_{max}\Big)\]

简单有效，但 \(I_{max}\) 的选择需要经验。太小 → 稳态力偏差；太大 → 冲击仍然存在。

方案 2：条件积分（Conditional integration）

\[\dot{I}_f = \begin{cases} e_f & \text{if } |f_{measured}| > f_{threshold} \text{ (确认接触中)} \\ 0 & \text{otherwise (非接触，冻结积分)} \end{cases}\]

本质洞察：条件积分的关键洞察是——积分项的作用是消除**接触中的稳态误差**，不应在非接触状态下积累。将积分条件绑定到"是否在接触中"是物理上正确的做法。

方案 3：泄漏积分器（Leaky integrator）

\[\dot{I}_f = e_f - \alpha I_f, \quad \alpha > 0\]

传递函数：\(I_f(s)/E_f(s) = 1/(s + \alpha)\)——这是一阶低通滤波器。稳态值 \(I_{f,ss} = e_{f,ss}/\alpha\)。

优势：自动防止无限积累（\(\alpha\) 项始终在"泄漏"能量）。代价：引入稳态偏差（\(e_{f,ss} = \alpha \cdot I_{f,ss}\)），\(\alpha\) 越大偏差越大。

// 带抗饱和的 PI 力控实现（实时安全：固定大小数组）
// 这个版本输出位置偏置 delta_x；kp 单位 m/N，ki 单位 m/(N*s)。
class ForcePI {
public:
    ForcePI(double kp, double ki, double i_max, double leak)
        : kp_(kp), ki_(ki), i_max_(i_max), leak_(leak) {}

    Eigen::Matrix<double,6,1> compute(
        const Eigen::Matrix<double,6,1>& f_desired,
        const Eigen::Matrix<double,6,1>& f_measured,
        double dt, bool in_contact)
    {
        Eigen::Matrix<double,6,1> error = f_desired - f_measured;

        // 条件积分 + 泄漏
        if (in_contact) {
            integral_ += error * dt - leak_ * integral_ * dt;
        }
        // 逐分量积分限幅
        for (int i = 0; i < 6; ++i) {
            integral_(i) = std::clamp(integral_(i), -i_max_, i_max_);
        }

        return kp_ * error + ki_ * integral_;
    }

    void reset() { integral_.setZero(); }

private:
    double kp_, ki_, i_max_, leak_;
    Eigen::Matrix<double,6,1> integral_ = Eigen::Matrix<double,6,1>::Zero();
};

练习¶

⭐ 积分漂移仿真：在 1-DOF 系统中实现 PI 力控（\(K_{fp} = 0.01\), \(K_{fi} = 0.005\)）。让机器人反复接触/脱离桌面（周期 2 秒）。对比无抗饱和、积分限幅（\(I_{max} = 0.05\)）、泄漏积分（\(\alpha = 5\)）三种方案的力响应曲线。
⭐⭐ 泄漏积分器分析：对 leaky integrator \(\dot{I} = e - \alpha I\)，求传递函数 \(I(s)/E(s)\)。画 Bode 图。\(\alpha = 1, 5, 20\) 时的 DC 增益和截止频率分别是多少？
⭐⭐ 级联稳定性：外环 PI 力控 + 内环 PD 位控的级联结构。画出完整闭环框图。推导整体传递函数（假设 1-DOF + 弹簧环境 \(K_e\)）。内环带宽必须满足什么条件才能保证稳定？

3. 主动刚度控制——虚拟弹簧模型 ⭐⭐¶

3.1 Salisbury 1980——阻抗控制的简化前身¶

回顾 F01 第 2.6 节：Salisbury 提出用 Jacobian 映射笛卡尔刚度：\(\tau = J^T K_x(x_d - x)\)。这是 Hogan 阻抗控制的前身——只有弹簧项，无阻尼和惯量。

控制律：

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

其中 \(K_x \in \mathbb{R}^{6\times6}\) 是笛卡尔空间刚度矩阵。

虚拟弹簧模型的物理图像：

                 虚拟弹簧 K_x
  [期望位置 x_d] ===//=== [末端 x] ---- 环境
                          F_spring = K_x(x_d - x)

当末端偏离期望位置时，虚拟弹簧产生回复力。在自由空间中，末端趋向 \(x_d\)；在接触中，末端偏离 \(x_d\) 一个与接触力成正比的距离：

\[\tilde{x} = x - x_d = K_x^{-1} f_{ext} \quad \text{（稳态）}\]

3.2 各向异性刚度设计¶

动机：不同方向需要不同的柔顺性。

任务	刚度配置	物理含义
擦桌子	\(K_z = 50\), \(K_{xy} = 1000\)	法向柔顺，切向刚性
拖动示教	\(K = 0\)（所有方向）	纯重力补偿
精密定位	\(K = 2000\)（各向同性）	各方向高刚度
插入装配	\(K_z = 50\), \(K_{xy} = 100\)	法向和切向都柔顺

FZI cartesian_compliance_controller 的 YAML 配置（ros2_control 生态）：

stiffness:
  trans_x: 1000
  trans_y: 1000
  trans_z: 50     # 法向柔顺
  rot_x: 5        # 柔顺
  rot_y: 5        # 柔顺
  rot_z: 100      # 刚性

关节空间等效刚度：

\[K_q = J^T K_x J\]

注意 \(K_q\) 是**构型相关的**——即使 \(K_x\) 是常数，\(K_q(q)\) 也随关节角变化。这意味着关节空间的有效刚度会随机械臂姿态改变。

3.3 刚度控制的局限与改进路径¶

局限	原因	改进方案（→ 后续章节）
无阻尼 → 振荡	纯弹簧，无能量耗散	加 \(D_d\) → 阻抗控制（5 节）
无惯量整形	末端惯量由机械结构决定	加 \(M_d\) → 完整阻抗控制
不能独立控力	力 = \(K_x \cdot e_x\)，耦合	并行力/位控（4 节）或直接力控（2 节）
稳态力误差	\(f = K_x(x_d - x)\)，\(x\) 不精确	加力积分项 → 并行控制

教学要点：Salisbury（1980）→ Hogan（1985）的演进逻辑：刚度控制解决了"柔软"的需求，但没有解决"阻尼"和"惯量整形"。Hogan 的阻抗控制是刚度控制加上阻尼（\(D_d\)）和惯量（\(M_d\)）后的完整版。

练习¶

⭐ 等效关节刚度：对 2-DOF 平面臂（\(l_1 = l_2 = 0.5\) m），\(K_x = \text{diag}(500, 200)\) N/m。计算关节刚度矩阵 \(K_q = J^T K_x J\) 在 \(q = [45°, 45°]\) 和 \(q = [90°, 0°]\) 时的值。\(K_q\) 随构型变化有什么物理含义？
⭐⭐ 振荡实验：在 MuJoCo 中实现纯刚度控制（无阻尼），让末端从 10mm 偏差位置释放。记录振荡频率和幅值衰减。然后加入临界阻尼 \(D_d = 2\sqrt{K_d \Lambda}\)，对比阶跃响应。

4. 并行力/位置控制——Chiaverini 1993 ⭐⭐¶

4.1 动机——不需要选择矩阵的力/位控制¶

Raibert-Craig 需要精确的选择矩阵 \(S\)——当约束几何未知或变化时，\(S\) 难以确定。Chiaverini & Sciavicco 1993 提出了**并行力/位控制**——力 PI 和位置 PD 同时作用于所有方向，不需要 \(S\)。

这是一个重要的工程创新：通过**增益大小的差异**隐式地实现了力/位分解，而非显式地用选择矩阵。

4.2 控制律推导¶

\[F_{task} = \underbrace{K_{fp} e_f + K_{fi} \int e_f \, dt}_{\text{力主导 PI（wrench 输出）}} + \underbrace{K_p e_x + K_d \dot{e}_x}_{\text{位扰动抑制 PD}}\]

\[\tau = J^T F_{task} + g(q)\]

物理直觉：在接触方向上，力 PI 直接输出任务空间 wrench 并占主导；在自由方向上应通过接触门控、积分冻结或参考力置零避免 PI 在无接触时持续积累，位 PD 则负责维持自由空间运动。

类比：想象两个人同时推一扇门——一个人力气大（力PI），一个人力气小（位PD）。在门能动的方向上，力气大的人决定运动；在门被墙挡住的方向上，力气小的人稳定位置。并行控制就是让"力气大的人"和"力气小的人"同时推门——谁占主导取决于环境约束。

4.3 与 Raibert-Craig 的关键区别¶

维度	Raibert-Craig	Chiaverini 并行
选择矩阵	需要（\(S\) 矩阵）	不需要
方向分解	显式（力/位分离）	隐式（增益决定）
约束几何	必须已知	可以未知
接触过渡	\(S\) 需要切换	自然过渡（无切换）
力跟踪精度	高（独立力控通道）	中（PD 项干扰力控）
实现复杂度	中（需约束帧）	低

4.4 闭环传递函数分析¶

若采用**位置偏置接口**的并行控制（力 PI 输出 \(\Delta x\)，再由位置/阻抗内环产生 wrench），对环境刚度 \(K_e\) 的接触方向（1-DOF 简化），闭环力传递函数为：

\[\frac{F(s)}{F_d(s)} = \frac{K_e(K_{fi}/s + K_{fp})}{Ms^2 + (D + K_e K_{fp})s + (K_p + K_e K_{fi})}\]

这个式子隐含一个接口约定：力 PI 输出的是位置偏置/柔顺命令，因此 \(K_e K_{fp}\) 与阻尼项同量纲，\(K_e K_{fi}\) 与刚度项同量纲。如果实现中 PI 直接输出期望力或速度命令，传递函数必须重新推导，不能直接套用这一式。

稳态力误差（阶跃力参考 \(f_d\)）：

由终值定理，\(e_{f,ss} = 0\)——积分项 \(K_{fi}\) 保证了零稳态力误差。

带宽分析：力控带宽受限于最慢的闭环极点。增大 \(K_{fp}\) 提高带宽但降低相位裕度——需要根据 \(K_e\) 的范围折中。

4.5 适用场景——未知曲面跟踪¶

并行力/位控最适合**几何未知的表面跟踪**任务：

打磨/抛光：法向力 10-30 N + 切向路径跟踪
去毛刺：法向力 20-50 N + 边缘跟踪
质量检测：接触力 1-5 N + 表面扫描

在这些场景中，表面法向未知且随位置变化——Raibert-Craig 需要实时更新 \(S\)（困难且可能引入延迟），而 Chiaverini 并行控制天然自适应。

练习¶

⭐⭐ 对比实验：在 MuJoCo Panda 上分别实现 Raibert-Craig 和 Chiaverini 并行控制，执行擦桌子任务（z 向力控 10N + xy 位控）。对比：(a) 力跟踪精度（RMS 误差），(b) 位置跟踪精度，(c) 接触过渡平滑度。
⭐⭐ 增益调参：在并行控制中，扫描 \(K_{fp}\) 从 0.001 到 1.0（10 个值），记录每个值下的力跟踪 RMS 误差和位置跟踪 RMS 误差。画出 trade-off 曲线。
⭐⭐ 未知曲面跟踪：让 Panda 在 MuJoCo 中沿一个未知半球面以 5N 法向力滑动。对比 Raibert-Craig（\(S\) 基于初始法向估计，不更新）和 Chiaverini 并行控制的跟踪效果。

4.6 被动柔顺与主动柔顺的工程集成¶

在实际工业系统中，被动柔顺（RCC/SEA）和主动柔顺（上述所有算法）通常**层叠使用**，而非二选一。

RCC（Remote Center Compliance）——被动柔顺的工业经典：

Whitney 1977 发明的 RCC 是一个纯机械装置：由弹性元件构成，使末端在接触时绕一个"远程旋转中心"自动对准。

RCC 的工程参数设计：
  K_lateral:   横向位移刚度 [N/mm]，决定自对准力
  K_angular:   角度偏转刚度 [Nm/rad]，决定角度纠正力
  远程中心距离 L: 从 RCC 底面到远程旋转中心的距离 [mm]

设计原则：
  L ≈ 工件长度（如销长 50mm -> L ≈ 50mm）
  K_lateral 选择使横向偏差 < 孔间隙时产生足够恢复力
  K_angular 选择使角度偏差 < 5 度时可自动纠正

代表产品：
  ATI Industrial Automation 的 RCC 系列
  Schunk 的 TCU 弹性补偿单元（±3mm, ±3 度）

三层柔顺设计——当代最佳实践（与 F01 呼应）：

层级 1: 机械被动层 (SEA/弹性法兰/RCC)
  -> 提供固有安全和粗力感知
  -> 零延迟、零故障、免维护
  -> 即使控制器崩溃也安全

层级 2: 主动控制层 (本章的四大算法之一)
  -> 精确力跟踪和可编程刚度
  -> 带宽 ~1 kHz
  -> 可在线调参

层级 3: 学习策略层 (RL/Diffusion, F09 详讲)
  -> 任务级自适应
  -> 带宽 10-50 Hz
  -> 自动发现最优阻抗策略

柔顺框架（Compliance Frame）——De Schutter 1988 的工程要点：

回顾 F01 第 5.1 节的分类树中提到的"柔顺框架"概念。De Schutter 1988 的任务帧形式化定义了如何在末端附近选择坐标系，使约束方向与坐标轴对齐，然后**每个轴独立设置柔顺/刚性**。这个工程方法论比"会写控制律"更重要——因为错误的柔顺框架选择会让任何算法都表现不佳。

柔顺框架配置示例（FZI cartesian_compliance_controller）：

擦桌子任务：
  柔顺框架 = 桌面法线坐标系
  x 轴（切向 1）: K = 1000 N/m  （刚性，跟踪轨迹）
  y 轴（切向 2）: K = 1000 N/m  （刚性，跟踪轨迹）
  z 轴（法向）:   K = 50 N/m    （柔顺，控制接触力）
  Rx, Ry:        K = 5 Nm/rad   （柔顺，适应表面倾斜）
  Rz:            K = 100 Nm/rad （刚性，保持工具朝向）

peg-in-hole 装配：
  柔顺框架 = 孔口法线坐标系
  x, y（切向）: K = 100 N/m  （柔顺，允许对准位移）
  z（插入方向）: K = 50 N/m   （柔顺，限制插入力）
  Rx, Ry:       K = 2 Nm/rad  （柔顺，允许角度纠正）
  Rz:           K = 50 Nm/rad （中等，保持旋转对齐）

本质洞察：工程师的核心能力是**分析任务的约束几何 → 选择柔顺框架 → 配置每轴刚度/阻尼**。这个能力比"会写阻抗控制律"更重要——因为控制律是通用的（本章已推导），但柔顺框架的选择是任务特定的，需要对物理交互有深刻理解。

常见陷阱¶

⚠️ 概念误区：认为"选了阻抗控制就不需要约束分析了" 新手想法："阻抗控制不需要选择矩阵 \(S\)，所以不需要分析约束几何。" 实际上：虽然阻抗控制**不需要显式的 \(S\)**，但你仍然需要通过**各向异性刚度**来隐式表达约束——法向低刚度、切向高刚度。选择 \(K_d\) 的对角值就是在做约束分析！如果你不理解约束几何，你的 \(K_d\) 设计会是错误的。正确做法：即使用阻抗控制，也要先做 Mason 约束分析（F01 1.4），然后根据分析结果设计各向异性 \(K_d\)。

⚠️ 编程陷阱：在力控循环中使用 std::cout 调试错误做法：在 1kHz 控制循环中加 std::cout << "force: " << f << std::endl; 现象：控制频率从 1kHz 降到 ~100Hz，接触时剧烈振荡根本原因：std::cout 涉及 IO 系统调用、动态内存分配和互斥锁——都是实时安全的禁区（M11 详讲）正确做法：使用 RealtimePublisher 或无锁环形缓冲区异步输出调试信息

⚠️ 思维陷阱：认为"阻抗控制总是比力控好" 新手想法："阻抗控制更通用、更稳定，所以总应该用阻抗控制。" 实际上：阻抗控制**不能精确跟踪力参考**——它控制的是力-位移关系，不是绝对力值。如果任务需要精确的 10.0±0.5N 恒力（如精密打磨），直接力控 PI 或并行控制可能更合适。正确思维：选择算法应该问三个问题：(1) 需要精确力跟踪吗？(2) 约束几何已知吗？(3) 硬件有力矩接口吗？然后按第 8 节的决策树选择。

5. 阻抗控制完整推导 ⭐⭐⭐¶

5.1 关节空间阻抗控制¶

期望行为：让每个关节表现为虚拟弹簧-阻尼器：

\[M_d^q \ddot{e} + D_d^q \dot{e} + K_d^q e = \tau_{ext}\]

其中 \(e = q - q_d\) 是关节位置误差。

控制律推导：

从关节动力学 \(M\ddot{q} + C\dot{q} + g = \tau + \tau_{ext}\) 出发，我们希望闭环误差表现为虚拟质量-阻尼-弹簧。若取 \(M_d^q \approx M(q)\)，反解 \(\tau\)：

\[\tau = M(q)\ddot{q}_d + C(q,\dot{q})\dot{q} + g(q) - K_d^q(q - q_d) - D_d^q(\dot{q} - \dot{q}_d)\]

这就是 M07 中的**计算力矩法加上阻抗项**。

简化版——重力补偿+PD（工程中最常用）：

\[\tau = g(q) - K_d^q(q - q_d) - D_d^q \dot{q}\]

省略了惯量和科氏力前馈。对于低速接触任务（打磨、装配），这已经足够好。Franka 的 joint_impedance_control 示例就是这个形式。

// libfranka joint_impedance_control 核心
// 注意: Franka FCI 按负载配置处理重力补偿；用户回调通常不要再加 model.gravity(state)
// 如果需要科氏力补偿，按 libfranka 示例通过 model.coriolis(state) 获取并加到输出力矩中
// 此简化示例省略了科氏力补偿, 适用于低速场景:

std::array<double, 7> tau_d;
for (size_t i = 0; i < 7; i++) {
    tau_d[i] = -stiffness[i] * (state.q[i] - q_desired[i])
               - damping[i] * state.dq[i];
}

反事实推理：如果不做动力学补偿（去掉 \(M\ddot{q}_d + C\dot{q} + g\) 前馈项）会怎样？在低速运动时差别不大（重力补偿最重要），但在快速运动中非线性动力学（惯量耦合、科氏力）会"污染"阻抗行为——末端在某些方向上表现得比预期"硬"或"软"。

5.2 笛卡尔空间阻抗控制——含动力学补偿¶

期望行为（Hogan 1985 阻抗方程）：

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

选择 \(M_d = \Lambda(q)\)（操作空间惯量，最常用）时，控制律大幅简化：

回顾 F02 第 4 节：操作空间动力学方程为 \(\Lambda\ddot{x} + \mu + p = F\)。当 \(M_d = \Lambda\) 时不需要惯量整形（不需要改变末端的"等效质量"）。

Step 1：在操作空间中，期望行为变为

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

这里的 \(f_{ext}\) 是环境通过机器人动力学自动施加到闭环右端的外力，**不是**一般阻抗实现中需要测量并前馈注入的控制输入。控制器只负责产生弹簧-阻尼-惯性关系；接触力由环境决定。

Step 2：不把外力放进命令项，控制器期望实现的末端加速度由参考加速度和阻抗反馈给出：

\[\ddot{x}_{cmd} = \ddot{x}_d - \Lambda^{-1}(D_d\dot{\tilde{x}} + K_d\tilde{x})\]

Step 3：通过操作空间动力学实现

\[F_{cmd} = \Lambda \ddot{x}_{cmd} + \mu + p = \Lambda\ddot{x}_d + \mu + p - D_d\dot{\tilde{x}} - K_d\tilde{x}\]

代回真实动力学 \(\Lambda\ddot{x}+\mu+p=F_{cmd}+f_{ext}\)，得到

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

Step 4：映射到关节力矩 \(\tau = J^T F_{cmd} + N^T\tau_{null}\)

在 \(\ddot{x}_d = 0\)（静态参考）且 \(\dot{x}_d=0\) 的情况下，简化为：

\[\boxed{\tau = J^T(-K_d\tilde{x} - D_d J\dot{q}) + (I - J^T\bar{J}^T)\tau_{null} + C\dot{q} + g}\]

这就是 libfranka cartesian_impedance_control.cpp 的核心：

// libfranka cartesian_impedance_control.cpp 核心 (~130行中最关键的3行)
// 教学写法保留 VectorXd/MatrixXd 以便阅读；真实 1 kHz 回调中应使用固定大小
// Eigen::Matrix<double, 7, 1> / Matrix<double, 6, 7>，并在初始化阶段预分配。
Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7);

// 任务空间阻抗力 -> 关节力矩
tau_task << jacobian.transpose() *
    (-cartesian_stiffness * error - cartesian_damping * (jacobian * dq));

// 零空间项 (关节中心化 + 阻尼)
tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) -
    jacobian.transpose() * jacobian_transpose_pinv) *
    (nullspace_stiffness * (q_d_nullspace - q) -
     (2.0 * sqrt(nullspace_stiffness)) * dq);

// 合成: gravity 由 FCI 底层自动补偿; coriolis 需手动通过 model.coriolis(state) 获取并加入
tau_d << tau_task + tau_nullspace + coriolis;

参数设计指南：

自然频率: omega_n = sqrt(K_d / Lambda_ii)     [rad/s]
阻尼比:  zeta = D_d / (2*sqrt(K_d * Lambda_ii))  [无量纲]

工业推荐:
  zeta in [0.7, 1.0] (临界阻尼到轻过阻尼)
  K_trans = 100-2000 N/m (取决于任务)
  K_rot = 5-200 Nm/rad
  M_d 通常取 Lambda(x) (不额外设定)

Franka libfranka 示例默认参数:
  K_trans = [150, 150, 150] N/m
  K_rot = [10, 10, 10] Nm/rad
  D = 2*sqrt(K) (临界阻尼)
  nullspace_stiffness = 0.5 (关节中心化刚度)

libfranka 笛卡尔阻抗控制完整实现解析：

libfranka 的 cartesian_impedance_control.cpp 是学习阻抗控制最好的工业级参考代码。以下逐行解析其核心控制循环（约 30 行）：

// === libfranka cartesian_impedance_control.cpp 核心控制回调 ===
// 以下代码在 1kHz 实时线程中运行

auto callback = [&](const franka::RobotState& state,
                     franka::Duration /*period*/) -> franka::Torques {

    // --- Step 1: 获取当前状态 ---
    // state.O_T_EE: 4x4 末端位姿矩阵（世界坐标系）
    // state.q: 7-DOF 关节角
    // state.dq: 7-DOF 关节速度
    Eigen::Map<const Eigen::Matrix<double,4,4>> T(state.O_T_EE.data());
    Eigen::Map<const Eigen::Matrix<double,7,1>> q(state.q.data());
    Eigen::Map<const Eigen::Matrix<double,7,1>> dq(state.dq.data());

    Eigen::Vector3d position(T(0,3), T(1,3), T(2,3));
    Eigen::Quaterniond orientation(T.block<3,3>(0,0));

    // --- Step 2: 计算位置误差（世界坐标系） ---
    Eigen::Matrix<double,6,1> error;
    error.head(3) << position - position_d;  // 位置误差

    // --- Step 3: 计算姿态误差（处理四元数双覆盖） ---
    if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
        orientation.coeffs() << -orientation.coeffs();
        // 确保取最短旋转路径
    }
    Eigen::Quaterniond error_quat(orientation.inverse() * orientation_d);
    error.tail(3) << -(T.block<3,3>(0,0) * error_quat.vec());
    // 负号: 使姿态误差与位置误差一样采用 current-minus-desired 约定；
    // 后续 -K*error 产生指向期望姿态的恢复力矩。
    // 旋转矩阵左乘: 将误差从末端系转到世界系

    // --- Step 4: 获取 Jacobian 和科氏力 ---
    std::array<double, 42> jacobian_array =
        model.zeroJacobian(franka::Frame::kEndEffector, state);
    std::array<double, 7> coriolis_array = model.coriolis(state);
    Eigen::Map<const Eigen::Matrix<double,6,7>> jacobian(jacobian_array.data());
    Eigen::Map<const Eigen::Matrix<double,7,1>> coriolis(coriolis_array.data());

    // --- Step 5: 计算任务空间阻抗力矩 ---
    // 这是教学示例；生产实时回调中不要在循环内创建动态大小 Eigen 对象。
    // tau_task = J^T * (-K * error - D * J * dq)
    Eigen::VectorXd tau_task(7);
    tau_task << jacobian.transpose() *
        (-stiffness_matrix * error            // 弹簧力
         - damping_matrix * (jacobian * dq)); // 阻尼力

    // --- Step 6: 计算零空间力矩（关节中心化） ---
    // 用 J^T 的伪逆近似（注意：不是动力学一致伪逆）
    Eigen::MatrixXd jacobian_transpose_pinv;
    pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv);

    Eigen::VectorXd tau_nullspace(7);
    tau_nullspace << (Eigen::MatrixXd::Identity(7,7) -
        jacobian.transpose() * jacobian_transpose_pinv) *
        (nullspace_stiffness * (q_d_nullspace - q) -   // 关节弹簧
         (2.0 * std::sqrt(nullspace_stiffness)) * dq); // 关节阻尼

    // --- Step 7: 合成最终力矩 ---
    Eigen::VectorXd tau_d(7);
    tau_d << tau_task + tau_nullspace + coriolis;
    // 注意: gravity 由 FCI 底层自动补偿，不需要用户加

    // --- Step 8: 力矩限幅（安全！） ---
    std::array<double, 7> tau_d_array{};
    Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
    return franka::Torques(tau_d_array);
};

代码中的关键设计决策解读：

行号	设计决策	为什么这样做	如果不这样做会怎样
Step 3	四元数双覆盖检查	\(q\) 和 \(-q\) 表示同一旋转	姿态误差取"长路"（>180度）
Step 3	\(-(R \cdot e_{quat}.vec())\)	误差方向+坐标系对齐	正反馈→姿态发散
Step 5	\(D \cdot (J \cdot dq)\) 而非 \(D \cdot \dot{\tilde{x}}\)	只用可测量的 \(\dot{q}\)，避免求 \(\dot{x}_d\)	需要对 \(x_d\) 做数值微分（噪声大）
Step 6	\(J^T\) 的 Moore-Penrose 伪逆	简化实现（非 \(\bar{J}\)）	零空间力矩会"泄漏"到末端
Step 7	不加重力项	FCI 底层自动补偿	如果手动加→双重补偿→末端飞起

⚠️ 编程陷阱：libfranka FCI 的力矩回调示例通常写成 tau_task + tau_nullspace + model.coriolis(state)，不要再叠加 model.gravity(state)；重力由 FCI/模型负载配置处理，否则会双重补偿。如果你看到有些实现没加 coriolis——那可能是低速接触任务中科氏力很小被有意忽略；但在快速运动中不加 coriolis 会导致阻抗行为变形。

从 libfranka 到 Pinocchio 的迁移对照：

如果你想在非 Franka 平台（如 MuJoCo 仿真或其他机械臂）上实现同样的阻抗控制，需要用 Pinocchio 替换 libfranka 的 model 接口：

libfranka 调用	Pinocchio 等价	注意事项
`model.zeroJacobian(...)`	`pinocchio::getFrameJacobian(...)`	注意参考系选择（LWA）
`model.coriolis(state)`	`pinocchio::nonLinearEffects(model, data, q, v) - pinocchio::computeGeneralizedGravity(...)`	Pinocchio 的 NLE 包含重力
`model.gravity(state)`	`pinocchio::computeGeneralizedGravity(model, data, q)`	data.g
FCI 自动重力补偿	用户必须手动加 `data.g`	最大差异！

5.3 稳定性分析¶

回顾 F02 第 3.3 节：我们证明了阻抗控制器是严格输出无源的：\(\dot{V} \leq f_{ext}^T\dot{\tilde{x}} - \dot{\tilde{x}}^T D_d \dot{\tilde{x}}\)。

无外力时的渐近稳定性（\(f_{ext} = 0\)）：

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

由 LaSalle 不变集定理：在 \(\dot{V} = 0\) 集合上，\(\dot{\tilde{x}} = 0\)，代入动力学得 \(K_d\tilde{x} = 0\)，由 \(K_d > 0\) 得 \(\tilde{x} = 0\)。不变集 = \(\{0\}\) → 渐近稳定。

有外力时的稳态行为：

稳态条件 \(\ddot{\tilde{x}} = 0, \dot{\tilde{x}} = 0\)：

\[K_d \tilde{x}_{ss} = f_{ext,ss} \implies \tilde{x}_{ss} = K_d^{-1} f_{ext,ss}\]

末端偏离期望位置一个与外力成正比、与刚度成反比的距离——这正是弹簧行为。

稳定性分析的详细步骤——从存储函数到渐近稳定的完整链条：

以下是笛卡尔阻抗控制稳定性证明的每一步，附带详细解释：

Step A：构造候选李雅普诺夫函数（存储函数）

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

为什么选这个函数？因为它是系统的"总能量"——第一项是动能，第二项是势能。\(M_d > 0, K_d > 0\) 保证 \(V > 0\)（\(\forall (\tilde{x}, \dot{\tilde{x}}) \neq 0\)）且 \(V = 0 \iff (\tilde{x}, \dot{\tilde{x}}) = 0\)。

Step B：验证 \(V\) 是正定的且径向无界的

正定性：\(V \geq \frac{1}{2}\lambda_{min}(K_d)\|\tilde{x}\|^2 + \frac{1}{2}\lambda_{min}(M_d)\|\dot{\tilde{x}}\|^2 > 0\)
径向无界：\(V \to \infty\) 当 \(\|(\tilde{x}, \dot{\tilde{x}})\| \to \infty\)

径向无界性保证了所有轨迹都是有界的（因为 \(V\) 不增且不能趋向无穷）。

Step C：计算 \(\dot{V}\)

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

代入 \(M_d\ddot{\tilde{x}} = f_{ext} - D_d\dot{\tilde{x}} - K_d\tilde{x}\)：

\[\dot{V} = \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}} - \cancel{\dot{\tilde{x}}^T K_d \tilde{x}} + \cancel{\tilde{x}^T K_d \dot{\tilde{x}}}\]

最后两项相消（\(K_d\) 对称性：\(a^T K_d b = b^T K_d a\)）。

Step D：分析 \(\dot{V}\) 的符号

无外力（\(f_{ext} = 0\)）：\(\dot{V} = -\dot{\tilde{x}}^T D_d \dot{\tilde{x}} \leq 0\)
\(\dot{V} = 0 \iff \dot{\tilde{x}} = 0\)（因为 \(D_d > 0\)）
\(\dot{V}\) 是半负定的（不是严格负定——在 \(\dot{\tilde{x}} = 0, \tilde{x} \neq 0\) 时 \(\dot{V} = 0\)）

Step E：应用 LaSalle 不变集定理

LaSalle 定理说：系统收敛到 \(\dot{V} = 0\) 集合中最大的不变集。

\(\dot{V} = 0\) 集合是 \(\{\dot{\tilde{x}} = 0\}\)。在这个集合上，\(\ddot{\tilde{x}} = 0\)（不变集要求 \(\dot{\tilde{x}}\) 保持为零）。代入动力学：

\[M_d \cdot 0 + D_d \cdot 0 + K_d \tilde{x} = 0 \implies \tilde{x} = 0 \quad (\text{因为 } K_d > 0)\]

所以最大不变集 = \(\{(\tilde{x}, \dot{\tilde{x}}) = (0, 0)\}\) = 原点。

结论：原点是**全局渐近稳定的**。\(\square\)

常见陷阱：很多教材在 Step D 后就宣称"渐近稳定"——这是错的！\(\dot{V} \leq 0\) 只能保证**李雅普诺夫稳定**（有界，但不一定收敛到原点）。需要 LaSalle 不变集定理（Step E）才能升级到**渐近稳定**。这个区别在考试和论文审稿中是常见的扣分点。

工程稳定性条件总结：

条件	数学要求	来源	违反后果
阻尼正定	\(D_d > 0\)	无源性（F02 3.3）	永远振荡不收敛
刚度正定	\(K_d > 0\)	势能正定（F02 1.2）	某方向不稳定
采样率足够	\(K_d < f(D_d, T)\)	离散无源性（F01 3.5）	高频颤振
柔性关节上界	\(K_d^{eq} \leq K_s\)	Ott 2008（F02 9.2）	关节弹簧振荡
模型补偿准确	\(\hat{g} \approx g\) 等	模型精度	稳态位置偏差

5.4 与 Ch53 WBC 中 QP 力控的对比¶

回顾 Ch53（WBC）：足式 WBC 用 QP 求解接触力分配：\(\min_{\ddot{q},\lambda} \|J\ddot{q} + \dot{J}\dot{q} - \ddot{x}_d\|^2\) s.t. 动力学 + 摩擦锥 + 力矩限制。

维度	机械臂阻抗控制	足式 WBC-QP
决策变量	无（闭式公式）	\(\ddot{q}, \lambda\)（QP 变量）
力的角色	环境施加（被动）	地面反力（主动决策）
约束处理	隐式（参数约束）	显式（摩擦锥、力矩限）
求解方式	闭式公式	QP 在线求解
计算量	~10 us（矩阵乘法）	~0.5-2 ms（QP 迭代）
适用场景	固定基座 + 1 个接触	浮动基座 + 多个接触

本质洞察：阻抗控制和 WBC-QP 是同一个问题的两种求解策略。阻抗控制利用"简单场景"（固定基座、单接触）的特殊结构得到闭式解；WBC-QP 处理"复杂场景"（浮动基座、多接触、摩擦锥）时必须用数值优化。两者的统一框架是**约束优化下的力-运动关系调控**。

5.5 柔性关节阻抗控制——Ott 2008（Franka/iiwa 的理论基础）¶

Franka Panda 和 KUKA iiwa 的关节包含**弹性元件**（谐波减速器 + 力矩传感器弹性），不能直接当做刚性关节处理。Ott 的柔性关节阻抗理论是 Franka 和 iiwa 控制器的理论基础。注意区分两个不同的 Ott 2008 参考文献：(1) Ott et al. 2008, IEEE T-RO 24(6)——期刊论文，聚焦统一的笛卡尔阻抗控制框架；(2) Ott 2008, Springer STAR vol.49——专著《Cartesian Impedance Control of Redundant and Flexible-Joint Robots》，包含完整的柔性关节阻抗理论推导。后者是更全面的参考。

双质量模型：

         K_s (关节弹簧)
  电机侧 ===//=== 连杆侧
  B*ddq_m + tau_s = tau_m     (电机侧)
  M_l*ddq_l + C_l*dq_l + g = tau_s + tau_ext  (连杆侧)
  tau_s = K_s*(q_m - q_l)    (弹簧力矩)

  K_s = 1000-15000 Nm/rad (Franka 典型约 3000 Nm/rad)

Ott 的关键结论：等效笛卡尔刚度有上界：

\[K_{d,eq} = \frac{K_\theta \cdot K_s}{K_\theta + K_s} \leq K_s\]

即**用户设定的刚度不能超过关节弹簧刚度**——这是物理限制，不是软件限制。

Ott 2008 控制律（简化版）：

\[\tau_m = K_s(q_m - q_l) + B\ddot{q}_m^{des}\]

其中 \(q_m^{des}\) 由外环阻抗律确定。内环（电机侧）整形为高带宽位置环，外环（连杆侧）实现期望阻抗。

反事实推理：如果忽略关节弹性，直接在 Franka 上实现刚性关节阻抗控制会怎样？当设定 \(K_d = 5000\) N/m（超过等效 \(K_s\) 上界）时，控制器试图让连杆侧"更硬"于关节弹簧——这在物理上不可能。结果是：关节弹簧振荡、力矩饱和、机器人安全停机。Ott 2008 的贡献正是给出了"什么刚度是可实现的"的精确理论边界。

与 SEA/QDD 的关系：

驱动类型	弹簧刚度 \(K_s\)	最大可实现笛卡尔刚度	力控带宽
SEA（Franka/iiwa）	3000 Nm/rad	~5000 N/m	~200 Hz
QDD（MIT Cheetah）	>>10000 Nm/rad（几乎刚性）	>>10000 N/m	>1000 Hz

QDD 的优势：几乎没有弹性限制，可以实现非常高的刚度。 SEA 的优势：天然的碰撞保护（弹簧吸收冲击能量）和力矩感知。

5.6 阻抗控制常见陷阱专栏¶

⚠️ 编程陷阱：stiffness_matrix * error 中 error 的顺序错误做法：error = x_d - x（期望减当前）用负号；或 error = x - x_d（当前减期望）不用负号现象：末端运动方向反转——远离期望位置而非趋向根本原因：弹簧力应为 \(F = -K(x - x_d) = K(x_d - x)\)。如果 error 定义为 \(x - x_d\)，则 \(F = -K \cdot error\)（需负号）；如果 error 定义为 \(x_d - x\)，则 \(F = K \cdot error\)（不需负号）。两种约定都正确，但**混用就是灾难**。正确做法：在代码注释中**明确写出** error 的定义约定，并在整个项目中统一。libfranka 用 error = x - x_d，控制律中用 -K * error。

⚠️ 概念误区：认为"阻尼项应该用 \(D(\dot{x} - \dot{x}_d)\)" 新手想法："阻尼力应该作用于速度误差，所以要减去期望速度 \(\dot{x}_d\)。" 实际上：对于**静态参考**（\(x_d\) 恒定，\(\dot{x}_d = 0\)），两者相同。但即使 \(x_d\) 是时变的，libfranka 和大多数工业实现仍然使用 \(-D \cdot (J\dot{q})\) 而非 \(-D \cdot (J\dot{q} - \dot{x}_d)\)——因为后者需要对 \(x_d\) 求时间导数，而数值微分会放大噪声。正确做法：如果 \(x_d\) 时变且有解析导数（如轨迹规划器输出），可以用 \(D(\dot{x} - \dot{x}_d)\)；否则用 \(D\dot{x}\)（更安全，相当于绝对阻尼）。

🧠 思维陷阱：认为"MuJoCo 中调好的参数可以直接用在 Franka 上" 新手想法："仿真中 \(K_d = 2000\) N/m 效果很好，直接上真机。" 实际上：MuJoCo 默认使用软接触模型（\(K_e\) 通常只有 \(10^3\)-\(10^4\) N/m），而真实桌面 \(K_e > 10^6\) N/m。仿真中 \(K_d = 2000\) N/m 时 \(K_d / K_e \approx 0.2\)（安全），真机中 \(K_d / K_e \approx 0.002\)（闭环阻尼比变化不大，但冲击力更大）。正确思维：仿真→真机时，先将 \(K_d\) 降低到仿真值的 30-50%，然后逐步增大。同时增大 \(D_d\)（因为真机环境更硬，需要更多阻尼防振荡）。

5.7 阻抗控制的工程实现要点¶

Jacobian 参考系选择：

在 Pinocchio 中，getFrameJacobian 有三种模式：

模式	Jacobian 含义	适用场景
`LOCAL`	末端体坐标系中的 Jacobian	力矩控制（\(\tau = J^T f\), \(f\) 在末端系）
`WORLD`	世界坐标系中的 Jacobian	不常用（仅原点在世界原点时）
`LOCAL_WORLD_ALIGNED`	末端点、世界轴的 Jacobian	阻抗控制推荐（误差在世界系）

⚠️ 编程陷阱：Jacobian 参考系和误差参考系**必须一致**。如果 Jacobian 用 LOCAL_WORLD_ALIGNED 但位置误差在 LOCAL 系中计算，力矩映射会产生姿态相关的耦合误差——非常难调试。

力矩限幅与安全监控：

// 力矩限幅（Franka 的安全层）
// Franka 每个关节有不同的力矩限制
constexpr std::array<double, 7> tau_max = {87, 87, 87, 87, 12, 12, 12};

for (size_t i = 0; i < 7; i++) {
    tau_d[i] = std::clamp(tau_d[i], -tau_max[i], tau_max[i]);
}

// 力矩变化率限幅（防止冲击）
// delta_tau = tau_d - tau_prev
// 限制 |delta_tau| < delta_tau_max * dt
constexpr double delta_tau_max = 1000.0;  // Nm/s
for (size_t i = 0; i < 7; i++) {
    double delta = tau_d[i] - tau_prev[i];
    delta = std::clamp(delta, -delta_tau_max * dt, delta_tau_max * dt);
    tau_d[i] = tau_prev[i] + delta;
}

练习¶

⭐⭐ 关节空间 vs 笛卡尔空间：在 MuJoCo Panda 上分别实现关节空间阻抗和笛卡尔空间阻抗，让末端接触刚性桌面。对比：(a) 力响应的方向性（关节空间阻抗在笛卡尔空间是各向同性的吗？），(b) 力跟踪精度。
⭐⭐⭐ 零空间实验：在笛卡尔阻抗控制中加入零空间关节中心化 \(\tau_{null} = -K_{null}(q - q_{mid}) - D_{null}\dot{q}\)。分别用 \(\bar{J}\)（动力学一致伪逆）和 \(J^+\)（Moore-Penrose）做零空间投影，验证 F02 中"\(J^+\) 会干扰末端"的结论。
⭐⭐⭐ 跨章综合：设计一个"打磨+零空间避障"的两优先级控制器：(1) 主任务：末端 z 方向低刚度 + xy 轨迹跟踪；(2) 零空间任务：关节 4 远离极限位置。写出完整控制律。这道题综合了 F01（约束分析）、F02（零空间投影）和本章（阻抗控制律）。

6. 自适应力控基础 ⭐⭐⭐¶

6.1 动机——当机器人模型不精确¶

前面的阻抗控制律都假设了 \(M, C, g\) 已知。但实际中： - 末端工具质量可能未知（换了工具忘记更新 URDF） - 关节摩擦随温度/磨损变化 - 负载在操作中变化（抓取/放下物体）

如果模型误差大，动力学补偿不准确 → 实际阻抗行为偏离期望。

6.2 自适应重力补偿——最简单的在线学习¶

重力项可以线性参数化：

\[g(q) = Y_g(q) \cdot \theta_g\]

其中 \(Y_g(q)\) 是重力回归矩阵（只依赖关节角，结构已知），\(\theta_g\) 包含质量和质心位置参数（数值未知）。

线性参数化的展开解释：

对一个 \(n\) 关节的串联机械臂，重力项可以写为 10 个惯性参数的线性组合（每个连杆有质量 \(m_i\)、质心位置 \(m_i c_{i,x}, m_i c_{i,y}, m_i c_{i,z}\)、惯性张量 6 个独立分量，共 10 个）。但重力只依赖质量和质心位置（不依赖惯性张量），所以每个连杆只有 4 个重力相关参数：

\[\theta_g = [m_1, m_1 c_{1x}, m_1 c_{1y}, m_1 c_{1z}, ..., m_n, m_n c_{nx}, m_n c_{ny}, m_n c_{nz}]^T \in \mathbb{R}^{4n}\]

回归矩阵 \(Y_g(q) \in \mathbb{R}^{n \times 4n}\) 的每一列是某个参数对重力力矩的贡献——这是纯运动学量（只依赖关节角和连杆几何），可以解析推导或用 Pinocchio 自动生成。

Pinocchio 计算回归矩阵：

// 使用 Pinocchio 的 computeJointTorqueRegressor 计算回归矩阵
// 注意: 这是通用的逆动力学回归器 Y(q,v,a)，满足 Y * theta = tau_RNEA
// 即 Y(q,v,a) * theta = M(q)*a + C(q,v)*v + g(q)
// 要得到重力回归矩阵，需设 v=0, a=0（消去惯量和科氏力项）:
pinocchio::Data::MatrixXs Y = pinocchio::computeJointTorqueRegressor(
    model, data, q,
    Eigen::VectorXd::Zero(model.nv),  // v = 0 (消去科氏力项)
    Eigen::VectorXd::Zero(model.nv)); // a = 0 (消去惯量项)
// 此时 Y * theta_full = g(q)（仅重力项）
// theta_full 包含所有 10*n_bodies 个惯性参数
// 若需要完整逆动力学回归器（含惯量和科氏力），传入实际的 v 和 a

在线参数估计（梯度下降法）：

\[\dot{\hat{\theta}}_g = -\Gamma \cdot Y_g^T(q) \cdot s\]

其中 \(s = \dot{q} + \alpha(q - q_d)\) 是滑模变量，\(\Gamma > 0\) 是学习率矩阵。

学习率 \(\Gamma\) 的选择：

\(\Gamma\) 太大	\(\Gamma\) 太小	推荐范围
快速收敛但噪声放大	缓慢收敛但平滑	\(\Gamma = \text{diag}(0.01\)-\(1.0)\)
可能振荡不收敛	环境变化时跟踪不上	按参数量级缩放

工程实现建议： - 初始 \(\hat{\theta}_g\) 取 URDF 中的标称值（即使不准也比零好） - \(\Gamma\) 对不同参数取不同值（质量参数学习率 ~0.1，质心参数 ~1.0） - 加参数投影（projection）防止估计值变为非物理（如负质量）

反事实推理：如果不做自适应而使用"保守估计"（如假设工具质量为最大可能值）会怎样？重力过补偿→末端被"向上推"→接触时法向力偏大。虽然安全但精度下降。自适应的价值：不牺牲精度地处理不确定性。

6.3 自适应阻抗控制¶

将参数自适应与阻抗控制结合（Slotine-Li 1987 框架的力控扩展）：

\[\tau = Y(q, \dot{q}, \dot{q}_r, \ddot{q}_r) \hat{\theta} + J^T(-K_d\tilde{x} - D_d\dot{\tilde{x}})\]

\[\dot{\hat{\theta}} = -\Gamma Y^T s\]

稳定性：存储函数加入参数误差项

\[V_{total} = V_{impedance} + \frac{1}{2}\tilde{\theta}^T\Gamma^{-1}\tilde{\theta}\]

可以证明 \(\dot{V}_{total} \leq -s^T K_d s\)，保证渐近稳定（需满足持续激励条件）。

练习¶

⭐⭐ 重力辨识：在 MuJoCo 中给 Franka 末端加一个 0.5 kg 的未知负载。实现在线重力参数梯度下降估计，观察参数 \(\hat{m}_{tool}\) 的收敛过程。
⭐⭐⭐ 自适应 vs 固定模型：同样的未知负载场景，对比自适应阻抗控制和固定模型阻抗控制在擦桌子任务中的力跟踪精度。

7. 每个算法的稳定性分析总结 ⭐⭐¶

7.1 稳定性分析方法对比¶

算法	稳定性分析方法	关键存储函数	稳定性类型
Raibert-Craig	分通道独立分析	各通道独立的 \(V\)	条件稳定（需正确 \(S\)）
直接力控 PI	闭环极点分析	N/A（线性系统）	BIBO 稳定（适当增益）
刚度控制	Lyapunov（弹性势能）	\(V = \frac{1}{2}\tilde{x}^T K_x \tilde{x}\)	稳定（无渐近）
并行力/位	闭环传递函数	N/A	条件稳定
阻抗控制	无源性 + LaSalle	\(V = \frac{1}{2}\dot{\tilde{x}}^T M_d \dot{\tilde{x}} + \frac{1}{2}\tilde{x}^T K_d \tilde{x}\)	渐近稳定 + 无源性保证

本质洞察：阻抗控制的稳定性分析是所有力控算法中最完善的——它不仅证明了单独系统的稳定性，还通过 Colgate-Hogan 定理保证了与**任何被动环境**耦合后的稳定性。这正是阻抗控制成为力控"默认选择"的理论基础。

7.2 环境刚度对稳定性的影响¶

不同算法对环境刚度 \(K_e\) 的敏感度差异巨大：

环境刚度 K_e 从小到大（泡沫 -> 木头 -> 铝 -> 钢铁）:

直接力控 PI:    [][][][][][]x  K_e 越大越不稳定（增益裕度下降）
Raibert-Craig:  [][][][][][]x  力控方向受 K_e 影响
刚度控制:       [][][][]x      高 K_e 时等效刚度被环境主导
并行力/位:      [][][][][]x    中等敏感度
阻抗控制:       [][][][][][]   无源性保证: 对所有 K_e 稳定!
                ^             ^
                软环境         刚性环境

练习¶

⭐⭐ 稳定性边界实验：在 MuJoCo 中实现直接力控 PI，逐步增大环境刚度 \(K_e\)（从 100 到 \(10^6\) N/m）。找到系统变得不稳定的临界 \(K_e\)。然后切换到阻抗控制，验证在同样的 \(K_e\) 范围内是否始终稳定。
⭐⭐⭐ 综合对比：选择阻抗控制和直接力控两种算法，在三种环境（泡沫 \(K_e = 500\)，木头 \(K_e = 10^4\)，钢铁 \(K_e = 10^6\)）下做恒力接触（\(f_d = 10\) N）。用表格对比：力跟踪精度、过冲量、稳定性、调参难度。

8. 力控带宽分析——从理论到实践 ⭐⭐¶

8.1 力控环的带宽瓶颈链¶

力控系统的有效带宽由最慢的环节决定——就像木桶效应，最短的板决定水位：

带宽限制链（从慢到快）：

力传感器滤波 (30-100 Hz)
    |
    v
控制器计算延迟 (~1 ms -> ~160 Hz)
    |
    v
关节柔性反共振 (50-150 Hz, Franka)
    |
    v
位置内环带宽 (50-100 Hz, 导纳控制)
    |
    v
采样 Nyquist 限制 (500 Hz @ 1kHz)
    |
    v
电流环带宽 (10-40 kHz)

有效带宽 = min(以上所有) ≈ 30-100 Hz (典型值)

8.2 各算法的带宽特性¶

算法	理论带宽	实际瓶颈	典型有效带宽
关节空间阻抗	\(\omega_n = \sqrt{K_d^q / M_{jj}}\)	关节柔性	50-200 Hz
笛卡尔空间阻抗	\(\omega_n = \sqrt{K_d / \Lambda_{ii}}\)	Jacobian 计算 + 关节柔性	30-150 Hz
直接力控 PI	PI 带宽	F/T 传感器滤波	20-80 Hz
导纳控制	导纳滤波器带宽	位置内环带宽（最大瓶颈）	10-50 Hz
并行力/位	力环和位环的较慢者	F/T 滤波 + 增益交叉	20-60 Hz

"不是X而是Y"：导纳控制的带宽**不是**由导纳参数 \(M_d, D_d, K_d\) 决定的，而是由**内环位置控制器的带宽**决定的。即使你设计了一个 200 Hz 带宽的导纳滤波器，如果位置内环只有 50 Hz 带宽，实际力控带宽仍然只有约 15-20 Hz。这是导纳控制在工业机器人（UR、Fanuc）上性能受限的根本原因。

8.3 提高力控带宽的工程手段¶

手段	效果	代价	适用场景
提高采样率	直接提升 Nyquist 限制	需更快的实时计算	所有
使用力矩接口（非位置）	跳过位置内环瓶颈	需 Franka/iiwa 硬件	阻抗控制
减小 F/T 滤波器截止	更快的力信号	噪声增大	力控 PI
前馈补偿	抵消内环延迟	需精确模型	导纳控制
QDD 驱动器	消除关节弹性瓶颈	碰撞安全下降	高动态任务

练习¶

⭐⭐ 带宽测量：在 MuJoCo Franka 阻抗控制中，给 \(x_d\) 施加从 1 Hz 到 200 Hz 的正弦扫频信号。记录末端实际位移的幅频响应。找到 -3dB 带宽——这就是你系统的实际力控带宽。
⭐⭐ 导纳 vs 阻抗带宽：实现相同阻抗参数的阻抗控制和导纳控制（导纳控制使用 100 Hz 位置内环）。做同样的扫频实验，对比两者的有效带宽。
⭐⭐⭐ 跨章综合：结合 F02 第 2 节（Bode 图分析）和本节的带宽链分析，为以下系统计算有效力控带宽：Franka + 阻抗控制 + 50 Hz F/T 滤波 + 1kHz 采样率 + 2ms 计算延迟。画出完整的开环 Bode 图标注每个瓶颈。

8.4 力控带宽不足的工程后果¶

当力控带宽不足时，会出现以下典型问题：

带宽不足场景	现象	根因	解决方案
带宽 < 接触瞬态频率	接触力尖峰	控制器来不及响应冲击	降低接触速度 / 降低 \(K_d\)
带宽 < 力参考变化频率	力跟踪相位滞后	控制器响应慢于参考变化	加前馈 / 提高带宽
带宽 < 环境振动频率	力控放大振动	振动频率落在控制器增益区	加陷波滤波器
带宽 < 摩擦突变频率	粘滑(stick-slip)	静摩擦→动摩擦切换太快	前馈摩擦补偿

经验法则：力控有效带宽应至少为任务最高频率分量的 3-5 倍。打磨（~10 Hz 力波动）→ 带宽 > 30 Hz 够用；精密装配（~50 Hz 力瞬态）→ 带宽 > 150 Hz 较理想。

8.5 MuJoCo 中的力控实现——仿真验证框架¶

在实际硬件部署前，必须在仿真中完整验证力控算法。MuJoCo 是力控仿真的首选引擎——它的接触模型比 PyBullet 和 Gazebo 更精确（软接触模型 + 隐式积分），且计算速度快（1 步 ~0.1 ms）。

MuJoCo 力控仿真的关键注意事项：

注意事项	MuJoCo 行为	与真实硬件的差异	处理方法
接触刚度	由 `solref`/`solimp` 参数控制	通常比真实环境"软"很多	增大 `solref[0]` 提高等效刚度
力矩延迟	零延迟（同步仿真）	真实 ~1 ms	加人为延迟（`mj_step` 后延迟一步）
传感器噪声	默认无噪声	真实 ~0.1-0.5 N	手动添加高斯噪声
关节柔性	默认刚性关节	Franka 有 SEA	用 `tendon` 或 `stiffness` 建模
采样率	可任意高	受硬件限制（1 kHz）	控制频率设为 1 kHz（`dt = 0.001`）

MuJoCo 笛卡尔阻抗控制伪代码：

// MuJoCo 笛卡尔阻抗控制循环
void impedance_control_step(mjModel* m, mjData* d,
    const Eigen::Vector3d& pos_d,
    const Eigen::Quaterniond& quat_d,
    const Eigen::Matrix<double,6,6>& K,
    const Eigen::Matrix<double,6,6>& D)
{
    // 1. 获取当前末端位姿
    int ee_body = mj_name2id(m, mjOBJ_BODY, "end_effector");
    Eigen::Map<const Eigen::Vector3d> pos(d->xpos + 3*ee_body);
    Eigen::Quaterniond quat(d->xquat[4*ee_body],
                            d->xquat[4*ee_body+1],
                            d->xquat[4*ee_body+2],
                            d->xquat[4*ee_body+3]);

    // 2. 计算 6D 误差
    Eigen::Matrix<double,6,1> error;
    error.head(3) = pos - pos_d;
    // 四元数误差（处理双覆盖）
    if (quat_d.coeffs().dot(quat.coeffs()) < 0)
        quat.coeffs() *= -1;
    Eigen::Quaterniond dq = quat.inverse() * quat_d;
    error.tail(3) = -(quat.toRotationMatrix() * dq.vec());

    // 3. 获取 Jacobian (MuJoCo 提供 3xn + 3xn)
    int nv = m->nv;
    // ⚠️ 教学示例用 new/delete；实时控制循环中应预分配固定缓冲区（见 M11）
    mjtNum* jacp = new mjtNum[3 * nv];
    mjtNum* jacr = new mjtNum[3 * nv];
    mj_jac(m, d, jacp, jacr, d->xpos + 3*ee_body, ee_body);
    // MuJoCo 的 3×nv Jacobian 数组按 C 风格行主序存储；
    // Eigen 默认列主序，Map 时必须显式写 RowMajor，否则列会被读乱。
    Eigen::Map<const Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::RowMajor>> Jp(jacp, 3, nv);
    Eigen::Map<const Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::RowMajor>> Jr(jacr, 3, nv);
    Eigen::MatrixXd J(6, nv);
    J << Jp, Jr;

    // 4. 计算关节速度的末端速度
    Eigen::Map<Eigen::VectorXd> qvel(d->qvel, nv);
    Eigen::Matrix<double,6,1> dx = J * qvel;

    // 5. 阻抗控制律
    Eigen::Matrix<double,6,1> F = -K * error - D * dx;
    Eigen::VectorXd tau = J.transpose() * F;

    // 6. 加重力补偿（MuJoCo 不自动补偿！）
    mj_inverse(m, d);  // 计算逆动力学
    for (int i = 0; i < nv; i++) {
        tau(i) += d->qfrc_bias[i];  // bias = coriolis + gravity
    }

    // 7. 施加力矩：ctrl 是 actuator 空间，不是 qvel/nv 空间。
    // 只有 "每个关节一个 motor actuator、gear=1" 时，ctrl[a] = tau[dof] 才成立。
    std::fill(d->ctrl, d->ctrl + m->nu, 0.0);
    for (int a = 0; a < m->nu; ++a) {
        if (m->actuator_trntype[a] != mjTRN_JOINT) {
            continue;  // tendon/slider-crank/site transmission 需要单独处理
        }
        const int joint_id = m->actuator_trnid[2 * a];
        const int dof_id = m->jnt_dofadr[joint_id];
        const double gear = m->actuator_gear[6 * a];
        if (std::abs(gear) > 1e-9) {
            // 对 motor actuator: generalized_tau[dof] = gear * ctrl[a]
            d->ctrl[a] = tau(dof_id) / gear;
        }
    }

    delete[] jacp;
    delete[] jacr;
}

⚠️ 编程陷阱：MuJoCo 不会像 Franka FCI 示例那样由底层负载配置和用户回调分层处理动力学补偿；在这个仿真力矩接口里，你必须手动加上 d->qfrc_bias（包含重力+科氏力）。忘记这一步会导致末端在自由空间中下垂——这是 MuJoCo 力控仿真中最常见的 bug。另一个常见错误是把 d->ctrl 当成 nv 维力矩数组：ctrl 的长度是 nu，含义由 actuator 类型、trnid 和 gear 决定；只有 1:1 torque motor 且 gear=1 的模型才可以直接令 ctrl[a]=tau[dof]。

仿真→真机的 transfer checklist：

□ 将 MuJoCo 的 d->qfrc_bias 替换为 Pinocchio 的 NLE
□ 删除 MuJoCo 的 mj_jac，替换为 Pinocchio 的 getFrameJacobian
□ 添加力传感器噪声模型（真机有，仿真中需手动加）
□ 降低 K_d 至仿真值的 50-80%（真机摩擦/柔性增加有效阻尼）
□ 加力矩限幅（仿真中通常不限，真机必须限）
□ 加接触检测逻辑（仿真中可直接读 MuJoCo 接触，真机需力阈值）
□ 验证坐标系约定（MuJoCo vs Pinocchio 的 Jacobian 参考系可能不同）

9. 算法对比总结与选型指南 ⭐¶

9.1 五大经典算法全景对比¶

算法	需要 F/T？	需要精确模型？	需要约束几何？	力跟踪精度	适用场景
Raibert-Craig 混合	是	部分	是	高	已知几何的装配
直接力控 PI	是	否	否	高	精确力跟踪
刚度控制	否	否	否	低	简单柔顺
Chiaverini 并行	是	部分	否	中	未知曲面打磨
笛卡尔阻抗	可选	是（动力学补偿）	否	中	通用柔顺交互

9.2 选型决策树——扩展版¶

Step 1: 硬件接口类型？
|
|-- 力矩接口 (Franka FCI / KUKA FRI / QDD)
|    |
|    +-- Step 2a: 任务需求分析
|         |
|         |-- 需要精确力跟踪 (|f_err| < 1N)?
|         |    |-- 约束几何已知且固定？
|         |    |    |-- 是 -> Raibert-Craig 混合控制
|         |    |    |    适用: 已知几何的装配、已知平面的擦拭
|         |    |    |    参数: S 矩阵、力PI增益、位PD增益
|         |    |    +-- 否 -> Chiaverini 并行力/位控
|         |    |         适用: 未知曲面打磨、自适应接触
|         |    |         参数: K_fp, K_fi, K_p, K_d
|         |    +-- 需要亚牛顿级精度 (|f_err| < 0.5N)?
|         |         +-- 是 -> 直接力控 PI + 高精度 F/T
|         |              适用: 质量检测、半导体探针
|         |              参数: K_fp, K_fi, I_max, leak
|         |
|         |-- 不需精确力跟踪, 需柔顺交互?
|         |    |-- 单接触 + 固定基座?
|         |    |    +-- 是 -> 笛卡尔阻抗控制
|         |    |         适用: 打磨、装配、拖动示教、协作
|         |    |         参数: K_d, D_d (zeta=1), 零空间行为
|         |    +-- 多接触 / 浮动基座?
|         |         +-- 是 -> WBC-QP (F07)
|         |              适用: 人形操作、双臂协作
|         |
|         +-- 需要变阻抗 / 学习策略?
|              +-- 是 -> 阻抗底层 + RL/IL (F09)
|                   适用: 复杂接触操作、任务自适应
|                   框架: CRISP (TUM), SERL (Berkeley)
|
|-- 位置/速度接口 (UR / Fanuc / ABB)
|    |
|    +-- Step 2b: 导纳控制是唯一选择
|         |
|         |-- 有外接 F/T 传感器?
|         |    +-- 是 -> 导纳控制 (F05)
|         |         |-- 力控带宽要求 > 30 Hz?
|         |         |    +-- 可能不满足 (位置内环瓶颈)
|         |         |         考虑升级到力矩接口硬件
|         |         +-- 力控带宽 < 20 Hz 够用?
|         |              +-- 导纳控制 OK
|         +-- 无 F/T?
|              +-- 动量观测器 + 导纳控制 (精度受限)
|                   或: 被动柔顺 (RCC/弹性法兰)

决策的量化依据——关键性能指标对比：

性能指标	Raibert-Craig	直接力控 PI	笛卡尔阻抗	导纳控制	并行力/位
力跟踪 RMS (N)	0.5-2	0.1-1	2-5	3-10	1-3
位跟踪 RMS (mm)	0.5-2	N/A	1-5	2-10	1-3
力控带宽 (Hz)	30-100	20-80	30-150	10-50	20-60
稳定性保证	条件	条件	无条件	条件	条件
对 \(K_e\) 鲁棒性	低	低	高	中	中
计算量 (us)	20-50	10-30	30-100	20-50	20-50
实现复杂度	中	低	高	中	中

本质洞察：没有"最好"的力控算法——只有"最匹配"的。选型的核心三问是：(1) 你的硬件有什么接口？（决定阻抗/导纳）(2) 任务需要精确力跟踪还是柔顺交互？（决定力控 vs 阻抗）(3) 约束几何已知吗？（决定混合 vs 并行）。回答了这三个问题，决策树就会自然收敛到唯一的选择。

9.3 从经典到现代的演进路线¶

经典算法（F03, 1981-1999）:
  显式公式 -> 单接触 -> 固定参数 -> 手动调参
    |
    v
高级力控（F06-F08, 2000-2018）:
  变阻抗 -> 多接触 WBC -> 在线参数调整 -> 无源性保证
    |
    v
学习型力控（F09, 2019-今）:
  RL 输出阻抗参数 -> Diffusion Policy + 底层阻抗 -> SERL/CRISP
  "学习不替代控制, 而是与控制共生"

每代演进解决了什么问题，又引入了什么新挑战：

代际	解决的问题	引入的新挑战	代表性工作
第一代（1981-1985）	力和位置的分离控制	需要已知约束几何	Raibert-Craig, Salisbury
第二代（1985-1999）	统一力-位关系控制	参数手动调、固定不变	Hogan, Khatib, Chiaverini
第三代（2000-2018）	参数在线调节+无源性保证	KB/能量罐过于保守	Ott, Kronander-Billard
第四代（2019-今）	自动发现最优阻抗策略	Sim-to-real gap、安全性	SERL, CRISP, Diffusion

关键趋势：每一代的"控制器"都在变简单（从复杂公式到简单阻抗），而"智能"在向更高层次转移（从控制器内部到策略层）。阻抗控制作为"底层操作系统"的角色越来越稳固——变化的是"谁来设置 \((x_d, K_d, D_d)\)"：

第一/二代：工程师手动设置
第三代：状态机/规则自动切换
第四代：神经网络策略自动输出

这个演进逻辑预示了未来方向：底层阻抗控制会更加标准化和硬件化（类似 libfranka FCI 的趋势），而**任务智能会完全交给学习型策略**。

9.4 工业应用的力控方案选择案例¶

案例 1：汽车门板打磨

任务: 铝合金门板打磨, 法向力 25 N, 切向速度 100 mm/s
硬件: UR10e + ATI Mini45 F/T + 气动打磨工具
分析:
  - UR10e 只有位置接口 -> 导纳控制
  - 曲面法向变化 -> 不需要精确约束几何
  - 力精度 ±3N 够用 -> 不需要极精密力控
选型: 导纳控制 (F05) + 并行力/位逻辑
参数:
  M_d = 5 kg, D_d = 100 Ns/m, K_d = 0 (纯导纳)
  力 PI: K_fp = 0.005, K_fi = 0.002
结果: 力跟踪 RMS ~2.5 N, 位跟踪 RMS ~1.5 mm

案例 2：电子元器件精密插入

任务: FPC 连接器插入, 间隙 0.15 mm, 插入力 < 5 N
硬件: Franka Panda (FCI 1kHz 力矩接口)
分析:
  - Franka 有力矩接口 -> 阻抗控制
  - 间隙小 -> 需要极低刚度 + 螺旋搜索
  - 插入力小 -> 需要精确力感知（关节力矩反推够用）
选型: 笛卡尔阻抗控制 + 螺旋搜索状态机
参数:
  搜索阶段: K_xy = 100 N/m, K_z = 50 N/m, zeta = 1.2
  插入阶段: K_xy = 200 N/m, K_z = 30 N/m, zeta = 1.0
  搜索半径: 2 mm, 频率: 2 Hz
结果: 成功率 98.5%, 平均插入时间 3.2 s

案例 3：人形机器人桌面擦拭

任务: 人形机器人右手持海绵擦桌子, 同时站立平衡
硬件: 人形机器人 (浮动基座 + 双臂 + 双腿)
分析:
  - 浮动基座 -> 必须用 WBC-QP (非固定基座阻抗控制)
  - 多接触 (双脚+右手) -> QP 同时处理
  - 站立平衡是最高优先级
选型: WBC-QP (F07) + 右手阻抗控制作为一个任务
  任务 1 (最高): CoM 稳定 (K_com = 500 N/m)
  任务 2: 右手末端阻抗 (K_z = 100 N/m, K_xy = 800 N/m)
  任务 3 (最低): 姿态保持 (K_posture = 10 Nm/rad)
  约束: 摩擦锥 (mu=0.5), 力矩限制, ZMP 约束

练习¶

⭐ 选型练习：为以下 5 个任务选择最合适的力控算法并说明理由：(a) Franka 拖动示教；(b) UR5e 恒力打磨；(c) Franka 精密 peg-in-hole；(d) 四足机器人站立平衡；(e) Franka + RL 桌面擦拭。
⭐⭐ 综合对比实验：在 MuJoCo Franka 上实现至少三种力控算法（建议：刚度控制、笛卡尔阻抗、并行力/位），执行相同的擦桌子任务。用表格对比力跟踪精度（RMS）、位置跟踪精度（RMS）和每步计算时间（us）。
⭐⭐⭐ 算法切换实验：在 MuJoCo 中实现一个"自适应选型器"——根据运行时的环境刚度估计自动在阻抗控制和并行力/位控之间切换。环境刚度可通过 \(\hat{K}_e \approx \Delta f / \Delta x\) 在线估计。当 \(\hat{K}_e < 10^4\) N/m 时用阻抗控制（柔软环境），当 \(\hat{K}_e > 10^5\) N/m 时切换到并行力/位控（刚性环境需要精确力跟踪）。
⭐⭐⭐ 跨章综合——完整力控栈设计：为 Franka Panda 的"抓取→运送→放置"任务设计完整的力控栈。要求：
抓取阶段：夹爪力控（恒力 5 N，PI 力控）
运送阶段：笛卡尔阻抗轨迹跟踪（\(K = 1000\) N/m）
放置阶段：接触检测→低刚度阻抗（\(K = 50\) N/m）→松开画出完整的状态机（至少 5 个状态），每个状态给出具体的控制器类型和参数。这道题综合了 F01（约束分析）、F02（参数约束）和 F03（所有算法）。

本章小结¶

知识点	核心内容	难度	后续章节
Raibert-Craig 混合控制	选择矩阵 \(S\)、力/位正交分解、约束帧	⭐⭐	F04 工程
直接力控 PI	力跟踪、积分漂移、三种抗饱和策略	⭐⭐	F04, F05
主动刚度控制	虚拟弹簧、各向异性、\(K_q = J^T K_x J\)	⭐⭐	F04 工程
Chiaverini 并行控制	力PI + 位PD 并行、不需 \(S\)、增益决定分解	⭐⭐	F04 工程
关节空间阻抗	\(\tau = g - K_d(q-q_d) - D_d\dot{q}\)	⭐⭐	F04 libfranka
笛卡尔空间阻抗	\(\tau = J^T(-K_d\tilde{x} - D_d\dot{x}) + N^T\tau_{null} + C\dot{q} + g\)	⭐⭐⭐	F04 libfranka
阻抗控制稳定性	无源性证明、LaSalle 定理、环境无关稳定	⭐⭐⭐	F06 变阻抗
与 WBC-QP 对比	闭式 vs QP、单接触 vs 多接触、统一框架	⭐⭐⭐	F07 WBC
自适应力控	在线参数估计、回归矩阵、Slotine-Li	⭐⭐⭐	F06 高级力控
算法选型	决策树、5 大算法对比表	⭐	F04, F05

累积项目：Mini-ForceControl 模块 2——力控算法库¶

本章新增模块：三种力控算法的 C++ 实现

// 力控算法基类
class ForceControllerBase {
public:
    virtual ~ForceControllerBase() = default;

    // 计算关节力矩
    virtual Eigen::VectorXd compute(
        const Eigen::VectorXd& q,
        const Eigen::VectorXd& dq,
        const Eigen::Matrix<double,6,1>& x_error,
        const Eigen::Matrix<double,6,1>& dx,
        const Eigen::Matrix<double,6,1>& f_ext,
        const Eigen::Matrix<double,6,1>& f_desired) = 0;

    virtual void setStiffness(const Eigen::Matrix<double,6,6>& K) = 0;
    virtual void setDamping(const Eigen::Matrix<double,6,6>& D) = 0;
};

// 笛卡尔阻抗控制器（本章实现）
class CartesianImpedanceController : public ForceControllerBase {
    // tau = J^T(-K*x_error - D*dx) + N^T*tau_null + coriolis + gravity
};

// 并行力/位控制器（本章实现）
class ParallelForcePositionController : public ForceControllerBase {
    // F = Kfp*ef + Kfi*integral(ef) + Kp*ex + Kd*dx_error
};

// Raibert-Craig 混合控制器（本章实现）
class HybridForcePositionController : public ForceControllerBase {
    // F = S*F_pos + Sbar*F_force
};

后续章节扩展： - F04 → FrankaImpedanceController（libfranka 实时回调集成） - F05 → AdmittanceController（ros2_control 插件） - F06 → VariableImpedanceController（能量罐 + KB 条件）

模块间接口约定：

// 所有力控算法共享的数据结构
struct ForceControlState {
    Eigen::Matrix<double,6,1> x_error;      // 笛卡尔误差 (pos+rot)
    Eigen::Matrix<double,6,1> dx;            // 笛卡尔速度
    Eigen::Matrix<double,6,1> f_ext;         // 外力估计
    Eigen::Matrix<double,6,1> f_desired;     // 期望力
    Eigen::VectorXd q;                       // 关节角
    Eigen::VectorXd dq;                      // 关节速度
    Eigen::MatrixXd J;                       // Jacobian (6 x nv)
    Eigen::VectorXd gravity;                 // 重力补偿力矩
    Eigen::VectorXd coriolis;                // 科氏力补偿
    bool in_contact;                         // 是否在接触中
    double dt;                               // 控制周期
};

// 所有算法返回关节力矩
struct ForceControlOutput {
    Eigen::VectorXd tau;                     // 关节力矩命令
    Eigen::Matrix<double,6,1> F_task;        // 任务空间力（调试用）
    double V_storage;                        // 存储函数值（无源性监控）
};

延伸阅读¶

资料	类型	难度	推荐理由
Raibert & Craig 1981, "Hybrid Position/Force Control", ASME J. DSMC 103	论文	⭐⭐	混合控制原始论文
Chiaverini et al. 1999, "Survey of Robot Interaction Control", IEEE/ASME T-Mech 4(3)	综述	⭐⭐	经典力控方案的分类与对比（教学标准参考）
Hogan 1985, "Impedance Control Parts I-III", ASME J. DSMC 107	论文	⭐⭐	阻抗控制原始理论
Ott 2008, "Cartesian Impedance Control of Redundant and Flexible-Joint Robots", Springer STAR vol.49	专著	⭐⭐⭐⭐	柔性关节阻抗完整理论（DLR/Franka 理论基础）。注意区分同年 Ott et al. IEEE T-RO 24(6) 期刊论文
Albu-Schaffer et al. 2007, "Unified Passivity-Based Framework", IJRR 26(1)	论文	⭐⭐⭐	统一无源性控制框架
Siciliano et al., "Robotics: Modelling, Planning and Control" Ch.9	教材	⭐⭐	力控算法标准教材推导
libfranka `cartesian_impedance_control.cpp`	代码	⭐⭐	笛卡尔阻抗的工业级实现（~130 行）
Villani & De Schutter 2016, Springer Handbook Ch.9 "Force Control"	教材章节	⭐⭐	力控算法对比的黄金参考
Slotine & Li 1987, "Adaptive Manipulator Control: A Case Study", IEEE T-AC 33(11)	论文	⭐⭐⭐	自适应控制经典框架（回归矩阵+参数估计）
De Luca & Mattone 2003, "Sensorless Robot Collision Detection", ISRR	论文	⭐⭐	动量观测器原始论文——无传感器力估计

故障排查手册¶

症状	可能原因	排查步骤	相关章节
阻抗控制接触时振荡	\(K_d\) 过高 / 采样率不足	1. 降低 \(K_d\) 至少 5x 2. 检查控制频率 3. 增大 \(D_d\)	F03 5.3, F02 3.5
力跟踪有稳态偏差	PI 无积分项 / 积分限幅太小	1. 检查 \(K_{fi}\) 非零 2. 增大积分限幅 3. 检查传感器零偏	F03 2.3
并行控制力精度低	PD 位控干扰力控	1. 降低位控 \(K_p\) 2. 增大力控 \(K_{fp}\) 3. 或切换到混合控制	F03 4.3
混合控制力控方向漂移	约束帧方向错误	1. 检查 \(S\) 矩阵 2. 重新标定约束帧 3. 用力传感器验证接触方向	F03 1.4
零空间任务干扰末端	用了 \(J^+\) 而非 \(\bar{J}\)	1. 验证 \(JN \approx 0\) 2. 切换到动力学一致伪逆	F02 4.3
自由空间末端下垂	重力补偿不准	1. 检查 URDF 惯性参数 2. 运行 SysId 3. 启用自适应重力补偿	F03 6.2
MuJoCo 中末端下坠	忘记加 `qfrc_bias`	1. 检查是否加了重力补偿 2. 打印 `d->qfrc_bias` 确认非零	F03 8.5
libfranka 重力双重补偿	用户手动加了 g(q)	1. 确认 FCI 已自动补偿重力 2. 删除用户代码中的重力项	F03 5.2
接触时力矩饱和	阻抗力矩 + 补偿力矩超限	1. 降低 \(K_d\) 2. 检查力矩限幅代码 3. 改善构型避免大臂力矩	F03 5.6
自适应参数漂移到非物理值	缺少参数投影	1. 加物理约束（\(m > 0\)） 2. 检查学习率 \(\Gamma\) 是否过大 3. 加死区	F03 6.2

力控调试的"黄金五步法"：

当力控系统表现异常时，按以下优先级排查——从最简单（也最常见）的原因开始：

Step 1: 检查坐标系一致性
  -> Jacobian 参考系、误差参考系、力传感器坐标系 是否统一?
  -> 最常见的 bug: 三者不在同一坐标系
  -> 诊断: 让末端静止, 施加已知方向的外力, 检查力矩方向是否正确
  -> 占所有 bug 的估计比例: ~30%

Step 2: 检查重力补偿
  -> 自由空间中末端是否静止? (不下垂也不飘)
  -> 缓慢移动到不同构型, 检查力读数是否接近零
  -> 诊断: 手动移动机械臂, 是否感觉"无重力" (完美补偿)
  -> 占所有 bug 的估计比例: ~25%

Step 3: 检查增益量级
  -> K_d 的量级是否合理? (N/m vs N/mm 混淆? 关节 vs 笛卡尔混淆?)
  -> D_d 是否与 K_d 匹配? (zeta = D/(2*sqrt(K*M)) 在 0.7-1.5 之间?)
  -> 诊断: 将 K_d 降到 50 N/m, 手推末端检查弹簧感是否合理
  -> 占所有 bug 的估计比例: ~20%

Step 4: 检查采样率和延迟
  -> 控制循环是否真的跑在 1 kHz? (测量实际周期)
  -> 是否有计算超时导致的周期抖动?
  -> 诊断: 打印每步计算时间, 检查 > 1ms 的比例
  -> 占所有 bug 的估计比例: ~15%

Step 5: 检查力传感器/力估计
  -> 力传感器零偏是否已标定?
  -> 工具重力补偿是否正确?
  -> 动量观测器的模型参数是否准确?
  -> 诊断: 在无接触时打印力读数, 应 < 0.5 N
  -> 占所有 bug 的估计比例: ~10%

教学要点：力控调试经验远比理论推导更难传授。上述"黄金五步法"来自数百次调试经验的总结——先检查坐标系（最常见也最难发现）、再检查重力（最基础也最影响全局）、然后才是增益调参。新手往往反着来——上来就调增益——然后花数小时调不好，因为真正的 bug 是坐标系不一致。