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

F02 力控制的数学基础——刚度/阻尼/惯量矩阵、无源性理论与频域设计

本章定位：本章为力控系列（F01-F10）提供数学工具箱。F01 建立了"是什么和为什么"的直觉，本章回答"用什么工具"——操作空间动力学的完整推导、弹簧-阻尼器-质量系统的频域分析、无源性理论的严格数学、阻抗参数的物理约束，以及力/力矩传感器的噪声模型。这些工具将在 F03 中直接应用于经典力控算法的推导和稳定性证明。

前置依赖：F01（力控导论）、M01（Pinocchio 动力学/雅可比）、M03（IK 与 Jacobian 基础）、线性代数（特征值/SVD）

下游章节：F03（经典力控算法）、F04（阻抗工程）、F06（变阻抗/无源性保证）、F07（WBC）

建议用时：2 周（理论推导 5 天 + Pinocchio 实验 4 天 + 练习 5 天）

---

前置自测 ⭐

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

编号	问题	答不出时回顾
1	写出 Hogan 阻抗方程 \(M_d\ddot{\tilde{x}} + D_d\dot{\tilde{x}} + K_d\tilde{x} = f_{ext}\)，解释每个参数的物理含义。	F01 第 2 节
2	什么是机械阻抗 \(Z(s)\) 和机械导纳 \(Y(s)\)？两者的关系是什么？	F01 第 2.4 节
3	写出关节空间动力学方程 \(M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau + J^T f_{ext}\)。\(M(q)\) 为什么是对称正定的？	M01 Pinocchio
4	什么是 Jacobian 伪逆 \(J^+ = J^T(JJ^T)^{-1}\)？它给出的解有什么最优性？	M01/M03
5	Laplace 变换中，\(s = j\omega\) 代入传递函数后，实部和虚部分别对应什么？	控制理论基础

---

本章目标

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

1. **从关节动力学完整推导**操作空间动力学方程 \(\Lambda(x)\ddot{x} + \mu(x,\dot{x}) + p(x) = F\)，理解每一步的物理意义
2. **区分**动力学一致伪逆 \(\bar{J}\) 与 Moore-Penrose 伪逆 \(J^+\) 的数学和物理差异，在力控场景中正确选择
3. **推导**标准阻抗控制器的无源性证明，掌握存储函数/耗散不等式/供给率的完整理论
4. **分析**阻抗参数（\(M_d, D_d, K_d\)）的物理约束——正定性、对称性、以及与环境耦合的稳定性条件
5. **设计**力控系统的频率响应——使用 Bode 图分析阻抗/导纳的频域行为，选择合适的带宽和阻尼比
6. **建模**力/力矩传感器的噪声特性，设计适当的滤波器

---

1. 刚度矩阵、阻尼矩阵与惯量矩阵的物理意义 ⭐

1.1 从一维到六维的弹簧-阻尼器-质量模型

回顾 F01：Hogan 阻抗方程 \(M_d\ddot{\tilde{x}} + D_d\dot{\tilde{x}} + K_d\tilde{x} = f_{ext}\) 让末端表现为虚拟的弹簧-阻尼器-质量系统。在 F01 中我们从直觉上理解了每个参数的含义，现在我们从数学上严格分析这三个矩阵。

一维情况（单自由度）：

\[m\ddot{x} + d\dot{x} + kx = f\]

这是最简单的二阶线性系统。三个标量参数完全确定系统行为：

参数	决定	时域效果	频域效果
\(k\)（刚度）	静态位移 \(x_{ss} = f/k\)	弹簧回复力	低频渐近线
\(d\)（阻尼）	过渡行为（振荡/单调）	能量耗散速率	共振峰高度
\(m\)（惯量）	高频响应	对快速力变化的惰性	高频渐近线

关键设计参数：

自然频率 \(\omega_n = \sqrt{k/m}\) 和阻尼比 \(\zeta = d/(2\sqrt{km})\) 完全描述系统的动态特性。

1-DOF 弹簧-阻尼器-质量系统的阶跃响应:

x(t)  ^
      |
x_ss  |--------===========================  zeta = 1.0 (临界阻尼)
      |     /
      |    /  ~~~~~~======================  zeta = 0.5 (欠阻尼, 振荡)
      |   /
      |  / ___=============================  zeta = 2.0 (过阻尼, 缓慢)
      | //
      |/
      +--+----+----+----+----+----+-----> t
         0    1    2    3    4    5
                     (t * omega_n)

六维情况（末端执行器的完整描述）：

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

其中 \(\tilde{x} = [e_{pos}^T, e_{rot}^T]^T \in \mathbb{R}^6\)（位置误差 3D + 姿态误差 3D）。

\(M_d, D_d, K_d \in \mathbb{R}^{6\times6}\) 现在是**矩阵**。矩阵的对角元素控制各轴独立行为，非对角元素控制轴间耦合。

1.2 刚度矩阵 \(K_d\) 的结构与物理约束

基本要求：\(K_d\) 必须是**对称半正定**（PSD）矩阵；如果所有受控方向都需要回到唯一平衡点，则取**对称正定**（SPD）矩阵。

为什么对称？因为刚度矩阵来源于势能函数：

\[V_{spring} = \frac{1}{2} \tilde{x}^T K_d \tilde{x}\]

弹簧力为势能的负梯度：\(f_{spring} = -\nabla V = -K_d \tilde{x}\)。对称性保证了 \(V\) 是良定义的二次型（否则 \(\tilde{x}^T K_d \tilde{x}\) 可能不等于 \((\tilde{x}^T K_d \tilde{x})^T\)，导致势能不唯一）。

为什么至少半正定？因为 \(V \geq 0\) 对所有 \(\tilde{x}\) 成立，当且仅当 \(K_d\) 半正定。如果 \(K_d\) 有负特征值，则存在方向使得弹簧"推离"平衡点——系统不稳定。若某个方向刚度为 0，该方向没有弹性恢复力，适合纯力控/导纳轴；若要全维位置姿态收敛，则这些方向也必须取正刚度。

类比：想象一个三维空间中的弹簧系统。\(K_d\) 的特征值是三个主方向上的刚度，特征向量是主方向。如果所有特征值都是正的，从任何方向推开质量块都会被拉回来（稳定）。如果某个特征值是负的，从那个方向推开后质量块会加速飞走（不稳定）。

各向同性 vs 各向异性：

各向同性（最简单，初学者推荐）：
  K_d = | k_t*I_3    0      |  (所有平移方向刚度相同)
        |   0     k_r*I_3   |  (所有旋转方向刚度相同)

各向异性（实际应用常用）：
  K_d = diag(k_x, k_y, k_z, k_rx, k_ry, k_rz)
  例: 擦桌子 -> k_x = k_y = 1000, k_z = 50 (法向柔顺)

耦合刚度（高级应用）：
  K_d 的非对角元素非零
  例: 斜面上的柔顺 -> 位移和旋转耦合
  注意: 必须保持 K_d 的对称半正定性；若要求全维回位，则保持正定!

常见陷阱：

⚠️ 概念误区：认为刚度矩阵 \(K_d\) 可以随意设定为任何对称矩阵。 新手想法："只要 \(K_d\) 对称就行。" 实际上：\(K_d\) 的特征值必须全部非负；需要全维位置恢复时必须全部为正。一个常见的错误是设定大的非对角元素使矩阵变为不定——比如 \(K_d = \begin{bmatrix} 100 & 200 \\ 200 & 100 \end{bmatrix}\) 有一个负特征值 \(-100\)，会导致不稳定。 正确做法：设定 \(K_d\) 后，一定检查所有特征值非负，并对需要回位的轴检查严格为正。可用 Eigen 的 SelfAdjointEigenSolver 验证。

1.3 阻尼矩阵 \(D_d\) 的设计

阻尼比的概念推广到多维：

一维阻尼比 \(\zeta = d / (2\sqrt{km})\) 推广到多维需要对 \(M_d\) 和 \(K_d\) 同时对角化。

双对角化方法（Albu-Schaffer et al., DLR）：

由于 \(M_d\) 和 \(K_d\) 都是对称正定的，存在变换 \(\Phi\) 使得：

\[\Phi^T M_d \Phi = I, \quad \Phi^T K_d \Phi = \Omega^2 = \text{diag}(\omega_1^2, ..., \omega_6^2)\]

在模态坐标中，各模态解耦：

\[\ddot{\xi}_i + 2\zeta_i \omega_i \dot{\xi}_i + \omega_i^2 \xi_i = \phi_i^T f_{ext}\]

临界阻尼设计（工程中最常用）：

\[D_d = 2 \zeta \cdot M_d^{1/2} (M_d^{-1/2} K_d M_d^{-1/2})^{1/2} M_d^{1/2}\]

当 \(M_d\) 和 \(K_d\) 都是对角矩阵时简化为：

\[D_d = \text{diag}(2\zeta_i \sqrt{k_i \cdot m_i})\]

大多数工程师直接用 \(\zeta = 1\)（临界阻尼）。

反事实推理：如果阻尼比 \(\zeta\) 选得太小（如 \(\zeta = 0.1\)）会怎样？系统在接触后会像弹簧一样反复振荡——末端在接触面上"弹跳"。如果 \(\zeta\) 选得太大（如 \(\zeta = 5\)）呢？系统响应极其缓慢——操作者推末端后要等好几秒才能到位。所以 \(\zeta \in [0.7, 1.0]\) 是力控的"甜点区间"：快速响应且无过冲。

1.4 惯量矩阵 \(M_d\) 的选择策略

\(M_d\) 的选择有三种常见策略：

策略	\(M_d\) 的值	优势	劣势	适用场景
自然惯量	\(M_d = \Lambda(q)\)	不需要惯量整形力矩	惯量随构型变化	大多数阻抗控制
恒定惯量	\(M_d = m_0 I_6\)	各方向行为一致	需要额外补偿力矩	精密力控
零惯量	\(M_d \to 0\)（一阶系统）	最简单，无振荡	对力突变敏感	导纳控制

本质洞察：选择 \(M_d = \Lambda(q)\)（操作空间惯量）不是**偷懒，而是深层的物理原因——当期望惯量等于真实惯量时，控制器**不需要施加额外力矩来改变末端的惯量感。改变惯量需要产生加速度，这意味着巨大的力矩——在 1kHz 控制环中，这些力矩可能超过关节限制。所以 \(M_d = \Lambda\) 是最"温和"的选择。

练习

1. ⭐ 对称性验证：构造一个 \(2\times2\) 非对称"刚度矩阵" \(K = \begin{bmatrix} 100 & 50 \\ 30 & 200 \end{bmatrix}\)。计算其对称部分 \((K + K^T)/2\) 和反对称部分 \((K - K^T)/2\)。说明为什么反对称部分不对势能有贡献。
2. ⭐⭐ 阻尼设计：给定 \(K_d = \text{diag}(500, 500, 100, 10, 10, 50)\) N/m 和 \(M_d = \text{diag}(2, 2, 2, 0.5, 0.5, 0.5)\) kg。用临界阻尼 \(\zeta = 1\) 计算 \(D_d\) 的对角元素。每个方向的自然频率 \(\omega_{n,i}\) 是多少？
3. ⭐⭐ 特征值分析：验证 \(K_d = \begin{bmatrix} 300 & 150 \\ 150 & 300 \end{bmatrix}\) 是否正定。计算其特征值和特征向量。物理上，特征向量对应什么？

---

2. 弹簧-阻尼器-质量系统的频域分析 ⭐⭐

2.1 阻抗与导纳的传递函数

从 Hogan 阻抗方程出发，取 Laplace 变换（零初条件）：

\[(M_d s^2 + D_d s + K_d) \tilde{X}(s) = F_{ext}(s)\]

阻抗传递函数（力/速度，1-DOF 简化）：

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

其中 \(V(s) = s\tilde{X}(s)\)。

导纳传递函数（速度/力）：

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

位移/力传递函数（柔顺度，力控中最常用）：

\[G(s) = \frac{\tilde{X}(s)}{F_{ext}(s)} = \frac{1}{M_d s^2 + D_d s + K_d} = \frac{1/K_d}{1 + 2\zeta/\omega_n \cdot s + s^2/\omega_n^2}\]

标准二阶形式，其中 \(\omega_n = \sqrt{K_d/M_d}\)，\(\zeta = D_d/(2\sqrt{K_d M_d})\)。

2.2 Bode 图分析——阻抗的频率响应

阻抗 \(Z(j\omega)\) 的 Bode 图揭示了力控系统在不同频率下的行为：

低频（\(\omega \ll \omega_n\)）：\(Z \approx K_d/(j\omega)\)，阻抗以 -20 dB/dec 下降。行为由弹簧主导——缓慢推动时感受到弹簧回复力。

共振频率（\(\omega = \omega_n\)）：\(Z \approx D_d\)，阻抗取最小值（对于欠阻尼系统可能出现反共振谷）。

高频（\(\omega \gg \omega_n\)）：\(Z \approx M_d j\omega\)，阻抗以 +20 dB/dec 上升。行为由惯量主导——快速推动时感受到惯性。

阻抗 |Z(jw)| 的 Bode 幅频图（1-DOF 示例）：

|Z| (dB)
  |
  |  \                     /
  |   \                   /
  |    \     D_d         /
  |     \   (最小值)    /
  |      +---+--------+
  |     K_d/w  w_n   M_d*w
  |   (-20dB/dec)  (+20dB/dec)
  +---------------------------------> log(w)
      低频        共振      高频
    (弹簧主导)   (阻尼主导)  (惯量主导)

阻抗 Bode 图的完整相位分析：

\[Z(j\omega) = M_d j\omega + D_d + \frac{K_d}{j\omega} = D_d + j\Big(M_d\omega - \frac{K_d}{\omega}\Big)\]

幅值：\(|Z(j\omega)| = \sqrt{D_d^2 + (M_d\omega - K_d/\omega)^2}\)

相位：\(\angle Z(j\omega) = \arctan\frac{M_d\omega - K_d/\omega}{D_d}\)

阻抗 Z(jw) 的 Bode 相频图：

Phase (deg)
  +90  |                     ___________
       |                    /
       |                   /
    0  |------------------+----------------> log(w)
       |                 w_n (相位穿越零)
       |               /
       |              /
  -90  |_____________/

  低频: Phase -> -90 deg (纯弹簧, 力超前速度 -90 deg)
  w_n:  Phase = 0 deg    (纯阻尼, 力与速度同相)
  高频: Phase -> +90 deg  (纯惯量, 力滞后速度 90 deg)

关键观察：相位在 \(\omega_n\) 处穿越零度。低频相位为 \(-90°\)（弹簧行为：力超前位移 \(90°\)，即相对速度为 \(-90°\)）；高频相位趋向 \(+90°\)（惯量行为：\(F = Ma\) 中力与加速度同相，即超前速度 \(90°\)）。阻尼比 \(\zeta\) 决定了相位穿越的"缓急"——\(\zeta\) 越大穿越越缓，\(\zeta\) 越小穿越越陡（共振峰越尖锐）。

导纳 Bode 图——工程设计的核心工具：

导纳 \(Y(s) = s/(M_d s^2 + D_d s + K_d)\) 是力控设计中更常用的频域工具，因为它直接给出"施加多大力能产生多大运动"：

\[|Y(j\omega)| = \frac{\omega}{\sqrt{(K_d - M_d\omega^2)^2 + (D_d\omega)^2}}\]

导纳 |Y(jw)| 的 Bode 幅频图（三种阻尼比对比）：

|Y| (dB)
  |
  |             * zeta=0.2 (尖锐共振峰)
  |            / \
  |           /   \     * zeta=0.7 (中等)
  |          /  +  \_  / \
  |         /  /     \/   \
  |        /  /  zeta=1    \
  |       /  /  (无峰,最平)  \
  |      /  /                 \
  |     /  /                   \
  |    /  /                     \
  +----/--/---------+------------\----------> log(w)
  1/K_d     w_n               1/(M_d*w)
  (+20dB/dec)                (-20dB/dec)

  低频 |Y| -> w/K_d: 力越大位移越大, 斜率 +20 dB/dec
  w_n 处: 共振峰高度 = 1/(2*zeta*M_d*w_n) = 1/D_d
  高频 |Y| -> 1/(M_d*w): 惯量限制高频运动, 斜率 -20 dB/dec

Bode 图设计方法——系统化的参数调节流程：

力控系统的 Bode 图设计遵循以下四步流程：

Step 1：确定力控带宽需求

从任务分析出发：打磨力波动频率 ~5-15 Hz，装配接触瞬态 ~20-50 Hz，人机协作力交互 ~2-10 Hz。力控带宽 \(\omega_n\) 应至少为任务最高频率的 3 倍。

Step 2：选择 \(K_d\) 和 \(M_d\) 满足带宽

\[\omega_n = \sqrt{K_d / M_d} \implies K_d = M_d \omega_n^2\]

若取 \(M_d = \Lambda_{ii}\)（操作空间惯量分量，Franka 约 1-5 kg），则：

目标带宽 \(f_n\)	\(\omega_n\) (rad/s)	\(K_d\)（\(M_d = 2\) kg 时）
5 Hz	31.4	1974 N/m
10 Hz	62.8	7896 N/m
20 Hz	125.7	31583 N/m
50 Hz	314.2	197392 N/m

注意 50 Hz 带宽对应 \(K_d \approx 2 \times 10^5\) N/m——远超 Franka 柔性关节刚度上界（\(\sim 5000\) N/m）。所以**实际力控带宽被柔性关节硬限制在约 20-30 Hz**。

Step 3：选择 \(D_d\) 满足阻尼比

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

工程推荐 \(\zeta \in [0.7, 1.0]\)。\(\zeta = 1\)（临界阻尼）是默认选择。

Step 4：验证环境耦合后的闭环 Bode 图

将环境模型代入，检查闭环带宽和阻尼比是否仍然满足要求。这一步在 2.3 节展开。

反事实推理：如果跳过 Step 4（不检查环境耦合）会怎样？你设计的开环 \(\zeta = 1\)，但与 \(K_e = 10^6\) N/m 的钢铁环境耦合后，闭环 \(\zeta_{cl}\) 可能降到 0.1——剧烈振荡。这就是"在仿真中调好的参数一上实机就炸"的根因之一——仿真中环境刚度通常比真实低很多。

数值设计案例——擦桌子任务：

给定：Franka Panda 末端 z 方向有效质量 \(\Lambda_{zz} \approx 2.5\) kg，任务力波动 ~5 Hz。

Step 1: 力控带宽 >= 3 * 5 Hz = 15 Hz -> omega_n = 2*pi*15 = 94.2 rad/s
Step 2: K_d = M_d * omega_n^2 = 2.5 * 94.2^2 = 22188 N/m
         -> 超过 Franka 上界! 降低到 K_d = 3000 N/m
         -> 实际 omega_n = sqrt(3000/2.5) = 34.6 rad/s = 5.5 Hz
Step 3: D_d = 2*1.0*sqrt(3000*2.5) = 173 Ns/m (临界阻尼)
Step 4: 与桌面 K_e = 10^5 N/m 耦合后:
         omega_cl = sqrt((3000+100000)/2.5) = 203 rad/s = 32 Hz
         zeta_cl = 173 / (2*sqrt(2.5*103000)) = 0.17  -- 严重欠阻尼!
         -> 需要增大 D_d 至 ~1000 Ns/m (zeta_cl ~ 1.0)
         -> 或降低 K_d 至 ~500 N/m (使 K_d << K_e)

本质洞察：Bode 图设计的核心矛盾是——高带宽需要高 \(K_d\)，但高 \(K_d\) 在刚性环境中破坏阻尼比。解决方案不是"找到完美的 \(K_d\)"，而是认识到 \(K_d \ll K_e\) 是力控的基本设计原则。当这个条件满足时，闭环行为几乎完全由控制器参数决定，不受环境影响——这正是"控制关系而非变量"的频域体现。

工程设计含义：

设计目标	参数调节	频域效果
增大静态柔顺性	降低 \(K_d\)	低频 $
抑制共振	增大 \(\zeta\)（增大 \(D_d\)）	共振峰压平
过滤高频力噪声	增大 \(M_d\)	高频 $
提高力控带宽	增大 \(\omega_n = \sqrt{K_d/M_d}\)	共振频率右移 → 更宽的可控频率范围

2.3 环境耦合后的闭环频率响应

当阻抗控制器（\(Z_r\)）与弹簧-阻尼器环境（\(Z_e = K_e/s + D_e\)）耦合时，接触力的频率响应为：

\[\frac{F_c(s)}{X_d(s)} = \frac{Z_e(s) \cdot K_d}{Z_r(s) + Z_e(s)}\]

简化到 \(D_e = 0\)（纯弹簧环境）：

\[\frac{F_c(s)}{X_d(s)} = \frac{K_e(M_d s^2 + D_d s + K_d)}{M_d s^2 + D_d s + (K_d + K_e)}\]

关键观察：闭环自然频率变为 \(\omega_{cl} = \sqrt{(K_d + K_e)/M_d}\)。当 \(K_e \gg K_d\) 时，\(\omega_{cl} \approx \sqrt{K_e/M_d}\)——闭环带宽完全由环境刚度决定，控制器失去了对频率响应的控制权。

稳态力的计算（阶跃位置参考 \(x_d\)）：

\[f_{ss} = \lim_{s\to 0} s \cdot \frac{F_c(s)}{s} \cdot X_d = \frac{K_e K_d}{K_d + K_e} x_d\]

当 \(K_e \gg K_d\) 时，\(f_{ss} \approx K_d x_d\)——力由控制器刚度决定（好的行为！）。 当 \(K_d \gg K_e\) 时，\(f_{ss} \approx K_e x_d\)——力由环境刚度决定（退化为位控行为）。

本质洞察：阻抗控制的精妙之处在于：当 \(K_d \ll K_e\)（末端比环境"软"）时，接触力 \(\approx K_d \cdot x_d\)——由控制器参数决定，不依赖未知的环境刚度。这正是"控制关系而非变量"的工程体现。

2.4 力控带宽的实际限制

理论上阻抗/导纳的带宽由 \(\omega_n\) 决定，但实际系统中还有多重限制：

限制因素	典型频率	公式/机制
采样率 Nyquist 限制	\(f_s/2\)	1kHz → 500 Hz
计算延迟	\(\sim 1/(2\pi \cdot 3\tau_{delay})\)	1ms 延迟 → ~53 Hz
力传感器滤波	低通截止频率	30-100 Hz（数字滤波器）
关节柔性（反共振）	Franka: ~50-150 Hz	双质量模型的反共振
位置控制器带宽（导纳控制）	内环带宽	UR: ~50-100 Hz
实际有效带宽	min(以上所有)	通常 30-100 Hz

带宽设计的三条黄金法则：

1. 力控环带宽 \(\leq\) 内环带宽 / 3（为相位裕度留余量）
2. 力传感器滤波截止 \(\geq\) 3 \(\times\) 力控环带宽（避免滤波延迟限制性能）
3. 采样率 \(\geq\) 10 \(\times\) 力控环带宽（确保离散化引起的误差可忽略）

练习

1. ⭐ Bode 图绘制：用 Python（scipy.signal）画出以下三种参数配置的导纳 \(Y(j\omega)\) Bode 图：(a) \(M_d=1, D_d=20, K_d=100\)；(b) \(M_d=1, D_d=40, K_d=100\)；(c) \(M_d=5, D_d=20, K_d=100\)。对比阻尼和惯量对带宽和共振的影响。
2. ⭐⭐ 闭环分析：对 (a) 的参数，分别与 \(K_e = 100, 1000, 10^5\) N/m 的环境耦合，画出闭环力频率响应。观察 \(K_e\) 如何改变系统行为。
3. ⭐⭐ 带宽设计：设计一个力控系统，要求：力控带宽 > 50 Hz，阻尼比 \(\zeta = 0.8\)，采样率 1 kHz。计算 \(M_d, D_d, K_d\) 的允许范围。

---

3. 无源性（Passivity）理论基础 ⭐⭐⭐

3.1 为什么需要无源性

回顾 F01 第 3.4 节：Colgate-Hogan 1988 定理告诉我们，如果控制器和环境都是无源的，耦合系统保证稳定。这给出了力控稳定性的充分条件。本节严格建立无源性的数学基础。

无源性在力控中的核心地位来自一个实际问题：我们通常不知道环境的精确模型。接触刚度 \(K_e\) 可能在 \(10^2\)（泡沫）到 \(10^8\)（钢铁）之间变化数个数量级。如果稳定性分析依赖于已知的 \(K_e\)，那么控制器需要为每种环境重新设计——这在实际中不可行。

无源性提供了**无模型的稳定性保证**：只要控制器无源，它与任何被动环境的耦合都稳定，不需要知道 \(K_e\)。

类比：电气工程中，一个纯电阻电路（无源）可以安全地连接到任何电源——不会产生振荡或爆炸。但如果电路中有放大器（有源），连接到某些负载时可能产生自激振荡。力控中的无源性扮演了同样的角色：无源的控制器是"安全的接口"。

3.2 无源性的严格定义

定义（输入-输出无源性）：

一个系统 \(\Sigma\) 具有输入 \(u(t)\) 和输出 \(y(t)\)，如果存在常数 \(\beta \geq 0\) 使得：

\[\int_0^T u(t)^T y(t) \, dt \geq -\beta, \quad \forall T \geq 0\]

则称系统是**无源的**（passive）。

物理含义：端口上的能量流入（\(u^T y\) 是瞬时功率，积分是累计能量）总是大于某个有限的负值。即系统最多只能释放有限的初始存储能量——不能"凭空产生"无限能量。

存储函数形式（等价定义）：

如果存在**存储函数** \(V(x) \geq 0\)（类比势能 + 动能）使得：

\[\dot{V}(x) \leq u^T y\]

则系统无源。这称为**耗散不等式**（dissipation inequality）。

严格输出无源性（Strict Output Passivity）：

\[\dot{V}(x) \leq u^T y - \epsilon \|y\|^2, \quad \epsilon > 0\]

系统不仅无源，而且**主动耗散能量**。阻抗控制器的阻尼项 \(D_d\) 正是提供这个"主动耗散"。

3.3 阻抗控制的无源性证明——完整推导

现在我们严格证明标准阻抗控制器的无源性。这个证明是力控理论中最重要的数学结果之一。

系统：阻抗控制器使末端行为满足

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

端口变量：输入 \(u = f_{ext}\)（环境施加的力），输出 \(y = \dot{\tilde{x}}\)（末端速度偏差）。

Step 1：构造存储函数

\[V(\tilde{x}, \dot{\tilde{x}}) = \underbrace{\frac{1}{2} \dot{\tilde{x}}^T M_d \dot{\tilde{x}}}_{T: \text{动能}} + \underbrace{\frac{1}{2} \tilde{x}^T K_d \tilde{x}}_{U: \text{势能}}\]

由于 \(M_d, K_d\) 正定，\(V \geq 0\)。且 \(V = 0\) 当且仅当 \(\tilde{x} = 0, \dot{\tilde{x}} = 0\)。

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

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

Step 3：代入动力学方程 \(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}}\]

Step 4：展开并化简

\[\dot{V} = \dot{\tilde{x}}^T f_{ext} - \dot{\tilde{x}}^T D_d \dot{\tilde{x}} \underbrace{- \dot{\tilde{x}}^T K_d \tilde{x} + \tilde{x}^T K_d \dot{\tilde{x}}}_{\text{相消}}\]

关键步骤：由于 \(K_d\) 对称，\(\dot{\tilde{x}}^T K_d \tilde{x}\) 是标量，其转置等于自身：\(\dot{\tilde{x}}^T K_d \tilde{x} = (\dot{\tilde{x}}^T K_d \tilde{x})^T = \tilde{x}^T K_d^T \dot{\tilde{x}} = \tilde{x}^T K_d \dot{\tilde{x}}\)，所以 \(-\dot{\tilde{x}}^T K_d \tilde{x} + \tilde{x}^T K_d \dot{\tilde{x}} = 0\)。

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

Step 5：得出结论

由于 \(D_d\) 正定，\(y^T D_d y \geq \lambda_{min}(D_d) \|y\|^2 > 0\)：

\[\boxed{\dot{V} \leq u^T y - \epsilon \|y\|^2, \quad \epsilon = \lambda_{min}(D_d) > 0}\]

系统是严格输出无源的。 \(\square\)

物理解读： - \(u^T y = f_{ext}^T \dot{\tilde{x}}\)：环境通过端口向系统注入的功率 - \(y^T D_d y = \dot{\tilde{x}}^T D_d \dot{\tilde{x}}\)：阻尼器耗散的功率（总是正的） - 不等式说：系统存储能量 \(V\) 的增长率 \(\leq\) 外部注入功率 \(-\) 阻尼耗散 \(\leq\) 外部注入功率

3.4 无源性定理——系统互联的稳定性

定理 1（并联无源）：两个无源系统 \(\Sigma_1, \Sigma_2\) 的并联（相同输入，输出相加）仍然是无源的。

证明：\(V = V_1 + V_2\)，\(\dot{V} = \dot{V}_1 + \dot{V}_2 \leq u^T y_1 + u^T y_2 = u^T(y_1 + y_2) = u^T y\)。\(\square\)

定理 2（串联/级联无源性）：两个无源系统的串联（\(\Sigma_1\) 的输出是 \(\Sigma_2\) 的输入）不一定无源。

为什么？串联改变了因果关系——\(\Sigma_1\) 的输出 \(y_1\) 变成 \(\Sigma_2\) 的输入 \(u_2\)，但 \(\Sigma_2\) 的输出 \(y_2\) 不再与 \(\Sigma_1\) 的输入 \(u_1\) 直接相关。端口功率 \(u_1^T y_2\) 与 \(u_1^T y_1\) 和 \(u_2^T y_2\) 都无直接关系——能量簿记断裂了。

类比：并联连接像两个并排的水库——总蓄水量是两者之和，不产生新水（无源）。串联像水从一个水库流到另一个——中间可能有高度差产生的势能转化为动能，看起来像"产生了"能量。

这个结论对力控的意义：阻抗控制（并联/反馈互联）的无源性分析是可靠的；但导纳控制（串联结构：力传感器→导纳滤波→位置控制器→机器人）的无源性分析更为复杂——需要逐层分析。

定理 3（反馈无源性/Colgate-Hogan）：

如果 \(\Sigma_1\) 和 \(\Sigma_2\) 都是**严格输出无源的**，则负反馈互联系统渐近稳定。

           +--------+        +--------+
  r=0 --> +| Sigma_1 |---+--->| Sigma_2 |---+
           +--------+   |    +--------+   |
              ^          |                 |
              |          +---------<-------+
              +--------(负反馈)

完整证明：

设 \(\Sigma_1\) 有存储函数 \(V_1\)，满足 \(\dot{V}_1 \leq u_1^T y_1 - \epsilon_1\|y_1\|^2\)。

设 \(\Sigma_2\) 有存储函数 \(V_2\)，满足 \(\dot{V}_2 \leq u_2^T y_2 - \epsilon_2\|y_2\|^2\)。

负反馈互联条件：\(u_1 = -y_2\)，\(u_2 = y_1\)。

取总存储函数 \(V = V_1 + V_2\)：

\[\dot{V} = \dot{V}_1 + \dot{V}_2 \leq u_1^T y_1 - \epsilon_1\|y_1\|^2 + u_2^T y_2 - \epsilon_2\|y_2\|^2\]

代入互联条件：

\[= (-y_2)^T y_1 - \epsilon_1\|y_1\|^2 + y_1^T y_2 - \epsilon_2\|y_2\|^2\]

关键一步：\((-y_2)^T y_1 + y_1^T y_2 = -y_2^T y_1 + y_1^T y_2 = 0\)（标量转置等于自身）

\[\boxed{\dot{V} \leq -\epsilon_1\|y_1\|^2 - \epsilon_2\|y_2\|^2 \leq 0}\]

\(\dot{V}\) 是负半定的。由 LaSalle 不变集定理，在 \(\dot{V} = 0\) 集合上 \(y_1 = 0, y_2 = 0\)，进而 \(u_1 = 0, u_2 = 0\)——系统回到原点。因此**渐近稳定**。\(\square\)

证明的物理解读：反馈互联中，\(\Sigma_1\) 向 \(\Sigma_2\) 传递的能量（\(y_1^T u_2 = y_1^T y_1 \cdot\) 某因子）恰好被 \(\Sigma_2\) 向 \(\Sigma_1\) 反馈的能量（\(y_2^T u_1 = -y_2^T y_2 \cdot\) 某因子）完全抵消——因为反馈的"负号"使两个能量流方向相反。剩下的只有各自的阻尼耗散 \(\epsilon_1\|y_1\|^2\) 和 \(\epsilon_2\|y_2\|^2\)——总能量单调下降。

应用于力控：

\(\Sigma_1\) = 机器人 + 控制器（阻抗 \(Z_r\)），端口：\((f_{robot}, v)\) \(\Sigma_2\) = 环境（阻抗 \(Z_e\)），端口：\((f_{env}, v)\)

耦合条件：\(f_{robot} = -f_{env}\)（Newton 第三定律），\(v_{robot} = v_{env}\)（速度连续）

如果两者都严格输出无源 → 耦合渐近稳定。

工程意义：这意味着只要你的阻抗控制器是无源的（\(D_d > 0\)），无论机器人碰到什么被动环境——从软海绵到硬钢板——都保证稳定。不需要知道环境参数。

定理 4（无源性的频域等价条件——正实引理）：

一个线性时不变系统 \(H(s) = C(sI - A)^{-1}B + D\) 是无源的，当且仅当 \(H(s)\) 是**正实的**（Positive Real）：

\[\text{Re}[H(j\omega)] \geq 0, \quad \forall \omega \geq 0\]

对于阻抗传递函数 \(Z(s) = M_d s + D_d + K_d/s\)：

\[\text{Re}[Z(j\omega)] = \text{Re}[M_d j\omega + D_d - jK_d/\omega] = D_d\]

由于 \(D_d > 0\)，实部恒正。注意 \(Z(s)\) 在 \(s = 0\) 处有一个简单极点（来自 \(K_d/s\) 项），因此它不是严格正实的（严格正实要求在整个闭右半平面包括虚轴上解析），而是**具有简单原点极点的正实传递函数**（positive real with a simple pole at origin）。从无源性角度看，\(K_d/s\) 对应的积分器（弹簧）是无损存储元件，\(D_d > 0\) 保证了耗散——系统整体是无源的。

与电路的联系：正实阻抗在电气工程中等价于"纯电阻+电感+电容网络"——这类网络不含放大器，不产生能量。力控中的阻抗控制器正是这样的"纯被动电路"——\(K_d\) 是电容（存储弹性势能），\(D_d\) 是电阻（耗散能量），\(M_d\) 是电感（存储动能）。

3.5 变阻抗对无源性的破坏——能量注入问题

当阻抗参数 \(K_d(t)\) 随时间变化时（变阻抗控制，F06 主题），无源性证明中的关键步骤不再成立。

存储函数变为：

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

时间导数多出一项：

\[\dot{V} = \dot{\tilde{x}}^T f_{ext} - \dot{\tilde{x}}^T D_d \dot{\tilde{x}} + \underbrace{\frac{1}{2}\tilde{x}^T \dot{K}_d(t) \tilde{x}}_{\text{新增项！}}\]

最后一项 \(\frac{1}{2}\tilde{x}^T \dot{K}_d \tilde{x}\) 可能为正！

物理含义：增大弹簧刚度等效于向虚拟弹簧"注入"弹性势能。如果注入速率超过阻尼耗散速率，总能量 \(V\) 可能持续增长——系统变得不稳定。

反事实推理：如果 \(\dot{K}_d < 0\)（刚度只降不升）呢？此时 \(\frac{1}{2}\tilde{x}^T\dot{K}_d\tilde{x} \leq 0\)，无源性仍然保持！所以**降低刚度总是安全的，升高刚度才是危险的**。这对变阻抗策略设计有直接指导意义。

两种解决方案（将在 F06 详讲）：

方案	核心思想	约束形式
瞬时被动性条件	限制 \(\dot{K}_d\) 使额外项被阻尼吸收	\(\frac{1}{2}\tilde{x}^T\dot{K}_d\tilde{x} \leq \dot{\tilde{x}}^T D_d \dot{\tilde{x}}\)
能量罐 (Energy Tank)	用虚拟能量存储器缓冲能量注入	\(E_{tank}(t) \geq 0\) 作为运行时约束

瞬时被动性条件的推导（注意：这是一个比 Kronander-Billard 2016 定理更严格的逐点条件，F06 中将介绍 KB 定理的正式条件）：

要求变阻抗系统仍然满足耗散不等式：

\[\dot{V} = \dot{\tilde{x}}^T f_{ext} - \dot{\tilde{x}}^T D_d \dot{\tilde{x}} + \frac{1}{2}\tilde{x}^T \dot{K}_d \tilde{x} \leq \dot{\tilde{x}}^T f_{ext}\]

即要求多出的能量注入项被阻尼完全吸收：

\[\frac{1}{2}\tilde{x}^T \dot{K}_d \tilde{x} \leq \dot{\tilde{x}}^T D_d \dot{\tilde{x}}\]

当 \(\dot{\tilde{x}} \approx 0\)（末端几乎静止）且 \(\tilde{x} \neq 0\)（有位移偏差）时，左边可能为正而右边 \(\approx 0\)——瞬时被动性条件要求 \(\dot{K}_d \leq 0\) 或 \(\tilde{x} = 0\)。这限制了"在有偏差时升高刚度"的能力——恰好是工程中最常见的需求。Kronander-Billard 2016 定理（F06 详述）通过引入增广 Lyapunov 函数放松了这一限制。

类比：瞬时被动性条件就像银行的贷款约束——你只能在"有还款能力"（阻尼耗散足够）时"借钱"（注入能量）。如果你静止不动（\(\dot{\tilde{x}} = 0\)），没有还款能力，就不允许注入任何能量。

能量罐方法的基本思想：

引入虚拟能量存储变量 \(s\)（"罐中水位"），满足：

\[\dot{s} = \frac{D_d \dot{\tilde{x}}^T \dot{\tilde{x}}}{s} - \frac{\alpha}{s} \quad \text{(从阻尼"收集"能量到罐中)}\]

刚度变化所需能量从罐中"支取"：

\[\text{if } E_{tank} = \frac{1}{2}s^2 > \frac{1}{2}\tilde{x}^T \dot{K}_d \tilde{x} \cdot dt: \quad \text{允许变化}$$ $$\text{else}: \quad \dot{K}_d = 0 \quad \text{（冻结参数，等罐充满）}\]

能量罐的优势：比 KB 条件更灵活——它允许"暂时借支"能量（只要罐不见底），而 KB 条件要求每个时刻都满足。代价：需要额外维护一个状态变量 \(s\)，且初始罐容量 \(s(0)\) 需要调参。

变阻抗对阻尼比的影响：

当 \(K_d\) 从 \(K_1\) 变化到 \(K_2 > K_1\) 时，如果 \(D_d\) 不同步更新，阻尼比 \(\zeta = D_d/(2\sqrt{K_d M_d})\) 会下降——可能从临界阻尼进入欠阻尼区间。

设计建议：变 \(K_d\) 时必须同步更新 \(D_d\)：

\[D_d(t) = 2\zeta_{target} \sqrt{K_d(t) \cdot M_d}\]

这保证阻尼比恒定。但注意：\(D_d\) 的变化本身不注入能量（因为 \(V\) 中没有 \(D_d\) 项），所以无源性不受影响。

3.6 李雅普诺夫稳定性在力控中的应用

无源性证明中的存储函数 \(V\) 与李雅普诺夫稳定性分析直接关联。

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

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

\(\dot{V}\) 是半负定的（当 \(\dot{\tilde{x}} = 0\) 时 \(\dot{V} = 0\)），不是严格负定。需要用 LaSalle 不变集定理：

在 \(\dot{V} = 0\) 的集合上（即 \(\dot{\tilde{x}} = 0\)），代入动力学方程 \(K_d\tilde{x} = 0\)。由于 \(K_d\) 正定 → \(\tilde{x} = 0\)。不变集 = \(\{(\tilde{x}, \dot{\tilde{x}}) = (0, 0)\}\) → 渐近稳定。

有外力时的输入-输出稳定性：

当 \(f_{ext} \neq 0\) 时，平衡点移至 \(\tilde{x}_{eq} = K_d^{-1} f_{ext}\)（弹簧平衡位移）。系统不再回到原点，而是收敛到新的平衡——这正是阻抗控制的设计意图。

与 SLAM 的联系：李雅普诺夫函数在力控中的角色类似于 SLAM 优化中的代价函数——都是"能量"函数，系统/优化器试图最小化它。力控中 \(V\) 自然下降（阻尼耗散）；SLAM 中代价函数通过 Gauss-Newton 迭代下降。结构上惊人相似。

练习

1. ⭐⭐ 无源性证明练习：对纯阻尼系统 \(D\dot{x} = f\)（只有阻尼器，无弹簧和惯量），构造存储函数并证明其无源性。它是严格输出无源的吗？
2. ⭐⭐⭐ 变阻抗分析：在 1-DOF 系统中，\(K_d\) 从 100 N/m 线性增加到 500 N/m（用时 1 秒），\(D_d = 20\) Ns/m, \(M_d = 1\) kg，初始偏差 \(\tilde{x}(0) = 0.05\) m。用数值仿真计算 \(V(t)\) 的变化——无源性是否被破坏？
3. ⭐⭐⭐ LaSalle 应用：对笛卡尔阻抗控制系统（\(M_d = \Lambda\), \(D_d > 0\), \(K_d > 0\), \(f_{ext} = 0\)），使用 LaSalle 不变集定理严格证明 \((\tilde{x}, \dot{\tilde{x}}) \to (0, 0)\)。明确指出每一步使用了哪个条件。
4. ⭐⭐⭐ 反馈无源性证明手推：手推定理 3（反馈无源性）的完整证明。关键步骤是交叉项 \((-y_2)^T y_1 + y_1^T y_2 = 0\) 的消去——写出每一步并解释为什么标量转置等于自身。然后讨论：如果 \(\Sigma_1\) 只是无源的（非严格）而 \(\Sigma_2\) 是严格无源的，互联系统是否仍然渐近稳定？
5. ⭐⭐⭐ 正实引理验证：对导纳传递函数 \(Y(s) = s/(Ms^2 + Ds + K)\) 计算 \(\text{Re}[Y(j\omega)]\)。\(Y(s)\) 是正实的吗？\(Y(s)\) 是否无源？这对导纳控制的稳定性分析有什么影响？
6. ⭐⭐⭐⭐ 跨章综合——能量罐设计：对 1-DOF 变阻抗系统，\(K_d\) 需要在 \([100, 2000]\) N/m 之间变化，\(M_d = 2\) kg，\(\zeta = 1\)（\(D_d\) 同步更新）。设计能量罐的初始容量 \(E_{tank}(0)\)，使得 \(K_d\) 能在 0.5 秒内从 100 线性增加到 2000 而不"见底"。提示：计算 \(\frac{1}{2}\tilde{x}^T \dot{K}_d \tilde{x}\) 在最坏情况（\(\tilde{x}_{max} = K_d^{-1} f_{max}\)）下的功率。

---

4. 操作空间动力学——从关节空间到笛卡尔空间 ⭐⭐

4.1 推导动机

回顾 M01：关节空间动力学 \(M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau\) 描述了机器人的"内部"行为。但力控需要在**笛卡尔空间**（末端位姿空间）设计控制器——因为任务是在笛卡尔空间定义的（如"沿桌面以 20N 法向力滑动"）。操作空间动力学正是将关节空间动力学"投影"到笛卡尔空间的数学工具。

Khatib 1987 的核心贡献：推导出操作空间动力学方程

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

并定义了**动力学一致伪逆** \(\bar{J}\)，使得零空间和任务空间在动力学意义下解耦。

4.2 完整推导

起点：关节空间动力学

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

Step 1：末端速度和加速度与关节量的关系

\[\dot{x} = J\dot{q}, \quad \ddot{x} = J\ddot{q} + \dot{J}\dot{q} \tag{2}\]

Step 2：对冗余机械臂（\(n > 6\)），关节加速度可分解为

\[\ddot{q} = \bar{J}(\ddot{x} - \dot{J}\dot{q}) + (I - \bar{J}J)\ddot{q}_{null} \tag{3}\]

其中 \(\bar{J}\) 是待定的伪逆，\((I - \bar{J}J)\ddot{q}_{null}\) 是零空间分量。

Step 3：定义 \(\bar{J}\) 使零空间在动力学意义下与任务空间解耦。

将 (3) 代入 (1) 并左乘 \(\bar{J}^T\)：

\[\bar{J}^T M \bar{J}(\ddot{x} - \dot{J}\dot{q}) + \bar{J}^T M(I - \bar{J}J)\ddot{q}_{null} + \bar{J}^T(C\dot{q} + g) = \bar{J}^T\tau + \bar{J}^T J^T f_{ext}\]

要求零空间项 \(\bar{J}^T M(I - \bar{J}J) = 0\)。这要求：

\[\bar{J} = M^{-1}J^T(JM^{-1}J^T)^{-1} = M^{-1}J^T\Lambda \tag{4}\]

其中 \(\Lambda = (JM^{-1}J^T)^{-1}\) 是操作空间惯量矩阵。

Step 4：验证 \(\bar{J}^T M \bar{J} = \Lambda\)

\[\bar{J}^T M \bar{J} = \Lambda J M^{-1} M M^{-1} J^T \Lambda = \Lambda (J M^{-1} J^T) \Lambda = \Lambda \Lambda^{-1} \Lambda = \Lambda \; \checkmark\]

Step 5：验证 \(\bar{J}^T J^T = I\)

\[\bar{J}^T J^T = (J\bar{J})^T\]

\[J\bar{J} = J M^{-1} J^T (J M^{-1} J^T)^{-1} = I \implies \bar{J}^T J^T = I \; \checkmark\]

Step 6：整理得操作空间动力学

\[\boxed{\Lambda(x)\ddot{x} + \mu(x,\dot{x}) + p(x) = F} \tag{5}\]

其中：

量	表达式	物理含义
\(\Lambda\)	\((JM^{-1}J^T)^{-1}\)	操作空间惯量（\(6\times6\), SPD）
\(\mu\)	\(\bar{J}^T C\dot{q} - \Lambda\dot{J}\dot{q}\)	操作空间科氏力/离心力
\(p\)	\(\bar{J}^T g\)	操作空间重力
\(F\)	\(\bar{J}^T\tau + f_{ext}\)	操作空间广义力

4.3 动力学一致伪逆 vs Moore-Penrose 伪逆

Moore-Penrose 伪逆：\(J^+ = J^T(JJ^T)^{-1}\)，最小化 \(\|\dot{q}\|_2^2\)。

动力学一致伪逆：\(\bar{J} = M^{-1}J^T\Lambda\)，最小化 \(\dot{q}^T M \dot{q}\)（动能）。

性质	\(\bar{J}\)	\(J^+\)
加权范数	最小动能 \(\dot{q}^T M \dot{q}\)	最小欧氏 \(\|\dot{q}\|^2\)
零空间解耦	是：\(N^T\tau_{null}\) 不影响 \(\ddot{x}\)	否：通过惯量耦合影响 \(\ddot{x}\)
计算代价	需 \(M^{-1}\)（\(O(n^2)\)）	仅需 \(J\)（\(O(6n)\)）
适用场景	力矩级控制（阻抗/WBC）	速度级控制（IK）

本质洞察：\(\bar{J}\) 与 \(J^+\) 的差异**不是**精度问题，而是**物理正确性**问题。如果你在阻抗控制中用 \(J^+\) 做零空间投影，零空间力矩会通过惯量耦合"泄漏"到末端——这是很多"阻抗控制抖动"的根因。只有 \(\bar{J}\) 能保证零空间和任务空间的**动力学解耦**。

4.4 操作空间科氏力项 \(\mu\) 的完整推导

操作空间动力学中最容易被教材"跳过"的是 \(\mu\) 项的推导。\(\mu = \bar{J}^T C\dot{q} - \Lambda\dot{J}\dot{q}\) 包含两部分——让我们追踪每一部分的来源。

回到 Step 6 的推导中间步骤。将 \(\ddot{q} = \bar{J}(\ddot{x} - \dot{J}\dot{q}) + N\ddot{q}_{null}\) 代入 \(M\ddot{q} + C\dot{q} + g = \tau + J^T f_{ext}\)，左乘 \(\bar{J}^T\)：

\[\bar{J}^T M \bar{J}(\ddot{x} - \dot{J}\dot{q}) + \underbrace{\bar{J}^T M N \ddot{q}_{null}}_{= 0 \text{ (by design)}} + \bar{J}^T C\dot{q} + \bar{J}^T g = \bar{J}^T \tau + \underbrace{\bar{J}^T J^T}_{= I} f_{ext}\]

已验证 \(\bar{J}^T M \bar{J} = \Lambda\)，展开第一项：

\[\Lambda \ddot{x} - \Lambda \dot{J}\dot{q} + \bar{J}^T C\dot{q} + \bar{J}^T g = \bar{J}^T \tau + f_{ext}\]

移项得：

\[\Lambda \ddot{x} + \underbrace{(\bar{J}^T C\dot{q} - \Lambda \dot{J}\dot{q})}_{\mu} + \underbrace{\bar{J}^T g}_{p} = \underbrace{\bar{J}^T \tau + f_{ext}}_{F}\]

因此 \(\mu = \bar{J}^T C\dot{q} - \Lambda\dot{J}\dot{q}\) 的两项有清晰的物理含义：

项	表达式	物理含义
\(\bar{J}^T C\dot{q}\)	关节空间科氏力在操作空间的投影	来自关节空间的科氏/离心力
\(-\Lambda\dot{J}\dot{q}\)	Jacobian 时变导致的虚拟力	因为 \(J\) 随构型变化，同样的关节速度在不同时刻产生不同的末端速度

\(\dot{J}\dot{q}\) 的物理意义：这一项常被误解为"关节加速度的贡献"。实际上，它是**参考系变化率**的体现——即使关节速度恒定（\(\ddot{q} = 0\)），末端加速度 \(\ddot{x} = J\ddot{q} + \dot{J}\dot{q} = \dot{J}\dot{q} \neq 0\)。这类似于在旋转参考系中看到的科氏力——来源于参考系本身在变化。

常见陷阱：在 Pinocchio 中计算 \(\dot{J}\dot{q}\) 必须先调用 forwardKinematics(model, data, q, v, a_zero) 或 computeJointJacobiansTimeVariation。忘记这一步会导致 \(\dot{J}\dot{q} = 0\)——操作空间控制器缺失科氏力补偿，高速运动时末端会偏离期望轨迹。

4.5 零空间投影与多优先级

零空间投影矩阵：\(N = I - \bar{J}J\)

关键性质（逐一验证）：

性质	表达式	意义
幂等性	\(N^2 = N\)	\(N\) 是投影矩阵
任务空间零化	\(JN = 0\)	零空间运动不产生末端运动
动力学一致性	\(N^T M = MN\)	\(M\) 意义下的正交投影

幂等性证明：

\[N^2 = (I - \bar{J}J)(I - \bar{J}J) = I - \bar{J}J - \bar{J}J + \bar{J}\underbrace{J\bar{J}}_{= I}J = I - \bar{J}J - \bar{J}J + \bar{J}J = I - \bar{J}J = N \; \checkmark\]

动力学一致性证明（这是 \(\bar{J}\) 区别于 \(J^+\) 的关键性质）：

\[N^T M = (I - J^T\bar{J}^T)M = M - J^T\bar{J}^T M\]

\[= M - J^T \Lambda J M^{-1} M = M - J^T \Lambda J\]

\[MN = M(I - \bar{J}J) = M - M \bar{J} J = M - M M^{-1} J^T \Lambda J = M - J^T \Lambda J\]

\[\therefore N^T M = MN \; \checkmark\]

物理含义：\(N\) 是 \(M\)-正交投影——它在**动能度量**下将运动分解为任务空间和零空间分量，使两者在能量意义下完全解耦。如果用 \(J^+\) 代替 \(\bar{J}\)，得到的 \(N_{J^+}\) 是欧氏正交投影——在欧氏度量下解耦，但在动能度量下不解耦。动能耦合意味着零空间力矩会通过惯量矩阵"泄漏"到任务空间——这正是 F02 第 4.3 节中"\(J^+\) 导致末端抖动"的数学根因。

多优先级控制律（Sentis-Khatib 2005）：

\[\tau = J_1^T F_1 + N_1^T J_2^T F_2 + N_2^T \tau_{null}\]

回顾 Ch53（WBC）：这个多优先级框架正是 WBC 的理论基础。在足式方向中，\(F_1\) 是质心控制力（最高优先级），\(F_2\) 是摆腿任务。在机械臂力控中，\(F_1\) 是末端阻抗力，\(F_2\) 是零空间行为（关节中心化、操作度优化等）。同一个数学框架，不同的物理场景。

4.5 Pinocchio 计算实现

#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/jacobian.hpp>

// 计算操作空间动力学量
// 输入: model, data, q, v, ee_frame_id
// 输出: Lambda (6x6), Jbar (nx6), N (nxn), p (6x1)

// 1. 计算 M^{-1}
pinocchio::computeMinverse(model, data, q);
Eigen::MatrixXd Minv = data.Minv;
// 补全下三角
Minv.triangularView<Eigen::StrictlyLower>() =
    Minv.transpose().triangularView<Eigen::StrictlyLower>();

// 2. 计算 Jacobian J (6 x nv)
pinocchio::computeJointJacobians(model, data, q);
pinocchio::updateFramePlacements(model, data);
Eigen::MatrixXd J(6, model.nv);
J.setZero();
pinocchio::getFrameJacobian(model, data, ee_frame_id,
    pinocchio::LOCAL_WORLD_ALIGNED, J);

// 3. Lambda = (J M^{-1} J^T)^{-1}
Eigen::Matrix<double,6,6> JMinvJt = J * Minv * J.transpose();
Eigen::Matrix<double,6,6> Lambda = JMinvJt.inverse();

// 4. Jbar = M^{-1} J^T Lambda
Eigen::MatrixXd Jbar = Minv * J.transpose() * Lambda;

// 5. N = I - Jbar J
Eigen::MatrixXd N = Eigen::MatrixXd::Identity(model.nv, model.nv)
                    - Jbar * J;

// 6. p = Jbar^T g(q)
pinocchio::computeGeneralizedGravity(model, data, q);
Eigen::Matrix<double,6,1> p = Jbar.transpose() * data.g;

// 验证: J * N 应近似为零矩阵
assert((J * N).norm() < 1e-10);
// 验证: N^2 应近似等于 N
assert((N * N - N).norm() < 1e-10);

4.6 奇异性处理——阻尼最小二乘（DLS）

在奇异位形附近，\(\Lambda = (JM^{-1}J^T)^{-1}\) 的条件数趋于无穷。

**阻尼最小二乘**正则化：

\[\Lambda_{DLS} = (JM^{-1}J^T + \lambda^2 I)^{-1}\]

\(\lambda\) 是阻尼因子。

可变阻尼策略（远离奇异时 \(\lambda = 0\)，接近时逐渐增大）：

\[\lambda^2 = \begin{cases} 0 & \text{if } \sigma_{min} \geq \sigma_0 \\ \lambda_{max}^2 (1 - (\sigma_{min}/\sigma_0)^2) & \text{if } \sigma_{min} < \sigma_0 \end{cases}\]

其中 \(\sigma_{min}\) 是 \(J\) 的最小奇异值，\(\sigma_0\) 是阈值（典型取 0.01-0.05）。

练习

1. ⭐⭐ 手动推导：对 2-DOF 平面机械臂（\(l_1 = l_2 = 1\) m），手推 \(\Lambda\)（\(2\times2\) 矩阵）的解析表达式。在 \(q_2 = 0\)（全伸展）时，\(\Lambda\) 的条件数是多少？
2. ⭐⭐ Pinocchio 验证：用 Pinocchio 加载 Franka Panda，在 10 个随机构型下计算 \(\Lambda\) 的条件数。最大条件数出现在什么构型？
3. ⭐⭐⭐ \(\bar{J}\) vs \(J^+\) 实验：在 MuJoCo 中实现 7-DOF Panda 的阻抗控制，分别用 \(\bar{J}\) 和 \(J^+\) 做零空间投影。施加零空间关节正则化力矩，对比末端位置是否受到干扰。

---

5. 力椭球与速度椭球的对偶关系 ⭐⭐

5.1 操作度与椭球

速度椭球：满足 \(\|\dot{q}\| \leq 1\) 时末端速度集合

\[\mathcal{E}_v = \{v \in \mathbb{R}^6 : v^T(JJ^T)^{-1}v \leq 1\}\]

半轴长度 = \(J\) 的奇异值 \(\sigma_1 \geq ... \geq \sigma_6\)。

力椭球：满足 \(\|\tau\| \leq 1\) 时末端力集合

\[\mathcal{E}_f = \{f \in \mathbb{R}^6 : f^T JJ^T f \leq 1\}\]

半轴长度 = \(1/\sigma_1 \leq ... \leq 1/\sigma_6\)。

对偶关系：速度椭球和力椭球的半轴**互为倒数**。

如果速度椭球在某方向很"长"（容易运动）	则力椭球在同一方向很"短"（难以施力）
\(\sigma_i\) 大 → 速度椭球该方向半轴长	\(1/\sigma_i\) 小 → 力椭球该方向半轴短

类比：自行车的齿轮比。高速挡让你骑得快但爬坡费力；低速挡让你爬坡容易但速度慢。机械臂的构型就像齿轮比——在某些姿态下运动灵活但施力困难，反之亦然。

Yoshikawa 操作度：\(w = \sqrt{\det(JJ^T)} = \prod_{i=1}^{6}\sigma_i\)

\(w\) 衡量椭球的"体积"——越大越灵活。\(w = 0\) 是奇异位形。

5.2 对力控设计的启示

力控任务需要同时考虑力和运动：

任务需求	对构型的要求	设计指导
法向精密力跟踪	力椭球法向轴长	让 \(J\) 的最小奇异值对应法向
切向快速运动	速度椭球切向轴长	让 \(J\) 的最大奇异值对应切向
打磨（法向力+切向速度）	力椭球法向长且速度椭球切向长	矛盾！需在构型空间中权衡

实际设计方法：在规划打磨路径时，同时优化操作度——选择沿路径的机械臂构型使得法向力能力和切向速度能力同时最大化。

动态操作度——考虑惯量的操作度：

Yoshikawa 操作度 \(w = \sqrt{\det(JJ^T)}\) 只考虑了运动学——它假设所有关节"一样重要"。但实际中，不同关节的惯量不同——驱动大惯量关节需要更多力矩。

动态操作度（Khatib 1995）考虑了惯量加权：

\[w_{dyn} = \sqrt{\det(\Lambda^{-1})} = \sqrt{\det(J M^{-1} J^T)}\]

\(w_{dyn}\) 大意味着末端在该构型下"容易加速"——因为 \(\Lambda^{-1}\) 大等价于有效质量 \(\Lambda\) 小。

操作度类型	公式	优化目标	适用场景
运动学操作度	\(\sqrt{\det(JJ^T)}\)	远离奇异	运动规划
动态操作度	\(\sqrt{\det(JM^{-1}J^T)}\)	低惯量方向	力控/冲击安全
力操作度	\(\sqrt{\det((JJ^T)^{-1})}\)	大力方向	重载操作

反事实推理：如果只用运动学操作度来优化打磨构型，可能选到一个"运动灵活但力很弱"的姿态——因为运动学操作度不考虑力能力。动态操作度同时考虑了两者，对力控任务是更合适的优化目标。

操作度在零空间优化中的应用：

在阻抗控制的零空间中，常用操作度梯度作为优化目标——让机器人在执行主任务的同时保持高操作度：

\[\tau_{null} = N^T \Big( -K_{null}(q - q_{mid}) + \alpha \nabla_q w(q) \Big)\]

其中 \(\nabla_q w(q)\) 是操作度对关节角的梯度，\(\alpha\) 是权重。Pinocchio 不直接提供 \(\nabla_q w\)，需要数值差分或解析推导。

练习

1. ⭐ 椭球可视化：用 Pinocchio 计算 Franka Panda 在默认构型下的 Jacobian，SVD 分解，画出 2D（xy 平面）速度椭球和力椭球。
2. ⭐⭐ 构型优化：对 Panda 随机采样 1000 构型，计算操作度 \(w\) 和最小奇异值 \(\sigma_{min}\)。画散点图。操作度最大的构型是否也是 \(\sigma_{min}\) 最大的？
3. ⭐⭐ 动态操作度：在同样的 1000 构型中，计算动态操作度 \(w_{dyn}\) 并与运动学操作度 \(w\) 做散点图对比。两者的最优构型相同吗？如果不同，说明物理原因。

---

6. 力/力矩传感器模型与噪声特性 ⭐⭐

6.1 传感器信号模型

力/力矩传感器的实际测量值可以建模为：

\[f_{measured}(t) = f_{true}(t) + f_{gravity}(q) + f_{bias}(T) + n_{sensor}(t) + n_{emi}(t)\]

分量	来源	频率特征	处理方法
\(f_{true}\)	真实接触力	DC ~ 数百 Hz	目标信号
\(f_{gravity}(q)\)	末端工具重力	随构型缓变	在线重力补偿
\(f_{bias}(T)\)	温度漂移	极低频（分钟级）	定期零偏标定
\(n_{sensor}\)	传感器热噪声	宽带白噪声	低通滤波
\(n_{emi}\)	电磁干扰	特定频率（电机 PWM）	陷波滤波器

6.2 低通滤波器设计

力信号的低通滤波是力控工程中最常见的信号处理步骤。

二阶 Butterworth 滤波器（最常用，通带最大平坦）：

\[H(s) = \frac{\omega_c^2}{s^2 + \sqrt{2}\omega_c s + \omega_c^2}\]

离散化实现（双线性变换 / Tustin 方法，实时安全）：

// 二阶 Butterworth 低通滤波器（实时安全：固定大小，无动态分配）
class ButterworthLPF2 {
public:
    ButterworthLPF2(double cutoff_hz, double sample_rate_hz) {
        double wc = 2.0 * M_PI * cutoff_hz;
        double T = 1.0 / sample_rate_hz;
        // 预畸变
        double wa = 2.0 / T * std::tan(wc * T / 2.0);
        double k = wa * T / 2.0;
        double k2 = k * k;
        double sqrt2k = std::sqrt(2.0) * k;
        double denom = 1.0 + sqrt2k + k2;

        b0_ = k2 / denom;
        b1_ = 2.0 * b0_;
        b2_ = b0_;
        a1_ = 2.0 * (k2 - 1.0) / denom;
        a2_ = (1.0 - sqrt2k + k2) / denom;
    }

    double filter(double input) {
        double output = b0_ * input + b1_ * x1_ + b2_ * x2_
                       - a1_ * y1_ - a2_ * y2_;
        x2_ = x1_; x1_ = input;
        y2_ = y1_; y1_ = output;
        return output;
    }

    void reset() { x1_ = x2_ = y1_ = y2_ = 0.0; }

private:
    double b0_, b1_, b2_, a1_, a2_;
    double x1_ = 0, x2_ = 0, y1_ = 0, y2_ = 0;
};

截止频率选择准则：

力控环带宽	推荐截止频率	理由
50 Hz	150-200 Hz	3-4x 余量减少相位延迟影响
100 Hz	300-400 Hz	同上
不确定	30-50 Hz	保守选择，大多数任务够用

6.3 工具重力补偿

统一坐标约定：\(W\) 为世界/基座系，\(S\) 为 F/T 传感器系，\(R_{WS}\) 表示从传感器系到世界系的旋转，\(g^W=[0,0,-9.81]^T\)。传感器原始 wrench 在 \(S\) 系表达时，工具重力也必须先变换到 \(S\) 系：

\[f_g^S = R_{WS}^T(m_{tool}g^W)\]

\[\tau_g^S = r_{CoM}^S \times f_g^S\]

因此环境外力估计为

\[\boxed{w_{ext}^S = \begin{bmatrix} f_{raw}^S \\ \tau_{raw}^S \end{bmatrix} - \begin{bmatrix} f_g^S \\ \tau_g^S \end{bmatrix}}\]

三姿态标定法——完整推导：

在无外部接触时，传感器读数完全由工具重力决定：

\[f_{raw,k} = R_k^T \begin{bmatrix} 0 \\ 0 \\ -m g \end{bmatrix}, \quad k = 1, 2, 3\]

其中 \(R_k\) 是第 \(k\) 个标定姿态下传感器相对于世界坐标系的旋转矩阵。

定义 \(f_g = \begin{bmatrix} 0 \\ 0 \\ -mg \end{bmatrix}\)（世界坐标系中的重力向量），则每个姿态给出一个方程：

\[f_{raw,k} = R_k^T f_g\]

三个姿态的 9 个标量方程（但 \(f_g\) 只有 3 个未知数，超定系统）用最小二乘求解：

\[\hat{f}_g = \arg\min_{f_g} \sum_{k=1}^{3} \|f_{raw,k} - R_k^T f_g\|^2\]

解为：\(\hat{f}_g = (\sum_k R_k R_k^T)^{-1} \sum_k R_k f_{raw,k}\)

从 \(\hat{f}_g\) 提取 \(m_{tool} = \|\hat{f}_g\|/g\)。

力矩方程用于求 \(r_{CoM}\)：

\[\tau_{raw,k} = r_{CoM}^S \times (R_k^T f_g)\]

展开叉积并线性化时要注意符号：\(r\times f = -[f]_\times r\)。因此可写成 \(\tau_{raw,k} = A_k r_{CoM}^S\)，其中 \(A_k=-[R_k^T f_g]_\times\)。合并三个姿态求最小二乘解。

标定质量验证清单：

检查项	合格标准	不合格处理
第四姿态残差	\(< 0.5\) N	增加标定姿态数到 5-6
质量一致性	\(\|m_{est} - m_{scale}\| < 0.05\) kg	检查传感器标定矩阵
重心位置合理性	\(r_{CoM}\) 在工具几何范围内	检查传感器安装方向
温度重复性	不同温度下质量估计差 \(< 0.02\) kg	加温度补偿或缩短标定周期

// 三姿态标定实现（C++/Eigen）
struct GravityCalibration {
    double m_tool;                    // 标定质量 (kg)
    Eigen::Vector3d r_com;            // 标定重心 (m, 传感器坐标系)
    Eigen::Vector3d f_gravity_world;  // 重力向量 (N, 世界系)

    static GravityCalibration calibrate(
        const std::vector<Eigen::Matrix3d>& R_sensor_world,  // 3+ 姿态
        const std::vector<Eigen::Vector3d>& f_raw,           // 对应力读数
        const std::vector<Eigen::Vector3d>& tau_raw)          // 对应力矩读数
    {
        // 最小二乘求 f_gravity
        Eigen::Matrix3d ATA = Eigen::Matrix3d::Zero();
        Eigen::Vector3d ATb = Eigen::Vector3d::Zero();
        for (size_t k = 0; k < R_sensor_world.size(); ++k) {
            ATA += R_sensor_world[k] * R_sensor_world[k].transpose();
            ATb += R_sensor_world[k] * f_raw[k];
        }
        Eigen::Vector3d fg = ATA.ldlt().solve(ATb);

        GravityCalibration cal;
        cal.f_gravity_world = fg;
        cal.m_tool = fg.norm() / 9.81;

        // 最小二乘求 r_com（从力矩方程）
        // tau_k = r_com x f_g^S = -skew(f_g^S) * r_com
        Eigen::MatrixXd A(3 * R_sensor_world.size(), 3);
        Eigen::VectorXd b(3 * R_sensor_world.size());
        for (size_t k = 0; k < R_sensor_world.size(); ++k) {
            Eigen::Vector3d fg_local = R_sensor_world[k].transpose() * fg;
            Eigen::Matrix3d skew;
            skew <<      0, -fg_local.z(),  fg_local.y(),
                   fg_local.z(),       0, -fg_local.x(),
                  -fg_local.y(), fg_local.x(),       0;
            A.block<3,3>(3*k, 0) = -skew;
            b.segment<3>(3*k) = tau_raw[k];
        }
        cal.r_com = A.colPivHouseholderQr().solve(b);
        return cal;
    }

    Eigen::Matrix<double,6,1> compensate(
        const Eigen::Matrix3d& R_current,
        const Eigen::Matrix<double,6,1>& raw_wrench) const
    {
        Eigen::Vector3d fg_local = R_current.transpose() * f_gravity_world;
        Eigen::Vector3d tg_local = r_com.cross(fg_local);
        Eigen::Matrix<double,6,1> compensation;
        compensation << fg_local, tg_local;
        return raw_wrench - compensation;
    }
};

练习

1. ⭐ 滤波器设计：设计一个 50 Hz 截止的二阶 Butterworth 滤波器（采样率 1 kHz）。计算在 50 Hz 处的相位延迟（度）。
2. ⭐⭐ 重力补偿：末端安装 0.8 kg 工具，重心在传感器下方 5 cm。机器人当前姿态使传感器 z 轴与世界 z 轴夹角 30 度。计算未补偿和补偿后的 z 方向力读数差异。
3. ⭐⭐ 噪声影响分析：在 Python 中生成一个 10 Hz 正弦力信号（幅值 20 N），叠加标准差 0.5 N 的高斯白噪声。分别用 30 Hz 和 100 Hz 截止频率滤波，计算两种情况下信号幅值衰减和相位延迟。

---

7. SE(3) 姿态阻抗——几何正确的实现 ⭐⭐⭐

7.1 为什么欧拉角阻抗是错误的

阻抗方程中的姿态误差 \(e_{rot}\) 如何定义？最直觉的做法是用欧拉角（RPY），但这在数学上和物理上都有严重问题。

问题 1：万向锁。当 pitch 角接近 \(\pm 90°\) 时，欧拉角表示退化——两个旋转自由度对应同一个运动方向。在阻抗控制中，这会导致刚度矩阵在某些方向上"消失"。

问题 2：非物理行为。欧拉角空间不是线性空间——两个旋转的"中点"在欧拉角空间中不是简单的平均值。直接对欧拉角做 PD 控制会在大角度偏转时产生非物理的恢复力矩。

反事实推理：如果你在欧拉角空间中做阻抗控制，当末端需要绕 z 轴旋转 180 度时会怎样？欧拉角 PD 可能让末端走"长路"（绕另外两个轴的复杂路径）而非"短路"（直接绕 z 轴旋转），因为欧拉角空间中的"短路"可能经过万向锁奇异。

7.2 旋转矩阵误差——Caccavale 1999 方法

Caccavale et al. 1999 提出了几何正确的姿态误差定义：

\[e_R = \frac{1}{2} \text{vee}(R_d^T R - R^T R_d) \in \mathbb{R}^3\]

其中 \(\text{vee}(\cdot)\) 是反对称矩阵到向量的映射（\(\hat{\omega}^{\vee} = \omega\)）。

对应的李雅普诺夫函数：

\[V_R = \frac{1}{2} \text{tr}(I - R_d^T R) \in [0, 2]\]

这个函数的物理含义：\(V_R = 0\) 当且仅当 \(R = R_d\)（无姿态误差），\(V_R = 2\) 当 \(R\) 和 \(R_d\) 差 180 度。

\[\dot{V}_R = e_R^T e_\Omega\]

其中所有角速度都在同一坐标系表达，常用的 body-frame 定义为 \(e_\Omega=\Omega-R^TR_d\Omega_d\)。若用这个 \(e_R\) 定义，旋转弹簧/阻尼力矩应取

\[\tau_R = -k_R e_R - k_\Omega e_\Omega\]

这样 \(\dot{V}_R=e_R^T e_\Omega\) 与控制力矩符号一致；若改用 \(-e_R\) 作为误差，控制律中的符号也必须同步翻转。

7.3 四元数误差——处理双覆盖

四元数表示旋转时存在双覆盖问题：\(q\) 和 \(-q\) 表示同一个旋转。在阻抗控制中必须处理：

// 四元数姿态误差（libfranka 实现风格）
// 输入: orientation (当前), orientation_d (期望)
// 输出: error_rot (3D 姿态误差向量)

// Step 1: 处理双覆盖——确保取最短路径
if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) {
    orientation.coeffs() << -orientation.coeffs();
}

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

// Step 3: 提取 3D 误差向量
// error_quaternion.vec() = sin(theta/2) * n，小时角约等于 (theta/2) * n
// libfranka 风格直接使用这个“半角尺度”误差；若改用 Log(R) = theta*n，需要重新整定旋转刚度
Eigen::Vector3d error_rot = -(transform.rotation() * error_quaternion.vec());
// 注意负号：使 error_rot 与平移误差一样采用 current-minus-desired 约定；
// 后续 -K*error 才产生指向期望姿态的恢复力矩。

尺度提醒：上面这段 libfranka 风格代码使用的是四元数虚部，尺度约为 \(\theta/2\)；Caccavale 误差 \(e_R = \frac{1}{2}\text{vee}(R_d^T R - R^T R_d)=\sin\theta\,n\) 小角度下约为 \(\theta n\)；精确 Log 误差也是 \(\theta n\)。三者方向一致但尺度不同，替换公式时不能直接沿用原来的旋转刚度。

与 SLAM 的联系：Caccavale 误差 \(e_R\) 是 \(\text{Log}(R_d^T R)^{\vee}\) 的一阶近似。GTSAM 中的 BetweenFactor<Rot3> 使用精确的对数映射作为残差。近似误差为 \(1 - \frac{\sin\theta}{\theta}\)（其中 \(\theta\) 为旋转角度），在 14 度时约 1%，30 度时约 4.5%。力控中通常使用近似形式因为计算更快，但当姿态误差可能超过 15 度时应考虑精确对数映射。

7.4 SE(3) 上的阻抗控制——几何一致的完整理论

在 SE(3)（刚体运动群）上定义阻抗控制需要处理两个数学困难：(1) 旋转空间不是线性空间——不能简单做差；(2) 位置和旋转的"刚度"有不同的量纲——\(K_d\) 矩阵的左上角和右下角单位不同。

SE(3) 位形误差的正确定义：

给定当前位姿 \(T = (R, p) \in SE(3)\) 和期望位姿 \(T_d = (R_d, p_d)\)，位姿误差定义为：

\[T_{err} = T_d^{-1} T = \begin{bmatrix} R_d^T R & R_d^T(p - p_d) \\ 0 & 1 \end{bmatrix}\]

对应的李代数误差（对数映射）：

\[\xi_{err} = \text{Log}(T_{err}) = \begin{bmatrix} \phi_{err} \\ \rho_{err} \end{bmatrix} \in \mathbb{R}^6\]

其中 \(\phi_{err} = \text{Log}(R_d^T R) \in \mathbb{R}^3\) 是轴角误差向量，\(\rho_{err}\) 是调整后的位置误差。

小角度近似（实际力控中通常足够）：

当旋转误差较小时，\(\text{Log}(R_d^T R) \approx \frac{1}{2}\text{vee}(R_d^T R - R^T R_d)\)（即 Caccavale 1999 的公式）。近似相对误差为 \(1 - \frac{\sin\theta}{\theta}\)：14 度时约 1%，30 度时约 4.5%。

当旋转误差 \(> 30°\) 时，必须使用精确的对数映射（Rodrigues 逆公式）：

\[\text{Log}(R_{err}) = \frac{\theta}{2\sin\theta}(R_{err} - R_{err}^T)^{\vee}\]

其中 \(\theta = \arccos\frac{\text{tr}(R_{err}) - 1}{2}\)。

SE(3) 阻抗的势能函数：

在 SE(3) 上定义一个几何一致的势能函数需要特别小心。Stramigioli 2001 提出了基于 SE(3) 测地线距离的势能：

\[V_{SE3} = \frac{1}{2}k_p \|p - p_d\|^2 + k_r \cdot \text{tr}(I - R_d^T R)\]

其中 \(k_p\) 是平移刚度标量，\(k_r\) 是旋转刚度标量。

这个势能的梯度给出了几何正确的恢复力/力矩：

\[F_{spring} = -\nabla_p V = -k_p(p - p_d) \quad \text{(线性弹簧，与欧氏情况相同)}\]

\[\tau_{spring} = -\nabla_R V = -k_r \cdot e_R \quad \text{(旋转弹簧，使用 Caccavale 误差)}\]

本质洞察：位置空间是平坦的（\(\mathbb{R}^3\)），所以位置阻抗与普通弹簧完全一样。旋转空间是弯曲的（\(SO(3)\)），所以旋转阻抗必须使用旋转群上的距离度量。注意 \(\text{tr}(I - R_d^T R) = 2(1 - \cos\theta)\) 不是 \(SO(3)\) 上的测地线距离平方（测地线距离平方为 \(\theta^2 = \|\text{Log}(R_d^T R)\|^2\)），而是一种**弦距度量**（chordal metric），也等于旋转势能函数 \(\psi(R) = \text{tr}(I - R_d^T R)\) 的值。两者在小角度下一致（\(2(1-\cos\theta) \approx \theta^2\)），但在大角度下有差异。弦距度量的优势是计算简单且梯度给出正确的恢复力矩方向。

耦合刚度矩阵——力/力矩的量纲不一致问题：

完整的 \(6 \times 6\) 刚度矩阵 \(K_d = \begin{bmatrix} K_{pp} & K_{p\phi} \\ K_{\phi p} & K_{\phi\phi} \end{bmatrix}\) 中，四个子块的量纲不同：

子块	量纲	物理含义
\(K_{pp}\)	N/m	位移产生力
\(K_{\phi\phi}\)	Nm/rad	旋转产生力矩
\(K_{p\phi}\)	N/rad	旋转产生力（耦合）
\(K_{\phi p}\)	Nm/m	位移产生力矩（耦合）

在大多数工程实现中，\(K_{p\phi} = K_{\phi p}^T = 0\)（无耦合），这避免了量纲混合和数值 conditioning 问题。但某些任务（如斜面上的柔顺操作）确实需要耦合项——此时必须注意数值缩放。

7.5 完整的 6D 阻抗误差向量

6D 误差向量：
  error = | error_pos |   位置误差（3D, 欧氏空间）
          | error_rot |   姿态误差（3D, 李代数近似）

6D 阻抗控制律：
  F_task = -K_d * error - D_d * (J * q_dot)
  tau = J^T * F_task + tau_nullspace + coriolis

关键设计细节：位置误差 \(e_{pos} = x - x_d\) 在哪个坐标系表达？

坐标系	含义	适用场景
世界坐标系（WORLD）	误差在固定坐标系中	简单，但与末端姿态无关
末端坐标系（LOCAL）	误差在末端坐标系中	直觉（"末端感受到的力"）
局部原点世界对齐（LWA）	原点在局部帧处，坐标轴与世界系对齐	Pinocchio 推荐，工程最常用

常见陷阱：Pinocchio 的 getFrameJacobian 有三种模式（LOCAL/WORLD/LOCAL_WORLD_ALIGNED）。LOCAL_WORLD_ALIGNED 的含义是：原点在局部帧（如末端执行器）的位置处，坐标轴与世界坐标系对齐——不是"位置在世界系、姿态在末端系"。这意味着 Jacobian 表达的是在该帧位置处、用世界系坐标轴描述的速度。阻抗控制中，Jacobian 和误差必须在**同一个参考系**下计算。如果 Jacobian 用 LOCAL_WORLD_ALIGNED 但误差在 WORLD 系表达（原点在世界原点），当帧的位置不在世界原点时会产生旋转-平移耦合误差——这是一个非常常见且难以调试的 bug。

libfranka 中的坐标系约定：

libfranka 的 cartesian_impedance_control.cpp 使用以下约定： - Jacobian：LOCAL_WORLD_ALIGNED（末端点、世界轴） - 位置误差：世界坐标系中 \((p - p_d)\) - 姿态误差：世界坐标系中 \(e_R = -(R \cdot q_{err}.vec())\)

关键点：-(R * error_quaternion.vec()) 中的负号使姿态误差与位置误差一样采用"当前减期望"的恢复力约定，后续控制律写成 \(F=-K e-D\dot{x}\)；前面的 \(R\) 矩阵将误差从末端系旋转到世界系——与 LOCAL_WORLD_ALIGNED Jacobian 一致。

⚠️ 编程陷阱：如果你在实现阻抗控制时忘记了这个负号，末端姿态会**向错误方向**运动——不是趋向期望姿态，而是远离期望姿态。这是一个**正反馈**，会导致系统立即不稳定。排查方法：让末端在自由空间中偏离期望姿态 10 度，观察是否收敛。如果发散，检查误差向量的符号。

练习

1. ⭐⭐ 双覆盖实验：构造两个四元数 \(q_1 = [0, 0, 0, 1]\)（无旋转）和 \(q_2 = [0, 0, \sin(85°), \cos(85°)]\)（绕 z 轴 170 度）。不处理双覆盖时，四元数误差是多少？处理后呢？哪个是"短路"？
2. ⭐⭐⭐ 坐标系验证：在 Pinocchio 中分别用 LOCAL 和 LOCAL_WORLD_ALIGNED 模式计算 Franka 的 Jacobian。在末端有 30 度旋转偏差的构型下，对比两种 Jacobian 在阻抗控制中的效果差异。

---

8. 接触过渡动力学——为什么接触瞬间最危险 ⭐⭐

8.1 牛顿冲击模型

当机器人末端以速度 \(\dot{x}_n^-\) 接触环境时，会发生碰撞冲击：

\[\dot{x}_n^+ = -e \cdot \dot{x}_n^-\]

其中 \(e \in [0, 1]\) 是恢复系数（\(e = 0\) 完全非弹性，\(e = 1\) 完全弹性）。

冲击力冲量：

\[\Delta p = m_{eff}(1 + e)|\dot{x}_n^-|\]

其中有效质量 \(m_{eff} = 1/(n^T \Lambda^{-1} n)\)（\(n\) 是接触法向，\(\Lambda\) 是操作空间惯量）。

工程含义：

接近速度	有效质量	恢复系数	冲击力峰值（1ms 持续）
10 mm/s	2 kg	0.5	~30 N
50 mm/s	2 kg	0.5	~150 N
100 mm/s	2 kg	0.5	~300 N

即使 50 mm/s 的低速接触也可能产生超过 100N 的瞬态力——这足以损坏精密工件或触发安全停机。

8.2 接触过渡策略

接触过渡是力控中最危险的阶段。推荐的四阶段策略：

Phase 1: 接近（Free space approach）
  速度限制: v_approach <= 20 mm/s
  刚度:     K_d 中等（500 N/m）
  目的:     缓慢接近，减小冲击

Phase 2: 接触检测（Contact detection）
  方法:     力阈值 |f| > f_threshold（2-5 N）
  或者:     动量观测器残差 > 阈值
  延迟:     < 10 ms（关键！延迟越大冲击越大）

Phase 3: 冲击吸收（Impact absorption）
  切换到:   低刚度 K_d = 50-100 N/m
  高阻尼:   zeta = 1.5（过阻尼）
  持续:     50-200 ms

Phase 4: 稳定接触（Steady contact）
  恢复到:   任务所需的 K_d（100-2000 N/m）
  正常阻尼: zeta = 0.8-1.0
  开始:     力跟踪/位置跟踪混合

参数平滑过渡——避免阶跃切换：

四阶段之间的参数切换不应是阶跃的（\(K_d\) 从 500 瞬间跳到 50），而应使用平滑过渡函数。推荐使用 S 型曲线（sigmoid）或五次多项式插值：

\[K_d(t) = K_{start} + (K_{end} - K_{start}) \cdot s\Big(\frac{t - t_0}{T_{trans}}\Big)\]

其中 \(s(\tau) = 6\tau^5 - 15\tau^4 + 10\tau^3\)（五次多项式，满足 \(s(0) = 0, s(1) = 1, \dot{s}(0) = \dot{s}(1) = 0\)）。

过渡时间 \(T_{trans}\) 的选择：

过渡	推荐 \(T_{trans}\)	理由
Phase 1 → 2	瞬时（由检测触发）	检测到接触必须立即响应
Phase 2 → 3	10-50 ms	快速降低刚度吸收冲击
Phase 3 → 4	100-500 ms	缓慢恢复刚度避免二次冲击

反事实推理：如果 Phase 2→3 使用阶跃切换（\(K_d\) 从 500 瞬间跳到 50）会怎样？\(K_d\) 的阶跃变化导致弹性势能突变 \(\Delta V = \frac{1}{2}(\Delta K_d)\tilde{x}^2\)——这个能量会被释放为瞬间运动，可能使末端弹离接触面。五次多项式平滑过渡确保 \(\dot{K}_d\) 在端点为零，消除能量的突变释放。

有效质量的构型依赖性：

冲击力与有效质量 \(m_{eff}\) 成正比，而 \(m_{eff}\) 是构型相关的：

\[m_{eff}(q, n) = \frac{1}{n^T \Lambda^{-1}(q) n} = \frac{1}{n^T J(q) M^{-1}(q) J^T(q) n}\]

其中 \(n\) 是碰撞方向单位向量。

对 Franka Panda，\(m_{eff}\) 的典型范围： - 沿手臂伸展方向（最小惯量）：\(m_{eff} \approx 1.5\)-\(2\) kg - 沿手臂法向（最大惯量）：\(m_{eff} \approx 4\)-\(8\) kg - 收缩构型：\(m_{eff}\) 全方向较大

工程启示：在接触前应选择 \(m_{eff}\) 最小的构型方向——通常是沿手臂伸展方向接近。这与直觉一致——你会用指尖（低惯量）而非拳头（高惯量）去触碰易碎物品。

8.3 Coulomb 摩擦锥

在接触面上，切向力受摩擦锥约束：

\[\|f_t\| \leq \mu f_n\]

其中 \(\mu\) 是摩擦系数，\(f_n\) 是法向力（必须为正——接触不能"拉"）。

摩擦锥线性化（WBC 中使用）：

将圆锥 \(\|f_t\| \leq \mu f_n\) 近似为 4 或 8 边金字塔：

\[A_{fc} \cdot f_c \leq 0\]

4 边金字塔约束：

\[\pm f_x \leq \mu f_z, \quad \pm f_y \leq \mu f_z, \quad f_z \geq f_{min}\]

回顾 Ch53（WBC）：摩擦锥约束在足式 WBC 中是核心约束——每个足端力必须在摩擦锥内，否则会打滑。在机械臂力控中，摩擦锥约束出现在打磨/擦拭任务中——切向力不能超过法向力乘以摩擦系数，否则工具会在工件表面滑动。同一个数学约束，不同的物理场景。

练习

1. ⭐ 冲击力计算：Franka Panda（末端有效质量约 2.5 kg）以 30 mm/s 接触钢铁表面（\(e = 0.3\)）。计算冲击力冲量和瞬时峰值力（假设碰撞持续 2 ms）。
2. ⭐⭐ 阻抗切换实验：在 MuJoCo 中让 Franka 以不同 \(K_d\)（100/500/2000 N/m）和不同初速（10/50/100 mm/s）接触刚性桌面。记录接触力峰值。绘制 \(F_{peak}\) vs \(K_d \cdot v\) 的关系。
3. ⭐⭐⭐ 跨章综合：结合 F01（约束空间分析）和本节的接触模型，为 peg-in-hole 装配任务设计完整的接触过渡策略。明确每个阶段的阻抗参数和切换条件。这道题需要综合 F01 和 F02 的知识。

---

9. 阻抗参数的物理约束总结 ⭐⭐

9.1 约束完整清单

约束	数学条件	物理原因	违反后果
\(K_d\) 对称半正定/正定	\(K_d = K_d^T, \lambda_{min}(K_d) \geq 0\)；需要回位的轴 \(>0\)	势能 \(V \geq 0\)	负特征值方向不稳定
\(D_d\) 对称正定	\(D_d = D_d^T, \lambda_{min}(D_d) > 0\)	耗散 \(P_{diss} \geq 0\)	无源性破坏
\(M_d\) 对称正定	\(M_d = M_d^T, \lambda_{min}(M_d) > 0\)	动能 \(T \geq 0\)	非物理行为
阻尼比合理	\(\zeta_i \in [0.7, 1.5]\)	避免振荡/迟钝	接触弹跳或慢响应
离散稳定性	\(K_d < f(D_d, T, B, D_e)\)	采样间隔能量注入	高频振荡
柔性关节上界	\(K_{d,eq} \leq K_s\)	不能超过物理弹簧	无法实现期望阻抗

9.2 柔性关节的刚度上界——Ott 2008

Franka/iiwa 的谐波减速器 + 力矩传感器构成 SEA，等效关节弹簧刚度 \(K_s\)。Ott 2008 证明：

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

即**用户设定的刚度永远无法超过物理弹簧刚度**。Franka 约 \(K_s \approx 3000\) Nm/rad，对应笛卡尔刚度上限约 3000-5000 N/m。

约束检查工具——Python 验证脚本框架：

设计阻抗参数后，应使用以下检查流程验证所有约束：

# 阻抗参数约束检查（设计后必须运行）
def check_impedance_constraints(K_d, D_d, M_d, K_s, T_s, K_e_range):
    import numpy as np
    # 1. 正定性
    eigK = np.linalg.eigvalsh(K_d)
    eigD = np.linalg.eigvalsh(D_d)
    eigM = np.linalg.eigvalsh(M_d)
    assert np.all(eigK > 0), f"K_d not PD: eigenvalues={eigK}"
    assert np.all(eigD > 0), f"D_d not PD: eigenvalues={eigD}"
    assert np.all(eigM > 0), f"M_d not PD: eigenvalues={eigM}"
    # 2. 阻尼比检查
    omega_n = np.sqrt(np.diag(K_d) / np.diag(M_d))
    zeta = np.diag(D_d) / (2 * np.sqrt(np.diag(K_d) * np.diag(M_d)))
    print(f"Natural freq: {omega_n/(2*np.pi)} Hz, Damping: {zeta}")
    assert np.all(zeta >= 0.7), f"Underdamped: zeta={zeta}"
    # 3. 柔性关节上界
    assert np.all(np.diag(K_d) < K_s), f"K_d exceeds K_s={K_s}"
    # 4. 离散稳定性 (Colgate condition for worst-case K_e)
    K_e_max = max(K_e_range)
    # ... Colgate-Schenkel check
    print("All constraints PASSED")

练习

1. ⭐⭐ 柔性关节影响：Franka 的 \(K_s = 3000\) Nm/rad。如果你设定关节刚度 \(K_\theta = 5000\) Nm/rad，实际等效刚度是多少？如果设 \(K_\theta = 10000\) 呢？画出 \(K_{d,eq}\) vs \(K_\theta\) 的曲线（\(K_s = 3000\) 固定），观察饱和行为。
2. ⭐⭐ 综合设计：为 Franka Panda 擦桌子任务设计完整参数集 \((K_d, D_d, M_d)\)。要求：z 向刚度 100 N/m、xy 向 1000 N/m、临界阻尼。列出所有值并检查约束。
3. ⭐⭐⭐ 约束冲突分析：一个任务要求力控带宽 > 50 Hz、z 向刚度 200 N/m、采样率 1 kHz、Franka Panda 硬件。说明这些要求是否可以同时满足。如果不能，分析哪些约束冲突，并提出折中方案。

---

本章小结

知识点	核心内容	难度	后续章节
刚度/阻尼/惯量矩阵	对称正定性、物理意义、设计方法	⭐	F03 算法
频域分析	Bode 图、带宽、环境耦合	⭐⭐	F04 工程
无源性理论	存储函数、耗散不等式、Colgate-Hogan 定理	⭐⭐⭐	F06 变阻抗
操作空间动力学	\(\Lambda, \mu, p\) 完整推导	⭐⭐	F03 阻抗推导
动力学一致伪逆	\(\bar{J}\) vs \(J^+\) 的物理区别	⭐⭐	F03, F07 WBC
零空间投影	\(N = I - \bar{J}J\)、多优先级	⭐⭐	F07 WBC
力椭球/速度椭球	对偶关系、构型选择	⭐⭐	F04 工程
传感器噪声模型	滤波器设计、重力补偿	⭐⭐	F04, F05 工程
SE(3) 姿态阻抗	旋转矩阵误差、四元数双覆盖	⭐⭐⭐	F04 工程
接触过渡动力学	冲击模型、四阶段过渡策略	⭐⭐	F03, F04
参数约束	正定性、离散稳定性、柔性关节上界	⭐⭐	F03, F04

---

累积项目：Mini-ForceControl 模块 1——操作空间动力学

本章新增模块：OperationalSpaceDynamics 类

class OperationalSpaceDynamics {
public:
    explicit OperationalSpaceDynamics(const pinocchio::Model& model);

    void compute(const pinocchio::Data& data,
                 const Eigen::VectorXd& q,
                 const Eigen::VectorXd& v,
                 pinocchio::FrameIndex ee_frame);

    const Eigen::Matrix<double,6,6>& Lambda() const;
    const Eigen::MatrixXd& Jbar() const;
    const Eigen::MatrixXd& N() const;
    const Eigen::Matrix<double,6,1>& mu() const;
    const Eigen::Matrix<double,6,1>& p() const;
    double conditionNumber() const;
    double minSingularValue() const;
};

---

延伸阅读

资料	类型	难度	推荐理由
Khatib 1987, "A Unified Approach for Motion and Force Control", IEEE J. RA 3(1)	论文	⭐⭐	操作空间动力学原始论文
Featherstone 2008, "Rigid Body Dynamics Algorithms" Ch.9	教材	⭐⭐⭐	操作空间动力学的严格推导
Dietrich et al. 2015, "Overview of Null Space Projections", IJRR 34(11)	论文	⭐⭐⭐	四类零空间投影的系统对比
Colgate & Hogan 1988, "Robust Control of Dynamically Interacting Systems"	论文	⭐⭐⭐	耦合稳定性定理原始证明
Ott 2008, "Cartesian Impedance Control of Redundant and Flexible-Joint Robots"	专著	⭐⭐⭐⭐	柔性关节阻抗完整理论
Siciliano et al., "Robotics" Ch.8-9	教材	⭐⭐	力控数学工具标准参考

---

故障排查手册

症状	可能原因	排查步骤	相关章节
\(\Lambda\) 计算结果含 NaN/Inf	奇异位形	1. 检查 \(\sigma_{min}(J)\) 2. 加 DLS 正则化 3. 远离奇异构型	F02 4.6
零空间力矩干扰末端	用了 \(J^+\) 而非 \(\bar{J}\)	1. 验证 \(JN \approx 0\) 2. 切换到 \(\bar{J}\)	F02 4.3
力传感器补偿后仍有偏差	重力参数不准 / 温度漂移	1. 重新三姿态标定 2. 检查温度 3. 缩短标定间隔	F02 6.3
阻抗刚度高于预期	柔性关节饱和	1. 检查 \(K_d\) vs \(K_s\) 2. 降低 \(K_d\)	F02 7.2
频率响应与设计不符	滤波器/计算延迟	1. 测量实际延迟 2. 检查滤波截止频率	F02 2.4
姿态阻抗控制发散	误差向量符号错误	1. 检查 \(e_R\) 的符号定义 2. 在自由空间测试姿态收敛方向 3. 检查四元数双覆盖处理	F02 7.3
Bode 图共振频率偏低	\(M_d\) 过大或 \(K_d\) 过小	1. 打印实际 \(\Lambda_{ii}\) 值 2. 检查参数单位（N/m vs N/mm） 3. 扫频实验验证	F02 2.2
变阻抗时系统振荡	刚度变化注入能量	1. 检查 \(\dot{K}_d\) 是否满足 KB 条件 2. 加能量罐监控 3. 同步更新 \(D_d\)	F02 3.5
接触瞬间力尖峰过大	接近速度过快 / \(m_{eff}\) 过大	1. 降低接近速度至 10 mm/s 2. 选择低 \(m_{eff}\) 构型 3. 加接触过渡策略	F02 8.1
F/T 滤波器引入力跟踪延迟	截止频率过低	1. 测量滤波相位延迟 2. 提高截止频率（代价是更多噪声） 3. 用 phase-compensated 滤波器	F02 6.2

高频问题的系统化诊断流程：

当力控系统出现高频振荡（\(>\) 50 Hz 的嗡嗡声或颤振）时，按以下流程排查：

Step 1: 断开控制器，机器人是否仍振荡？
  |-- 是 -> 机械问题（轴承/减速器/共振），非控制问题
  +-- 否 -> 控制器引起，继续

Step 2: 将 K_d 降低到 50 N/m（极低），是否仍振荡？
  |-- 是 -> 不是 K_d 过高的问题，检查:
  |         - 力传感器噪声是否被放大（降低滤波截止频率）
  |         - 计算延迟是否过大（检查控制周期抖动）
  |         - Jacobian 参考系是否与误差不一致
  +-- 否 -> K_d 过高导致离散无源性破坏
            -> 按 Colgate 条件计算最大允许 K_d
            -> K_d_max = D_d / T * 2 / (1 + D_d/(B+D_e))

Step 3: 振荡频率是否恰好等于控制频率的某个分频？
  |-- 是 -> 混叠问题（力信号频率 > Nyquist 频率）
  |         -> 加抗混叠滤波器或提高采样率
  +-- 否 -> 检查关节柔性反共振频率
            -> Franka 典型反共振: 50-150 Hz
            -> 增大阻尼或加陷波滤波器