机器人阻抗控制：从弹簧阻尼模型到笛卡尔力矩控制

阻抗控制不直接要求机器人严格跟踪某条位置轨迹，而是规定“运动误差与交互力之间应该呈现怎样的动态关系”。它让机器人表现得像一个可调的质量—弹簧—阻尼系统，常用于装配、打磨、拖动示教、人机协作和接触任务。

阻抗控制中的 $M(q)$、$g(q)$、payload 和摩擦模型从哪里来，以及如何通过数据得到并验证，可参考机器人动力学参数辨识专题。本文只讨论这些模型量进入阻抗控制后的使用方式。

安全提示：阻抗控制通常最终输出关节力矩。错误的雅可比、坐标系、动力学模型、刚度或力矩限幅都可能产生危险运动。真机调试必须从低刚度、低速度和保守力矩限制开始，并具备急停、watchdog、碰撞检测和回退控制模式。

这是一篇从原理延伸到工程落地的长文，可以按目标选择阅读路径：

页面会根据屏幕宽度自动缩放正文、表格和图片；对于标注较密的技术图，还可以点击进入站点统一的放大视图查看细节。

1. 阻抗控制到底控制什么

理想笛卡尔阻抗关系为：

其中 $\delta x=x-x_d$ 是相对目标的位移，$M_d$、$D_d$、$K_d$ 分别是期望惯性、阻尼和刚度，$F_{ext}$ 是外部作用力。直观上：

• $K_d$ 决定机器人偏离目标位置时“推回去”的力度；
• $D_d$ 抑制振荡并决定响应快慢；
• $M_d$ 决定机器人表现出的惯性感；
• 外力越大，允许产生的位移或动态响应越明显。

这里 $F_{ext}$ 表示环境施加在机器人上的外力，$\delta x$ 表示机器人相对目标的位移。控制器内部后文采用回正误差 $e=x_d-x=-\delta x$。区分这两个量可以避免把“外力导致的位移方向”和“弹簧控制力方向”混为一谈。

在静态且速度为零时：

因此一维刚度为 $1000\,\mathrm{N/m}$ 时，$10\,\mathrm{N}$ 的恒定外力对应约 $10\,\mathrm{mm}$ 的静态位移。这个关系适合建立直觉，但真实机器人还受到重力补偿、摩擦、限幅和环境刚度影响。

1.1 本文统一使用的符号

六维量同时包含平移和旋转分量，因此不能只写“单位为 N”或直接比较平移、旋转增益的数值。本文默认下标 $d$ 表示 desired，$ext$ 表示环境施加在机器人上的 external wrench。

1.2 六维量不能直接求欧氏范数

将位置误差写成六维向量，不意味着可以直接计算 $\|e\|_2$：前三维是 m，后三维是 rad；wrench 的前三维是 N，后三维是 N·m。若确实需要单一指标，可以先选择具有任务意义的特征长度 $\ell$，定义：

这样 $e_s$ 的所有分量具有长度尺度，$F_s$ 的所有分量具有力尺度，而且功率保持不变：

$\ell$ 不是数学常数，应根据工具尺寸、任务允许的转角对应多大末端位移来选择并写入配置。例如长工具与短工具通常不应使用同一个特征长度。另一种更直观的方案是不合并六维量，分别报告平移、旋转、力和力矩的逐轴最大值。

同理，直接对未缩放的六维 Jacobian 求条件数，会让结果依赖所用的长度单位。若用 $S=\operatorname{diag}(I_3,\ell I_3)$ 将 twist 归一化，应对 $J_s=SJ$ 监控奇异值。机器人同时包含转动和移动关节时，列空间也具有混合单位，还需定义关节尺度矩阵。所有报警阈值必须和同一组尺度绑定；否则把模型从 m 改成 mm 就可能改变“接近奇异”的判断。

2. 阻抗控制与导纳控制

两者描述的目标机械行为可以相似，但实现接口不同。位置控制器外面套一个“虚拟质量—弹簧—阻尼”并输出位置偏移，通常属于导纳控制；直接计算任务空间 wrench 并映射为关节力矩，通常属于阻抗控制。

3. 关节空间阻抗控制

最基本的关节阻抗形式是：

$\tau_{ff}$ 可以包含重力、科氏/离心力和期望加速度前馈：

关节阻抗实现简单、计算量低，也容易逐关节限幅；但关节刚度并不直接等于末端在 $x/y/z$ 或旋转方向上的刚度。机器人姿态变化时，末端呈现的刚度也会变化。

在固定构型附近进行小位移线性化，有：

若希望笛卡尔虚拟弹簧储能为 $V=\frac12\delta x^TK_x\delta x$，代入上式可以得到局部等效关节刚度：

当机器人恰好是非冗余、$J$ 为方阵且可逆时，可以形式化地反推：

这个反推不能直接用于冗余机器人或奇异位形。对正定关节刚度 $K_q$，可以改从静态柔顺度推导。小外力 $F$ 引起的关节位移近似满足：

因此：

末端柔顺度与刚度为：

最后一个逆只在任务方向满秩时成立；接近奇异位形时，某些方向柔顺度会急剧增大，不能靠普通求逆得到可靠刚度。实现时应使用矩阵分解并监控秩与条件数。

不同的零空间刚度可能对应相同或相近的末端局部行为，但关节限位裕度和内部运动会不同。即使在可逆构型，$J(q)$ 也随姿态变化，相同 $K_q$ 会产生不同 $K_x$。

在相同的小速度、零外载近似下，阻尼也可以按能量耗散关系局部映射：

但 $J$ 随时间变化时还存在 $\dot J\dot q$、动力学耦合和滤波延迟，因此不能仅用这个式子保证动态阻尼比。

上述关系还是零外载附近的一阶近似。存在较大末端 wrench 时，$\tau=J^TF$ 对 $q$ 求导还会出现由 $\partial J^T/\partial q$ 引起的几何刚度项；结构柔性、减速器和内层伺服也会改变实测刚度。因此工程上应通过逐方向加载实验验证 $F/\delta x$，不能只由 Jacobian 换算后就宣称得到目标笛卡尔刚度。

3.1 根据接口和任务选择实现形式

“阻抗控制器”并不是唯一固定结构。开始编码前，应先根据底层接口和任务目标选择层级：

如果机器人只暴露高刚度位置接口，就不能假装向接口发送的“力矩字段”会形成真正的力矩闭环；此时通常选择导纳外环。若机器人提供可靠的关节力矩或电流接口，也仍要确认驱动器内部是否包含重力、摩擦、速度环、碰撞限幅或额外滤波，因为这些隐藏内环会改变实际阻抗。

第一次实现建议先建立关节空间低刚度基线，验证力矩方向、重力补偿和安全状态，再增加笛卡尔映射。只有任务确实需要一致的动态响应时，才逐步加入 $\Lambda$、$\dot J\dot q$ 和完整操作空间补偿；复杂度应与可验证收益对应。

4. 笛卡尔空间阻抗控制

下面的闭环图同时画出了阻抗环、操作空间映射、机器人动力学、环境接触和状态反馈。橙色模块表示输出限制与外部环境，蓝绿色模块表示控制和动力学计算：

4.1 位姿误差

平移误差可以直接写为：

旋转误差不能直接对欧拉角相减。常见做法是使用四元数或 $SO(3)$ 对数映射。若使用四元数，需要先保证当前与目标四元数符号位于同一半球，再构造最短旋转：

然后将轴角向量转换到控制器采用的参考坐标系。最终六维误差为：

也可以使用旋转矩阵的对数映射：

必须明确 $e$、速度和 wrench 都表达在基坐标系、末端坐标系还是混合坐标系中；坐标系不一致是阻抗控制方向错误的常见原因。

本文默认六维 motion 向量和 wrench 的排列为：

其中 $f$ 是力，$\mu$ 是力矩。部分动力学库使用 [angular, linear] 顺序；必须同时调整 Jacobian 行、误差向量、刚度矩阵、速度和 wrench，不能只交换显示顺序。还要确认库的 frame Jacobian 是 LOCAL、WORLD 还是 LOCAL_WORLD_ALIGNED 定义。

把坐标系写成控制器接口契约

“全部使用同一个坐标系”仍然不够具体。建议在控制器接口或设计文档中逐项冻结下面的契约，并在启动日志中打印实际选择：

这里的“工具控制点”不一定等于 URDF 末端 frame 或力传感器原点。假设控制点相对传感器原点有位移 $r$，在不改变表达方向的简化情形下，控制点处的等效力矩包含力臂项：

$r$ 的方向取决于变换定义；实现时应通过完整 wrench 变换得到，不能看到该式后凭经验选择正负号。

提交真机前至少做三个不依赖复杂轨迹的方向测试：

1. 单位关节速度测试：逐关节令 $\dot q_j>0$，比较 $J\dot q$ 与 FK 有限差分得到的末端速度，确认行顺序、方向和控制点一致。
2. 虚功测试：随机取小量 $\delta q$ 和 wrench $F$，用两次 FK 得到实际微小位移 $\delta x_{FK}$，检查 $F^T\delta x_{FK}\approx(J^TF)^T\delta q$；再跨 frame 检查 $F^Tv$ 在变换前后不变。若等式两边都直接复用同一个 $J$，只能验证代数恒等式，发现不了错误的 Jacobian 定义。
3. 单位 wrench 测试：在仿真或悬空低限幅状态下，依次施加六个很小的正向 wrench，确认 $J^TF$ 产生的末端运动趋势与标注轴一致；每一轴单独测试，出现反向立即停止。

第一项验证运动映射，第二项验证 motion/wrench 的对偶关系，第三项验证从控制输出回到真实机构的完整符号链。三项都通过，才说明“公式一致”真正延伸到了库接口和机器人接线。

4.2 虚拟弹簧—阻尼 wrench

最常见的目标 wrench 为：

积分项不是阻抗控制的必需组成。它可以减小摩擦或模型偏差导致的稳态误差，但会在接触、饱和和目标突变时积累，因此必须限幅、复位并采用 anti-windup。

若 $M_d,K_d$ 取对角形式，并希望各轴阻尼比为 $\zeta$，可以使用：

等价地，可先选择期望自然频率 $\omega_n=2\pi f_n$：

$\zeta=1$ 对应临界阻尼的常用起点，但离散采样、模型误差、通信延迟和环境刚度都会改变实际稳定裕度。

这里的误差和外力符号必须与实现保持一致。本文采用 $e=x_d-x$ 生成回正 wrench，同时用 $\delta x=x-x_d$ 描述外力产生的物理位移。环境施加在机器人上的力与机器人施加在环境上的力方向也相反；改变约定时，弹簧、阻尼和外力方程必须整体一致，不能只修改其中一个负号。

4.3 操作空间惯量与力矩映射

先从关节动力学开始：

$h$ 汇总科氏、离心和重力项。忽略零空间并假设任务满秩，可将其写成操作空间形式：

机器人质量矩阵为 $M(q)$，末端雅可比为 $J(q)$，操作空间惯量为：

最直接的逆动力学阻抗令：

代回操作空间动力学后得到：

这说明闭环的表观惯量自然是随构型变化的 $\Lambda(q)$，而刚度和阻尼由控制器指定。对应关节力矩为：

若只关心较慢的准静态接触，常省略完整的惯量、$\mu$ 和 $p$ 整形，仅用 $J^T(K_de+D_d\dot e)$ 加上接口所需的动力学补偿。

若希望指定与 $\Lambda$ 不同的常量虚拟惯量 $M_d$，一种常见中间步骤是先计算期望任务加速度：

考虑 $\dot J\dot q$ 后，任务空间 wrench 可以写为：

然后映射为任务力矩：

但必须注意：仅使用上述加速度映射而不反馈 $F_{ext}$，通常不能对任意 $M_d$ 精确实现开头定义的 $M_d\delta\ddot x+D_d\delta\dot x+K_d\delta x=F_{ext}$；外力通道仍受到真实 $\Lambda$ 影响。精确惯量整形需要一致推导外力反馈项，并依赖可靠的外力测量或估计。许多工程控制器因此接受自然表观惯量，重点整形 $K_d,D_d$，或只在低频范围内近似目标 $M_d$。

接近奇异位形时，$JM^{-1}J^T$ 可能病态。工程实现需要稳定伪逆、阻尼正则化、奇异值监控和速度/力矩降级策略，不能直接对矩阵求逆。

两种常见实现为什么公式不同

较简单的实现直接将弹簧—阻尼输出视为期望 wrench：

这种形式直观、计算量较低，常用于已具备重力补偿的力矩接口。另一种形式先把阻抗输出解释为虚拟系统中的“驱动力”，通过 $M_d^{-1}$ 得到期望加速度，再使用 $\Lambda$ 进行操作空间惯量整形：

两种形式中的 $K,D$ 数值含义不完全相同，不能直接复制参数。阅读或移植控制器时，应先确认中间量究竟表示 wrench、加速度还是已经经过操作空间惯量整形的 wrench。

4.4 各向异性刚度与任务坐标系

实际接触任务通常不希望六个方向使用相同刚度。例如插孔时，可以沿插入轴设置较低刚度以顺应接触，横向设置适中刚度用于对准，绕插入轴设置较低旋转刚度避免卡死：

这里的 $K_T$ 通常定义在工具或任务坐标系 $T$ 中。若控制误差和 wrench 在基坐标系 $B$ 中表达，需要使用伴随变换将刚度、阻尼和误差一致地转换，不能仅旋转平移的三个对角元素。对只考虑同一点纯旋转的简化情形，可写成：

当坐标原点也发生平移时，应使用完整的 wrench/twist 伴随变换。否则力与力矩之间的耦合会被遗漏。

设 ${}^BT_T=(R,p)$ 表示任务坐标系 $T$ 在基坐标系 $B$ 中的位姿。按本文 [linear, angular] 排列，twist 的伴随矩阵为：

于是 twist 与 wrench 分别按互为对偶的方式变换：

这里的逆转置保证功率不随坐标表示改变：

对小位移误差，若 ${}^B\delta x=A{}^T\delta x$ 且 ${}^TF=K_T{}^T\delta x$，则刚度矩阵应变换为：

阻尼矩阵使用同样的双边变换。若控制点相对传感器原点有偏移 $p$，纯力还会产生 $p\times f$ 力矩；这就是只旋转前三维力向量会出错的直接原因。不同库对 ${}^BT_T$ 的方向、空间向量排列和 LOCAL/WORLD 定义可能不同，使用公式前必须先核对约定。

混合位置/力或选择性阻抗控制还可以使用选择矩阵 $S$：

$S$ 选择运动/阻抗控制方向，$I-S$ 选择力控制方向。切换方向或接触状态时必须平滑更新参考值和积分状态，避免 wrench 跳变。

接触法向应该给位置目标还是力目标

这取决于任务真正要求什么。纯阻抗控制给定的是平衡位置和刚度，它只会间接产生接触力。以任务坐标系 $T$ 的法向 $z$ 为例，忽略摩擦和动态项后：

如果工件表面位置 $z_s$ 已知且足够准确，可以由期望接触力反推平衡点：

但只要表面位置、环境刚度或工具长度发生变化，实际力就会偏离 $f_{z,d}$。因此“设置一个位置偏移”不是严格的力控制，也不能用力矩限幅代替接触力上限。需要稳定保持指定法向力时，应测量或可靠估计 $f_z$，在法向使用力反馈；其余方向仍可使用阻抗：

这里假设六维顺序是 [x,y,z,rx,ry,rz]，因此第三维由力环负责。一个简单法向力环可写成：

该式只展示结构。符号取决于本文前面冻结的“环境作用于机器人”约定；真机前必须用很小的正向命令验证。力环增益也不能照搬位置阻抗增益，两者单位和闭环对象都不同。

一次典型的“接近表面并保持法向力”流程为：

从阻抗切到力反馈时，应让新力环的初始输出等于切换前的法向 wrench，或者先令 $f_{z,d}=\hat f_z$ 再缓慢改变目标。否则即使两个控制器各自稳定，切换瞬间也会产生阶跃。退出时先卸载再撤离，能够避免积分残留把机器人重新压向工件。

期望作业力、允许峰值力和触发急停的力必须是三个独立参数。例如 $20\,\mathrm N$ 的作业目标不意味着 $20\,\mathrm N$ 就应触发故障；同时也不能只依靠 $\tau_{max}$ 推断末端安全力，因为 $J(q)$ 会使同一关节力矩限制在不同构型对应不同 wrench 能力。

4.5 六维增益矩阵必须满足什么条件

对角矩阵容易整定，但完整的 $6\times6$ 矩阵可以表达轴间耦合。例如沿倾斜表面法向的柔顺，应先在任务坐标系中定义主方向增益，再整体变换到控制坐标系，而不是分别猜测基坐标系的 $x/y/z$ 参数。

为了让无外力的理想系统具有正的储能和耗散，通常要求：

对应的候选储能函数为：

若矩阵存在明显非对称部分、负特征值或非常大的条件数，控制器可能在某些方向主动注入能量，或因数值误差产生异常 wrench。加载配置时应先对称化候选矩阵并检查特征值；但不能把严重错误的矩阵“修正后继续运行”，而应拒绝该配置并报告来源。

当 $M_d$ 和 $K_d$ 不是同时对角化的简单情形时，逐元素使用 $D_d=2\zeta\sqrt{M_dK_d}$ 不再严谨，因为 $M_dK_d$ 一般不对称。可以先通过广义特征值问题

得到模态方向与自然频率，再在模态坐标中指定阻尼比并变换回来。工程上若没有明确的耦合需求，优先在物理含义清楚的任务坐标系使用对角参数，通常更容易验证。

在线改变完整增益矩阵时，每个时刻仍需保持上述半正定/正定条件。两个有效矩阵的线性凸组合仍保持半正定，但突然改变刚度会改变存储能量；因此即使插值后的矩阵数学有效，也仍应使用变化率限制或能量约束。

5. 冗余机器人的零空间控制

当关节自由度多于任务维度时，可以在不明显影响末端任务的方向上调节关节姿态。动态一致广义逆为：

力矩空间零空间投影可写为：

零空间力矩为：

总目标力矩：

$\tau_{model}$ 是否需要单独包含重力/科氏补偿，取决于任务 wrench 推导和底层接口约定。最危险的错误之一是同一动力学项被补偿两次，或完全遗漏。

零空间目标不一定只是固定关节姿态，也可以来自一个代价函数 $V(q)$：

常见代价包括偏离中间关节位置、接近软限位以及可操作度降低。例如一个简单的归一化关节居中代价为：

靠近机械限位时可以使用增长更快的 barrier，但梯度必须限幅并在软限位内提前生效。零空间投影只表示理想模型下不影响主任务；模型误差、数值伪逆、离散延迟和饱和仍会造成耦合。因此零空间力矩、变化率和势函数梯度都需要独立限制，并在主任务误差或奇异性增大时降低权重。

5.1 如何验证零空间投影

对满行秩任务和未正则化的动态一致逆，理想情况下应满足：

第二个关系比直接检查 $JN^T$ 更重要，因为 $N^T$ 投影的是关节力矩，而力矩通过 $M^{-1}$ 才形成关节加速度。可以定义一个无量纲投影残差：

还可以随机生成投影前力矩 $\tau_0$，令 $\tau_{null}=N^T\tau_0$，直接比较投影前后的任务加速度泄漏：

若随机样本的分母接近零，说明 $\tau_0$ 本身几乎不激励任务方向，应丢弃该样本而不是把 $r_{leak}$ 当作有效结果。测试应覆盖大量安全构型、不同任务维度以及接近奇异但仍允许运行的构型，而不是只在一个中间姿态测试。$M$、$J$、$N$ 必须来自同一个 $q$、同一模型版本和同一任务 frame。

使用阻尼伪逆时，$JJ^{\#}$ 不再严格等于 $I$，零空间也会产生可预期的任务泄漏；阻尼越大，数值更平稳，但任务与零空间隔离越弱。因此验收阈值应随奇异值或阻尼调度定义，不能固定要求机器精度。反过来，如果远离奇异位形仍有明显泄漏，优先检查投影转置、质量矩阵、Jacobian frame 和矩阵求解实现。

即使投影前满足上述关系，随后逐关节裁剪总力矩也会改变 $\tau_{null}$ 的方向，使零空间重新影响主任务。可以在限幅后计算实际任务力矩残差，或使用带力矩约束的分层 QP，把主任务优先级、零空间目标和关节边界同时放入求解器。零空间验证必须覆盖“投影输出”和“最终发送力矩”两个位置。

6. 通用实现流程

一个完整的笛卡尔阻抗控制循环可以按下图组织：

核心关系与代码对应如下：

积分常见做法是只作用于平移误差，旋转误差不积分；控制器复位时应清除积分状态和上一周期力矩命令。无效维度、非有限输入、质量矩阵分解失败或中间结果非有限时，控制器应拒绝更新并进入预先验证的安全状态。

闭环图中的 Dynamics compensation 是可选支路。基本目标力矩可写成：

是否再显式叠加 $g(q)$ 或 $C(q,\dot q)\dot q$，取决于底层力矩接口或驱动器是否已经补偿这些项。既不能默认存在，也不能在两个层级重复补偿。

6.1 输出保护的正确顺序

设本周期未经限制的力矩为 $\tau_{raw,k}$，上一周期真正发送给驱动器的命令为 $\tau_{cmd,k-1}$。先限制单周期变化量：

再应用绝对力矩限制：

下一周期必须以 $\tau_{cmd,k}$ 而不是 $\tau_{raw,k}$ 为变化率限制的起点。若驱动层还会执行更严格的动态限幅，最好读取或估计其实际接受的命令，否则上层控制器会误以为期望 wrench 已经执行。

力矩限幅会改变任务 wrench 的方向，不只是缩小其模长。逐关节独立裁剪后，笛卡尔空间可能产生非期望分量；高风险接触任务可进一步使用带关节力矩约束的二次规划，使任务误差、零空间目标和力矩边界在同一个优化问题中协调。

从关节力矩预算计算方向力上限

如果任务只需要沿一个已定义方向施力，可以在逐关节裁剪之前计算该方向的可行区间。设 $a\in\mathbb R^6$ 表示任务 frame 中一个纯力方向，标量幅值为 $f$，则：

其中 $\tau_{fixed}$ 是本周期已经被重力补偿、零空间、摩擦前馈等占用的力矩。即使重力补偿由驱动器内部完成，只要这里约束的是电机或减速器的物理总力矩，预算中仍应扣除驱动器预计会添加的部分；只有约束明确针对“上层附加力矩命令”时，才按接口定义排除它。令：

对每个关节的非对称力矩限制 $\tau_{min,i}\le\tau_i\le\tau_{max,i}$，都可得到一个标量区间：

当 $c_i<0$ 时交换上下界；当 $c_i\approx0$ 时，该关节不限制这个方向的 $f$，但仍需确认 $\tau_{fixed,i}$ 自身没有越界。所有关节区间取交集后得到 $[f_{min},f_{max}]$。若交集为空，说明仅固定项就已冲突，或者当前构型无法在约束内产生该方向的 wrench，控制器应降级而不是继续裁剪。

例如某个纯力方向对两个关键关节的力臂系数为 $c=[0.2,0.5]\,\mathrm m$，扣除重力与零空间后正向剩余力矩分别为 $[10,15]\,\mathrm{N\cdot m}$，则正向力上界为：

第二个关节先成为瓶颈。实际命令还应乘小于 1 的安全系数，并为模型误差、摩擦、碰撞瞬态和底层控制器保留余量。

同样的方法也可纳入力矩变化率限制：先将本周期允许区间收紧为

再计算 $f$ 的区间。这样得到的是“本周期能够实现多少”，可能显著小于静态力矩上限。$J(q)$ 和 $\tau_{fixed}$ 都随状态变化，因此可行区间需要在线更新；配置中的 wrench_abs_limit 只应作为额外的任务级上限。

这里的 $a$ 应代表物理意义明确的纯力轴或纯力矩轴。不要把 N 与 N·m 混在同一个欧氏单位向量里归一化。一般六维 wrench、多方向优先级和零空间同时存在时，应改用带约束 QP；上述一维区间法适合快速检查单方向接触力是否可能实现。

6.2 积分项与 anti-windup

最简单的积分状态更新为：

仅限制 $\eta$ 的大小还不够：当关节力矩已经饱和时，积分仍可能继续朝错误方向累积。常见处理有两种：

1. 条件积分：输出饱和且当前误差会进一步加重饱和时暂停积分；误差有助于退出饱和时允许释放积分。
2. 反算回馈：根据实际可实现 wrench 与未限幅 wrench 的差值，将积分状态拉回。

反算形式可以写成：

其中 $F_{achieved}$ 可由限幅后的任务力矩通过稳定的 $J^{T\#}$ 近似恢复。接近奇异位形时该估计不可靠，应退回条件积分或直接冻结积分。目标切换、接触状态切换、控制器重新使能、长时间饱和和故障恢复时，都应明确决定积分是清零、保持还是平滑衰减，不能沿用未定义的旧状态。

6.3 速度滤波、时间对齐与实时循环

阻尼项直接使用速度，因此编码器差分噪声会被 $D_d$ 放大为力矩噪声。若驱动器没有提供质量足够的关节速度，可以先差分再低通：

一阶低通系数应由实际周期和截止频率计算：

滤波截止频率过高不能有效抑制噪声，过低则引入相位滞后并削弱实际阻尼。应根据速度噪声频谱和目标闭环带宽选择 $f_c$，并把滤波器加入离散仿真，而不是只在真机代码里临时增加。

$q$、$\dot q$、关节力矩和外力若来自不同传感器，必须按时间戳对齐。使用“最新一帧”并不等于同一时刻，尤其是外部力传感器或网络状态反馈。延迟为 $T_d$ 时，可以在低速条件下用 $q(t)\approx q(t-T_d)+\dot q(t-T_d)T_d$ 做有限预测，但预测不能替代可靠同步，也不适合高速或强接触阶段。

实时控制循环还应遵循以下约束：

• 用实际测得的 $\Delta t_k$ 更新滤波器、积分器和变化率限制，同时对异常周期设置上下界；
• 预分配矩阵和日志缓冲区，循环内避免动态内存分配、文件写入和格式化输出；
• 参数更新通过无锁快照或双缓冲进入实时线程，并在非实时线程完成合法性检查；
• 动力学分解失败时不要复用未初始化结果，也不要在实时线程中重建模型；
• 将日志写入有界环形缓冲区，由非实时线程异步落盘，并监控丢弃计数；
• 区分传感器时间戳、控制计算时间和命令生效时间，以便离线重建真实延迟。

如果计算时间长期逼近控制周期，即使尚未发生 deadline miss，也应该降级。操作空间求解、SVD 和日志不能无限挤占安全裕度；应在目标硬件上统计计算耗时分布，而不只记录平均值。

7. 可运行示例与控制循环

7.1 一维阻抗仿真

下面的 Python 示例模拟：机器人末端在 $0.5\sim1.5\,\mathrm{s}$ 受到 $10\,\mathrm{N}$ 外力，期望阻抗为 $M_d=2\,\mathrm{kg}$、$D_d=80\,\mathrm{N\cdot s/m}$、$K_d=1000\,\mathrm{N/m}$。理论静态位移为：

import csv

dt = 0.001
duration = 2.5
mass = 2.0          # kg
damping = 80.0      # N*s/m
stiffness = 1000.0  # N/m

position = 0.0
velocity = 0.0
rows = []

for step in range(int(duration / dt) + 1):
    time_s = step * dt
    external_force = 10.0 if 0.5 <= time_s < 1.5 else 0.0

    # M*x_ddot + D*x_dot + K*x = F_ext
    acceleration = (
        external_force - damping * velocity - stiffness * position
    ) / mass
    velocity += acceleration * dt
    position += velocity * dt

    if step % 10 == 0:
        rows.append((time_s, external_force, position, velocity))

with open("impedance_response.csv", "w", newline="") as output:
    writer = csv.writer(output)
    writer.writerow(["time_s", "force_n", "position_m", "velocity_m_s"])
    writer.writerows(rows)

peak_during_force = max(
    abs(position_m) for time_s, _, position_m, _ in rows
    if 0.5 <= time_s < 1.5
)
final_position = rows[-1][2]

print(f"expected static displacement: {10.0 / stiffness:.6f} m")
print(f"peak displacement:            {peak_during_force:.6f} m")
print(f"final position:               {final_position:.6f} m")

assert peak_during_force < 0.02
assert abs(final_position) < 1e-4

可以直接下载完整示例 impedance_1d.py，或将上面的代码保存为同名文件后执行：

python3 impedance_1d.py

预期输出接近：

expected static displacement: 0.010000 m
peak displacement:            0.010013 m
final position:               -0.000000 m

程序会生成 impedance_response.csv。可以绘制 force_n 和 position_m，检查施力后位移是否趋近 $10\,\mathrm{mm}$、撤去外力后是否平稳回零。示例采用先更新速度、再更新位置的半隐式 Euler，只用于教学；真实控制器需要结合采样周期选择经过稳定性验证的离散化方法。

这组参数对应的自然频率和阻尼比为：

$\zeta$ 略小于 1，因此响应较快并只有很小超调。示例峰值约为 $10.013\,\mathrm{mm}$，相对 $10\,\mathrm{mm}$ 静态值的超调约为：

若只将刚度提高到 $2000\,\mathrm{N/m}$，同样的外力会把静态位移减为 $5\,\mathrm{mm}$，但在质量和阻尼不变时，自然频率升高、阻尼比下降，响应可能更容易超调。若希望保持相同阻尼比，应同步按 $D_d\propto\sqrt{K_d}$ 调整阻尼。这个例子说明“刚度更高”不仅改变静态柔顺度，也同时改变闭环动态。

7.2 六维控制循环骨架

下面是与上述通用结构一致的伪代码：

CartesianImpedanceController controller(model, end_effector_frame, dt);

Vector6d stiffness;
stiffness << 100, 100, 100, 10, 10, 10;
controller.SetStiffness(stiffness);
controller.SetDampingRatio(Vector6d::Ones());

while (running) {
    Eigen::VectorXd q = ReadJointPosition();
    Eigen::VectorXd dq = ReadJointVelocity();
    Eigen::VectorXd q_cmd = ReadSafePositionReference();
    Eigen::VectorXd dq_cmd = ReadSafeVelocityReference();

    Eigen::VectorXd tau;
    if (!controller.Update(q, dq, q_cmd, dq_cmd, tau)) {
        EnterSafeState();
        break;
    }
    SendTorqueCommand(tau);
}

示例刚度仅用于说明 API，不是任何真实机器人的安全参数。实际值必须结合负载、采样周期、结构柔性、传感器噪声、环境刚度和关节力矩限制验证。

7.3 使用 Pinocchio 计算动力学量

Pinocchio 可以提供 FK、frame Jacobian、$\dot J$ 和质量矩阵。下面是 C++ 风格的核心计算骨架，具体函数签名应以所使用的 Pinocchio 版本为准：

pinocchio::Data data(model);

pinocchio::forwardKinematics(model, data, q, dq);
pinocchio::updateFramePlacements(model, data);

Eigen::MatrixXd J = Eigen::MatrixXd::Zero(6, model.nv);
pinocchio::computeFrameJacobian(
    model, data, q, frame_id,
    pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, J);

pinocchio::computeJointJacobiansTimeVariation(model, data, q, dq);
Eigen::MatrixXd Jdot = Eigen::MatrixXd::Zero(6, model.nv);
pinocchio::getFrameJacobianTimeVariation(
    model, data, frame_id,
    pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, Jdot);

// Pinocchio Motion::toVector() and this article both use
// [linear, angular]. Verify the actual library/wrapper contract once.
const pinocchio::Motion frame_velocity = pinocchio::getFrameVelocity(
    model, data, frame_id,
    pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED);
const double jacobian_velocity_error =
    (J * dq - frame_velocity.toVector()).norm();
if (jacobian_velocity_error > velocity_consistency_tolerance) {
    throw std::runtime_error("Jacobian/frame velocity convention mismatch");
}

Eigen::MatrixXd M = pinocchio::crba(model, data, q);
M.triangularView<Eigen::StrictlyLower>() =
    M.transpose().triangularView<Eigen::StrictlyLower>();

const Eigen::MatrixXd M_inv_Jt = M.ldlt().solve(J.transpose());
Eigen::MatrixXd lambda =
    DampedPseudoInverse(J * M_inv_Jt, damping);

Eigen::VectorXd task_acceleration = desired_task_acceleration
    + virtual_mass.ldlt().solve(K * error + D * velocity_error);
Eigen::VectorXd task_wrench =
    lambda * (task_acceleration - Jdot * dq);
Eigen::VectorXd task_torque = J.transpose() * task_wrench;

实现时要保持 ReferenceFrame 一致：位姿误差、twist、$J$、$\dot J$ 和 wrench 必须在相容坐标系表达。不要显式计算 $M^{-1}$ 或 $\Lambda^{-1}$ 的普通逆，应使用 LDLT/LLT 求解和带阻尼伪逆，并检查分解、维度和非有限结果。

Pinocchio 官方 Motion 接口文档将六维数组说明为 [linear velocity, angular velocity]，因此上面得到的 $J$ 可以直接配合本文的 [linear, angular] 误差与增益排列。这里不应因为其他库采用不同惯例就盲目交换三维分块。更稳妥的做法是像示例一样，在初始化或非实时测试中比较 $J\dot q$ 与 getFrameVelocity(...).toVector()；还可以分别调用 frame_velocity.linear() 和 frame_velocity.angular() 检查两段含义。该检查必须使用相同 frame_id、ReferenceFrame、$q$ 和 $\dot q$，否则差值没有诊断意义。

若项目自己的适配层决定改成 [angular, linear]，需要同时左乘同一个排列矩阵到 $J$ 和 $\dot J$，并一致重排误差、速度、wrench、$K/D/M_d$ 与限幅；只交换 Jacobian 会破坏 $F^TJ\dot q=\tau^T\dot q$ 的功率一致性。建议把排列转换集中在动力学适配层，控制循环内部只允许一种六维布局。

空间加速度与经典线加速度

Pinocchio 的 getFrameAcceleration() 返回空间加速度，而控制点位置 $p$ 的二阶导数是经典线加速度。二者的线性部分并不总是相同。对同一点且采用相容表达时，有：

因此末端同时旋转和平移时，不能把 getFrameAcceleration(...).linear() 直接当作 $\ddot p$。Pinocchio 官方 frame API为此单独提供了 getFrameClassicalAcceleration()，其说明明确包含离心效应。

对于本文使用的几何 Jacobian，运动学关系是：

可以在非实时测试中令 $\ddot q=0$，调用二阶 forwardKinematics(model,data,q,dq,zero_ddq)，再比较 Jdot * dq 与相同 frame 约定下的 getFrameClassicalAcceleration(...).toVector()。还应沿一条平滑轨迹用中心差分检查 $J$ 的变化：

差分时必须在同一个 LOCAL_WORLD_ALIGNED 表达中重新计算两侧 Jacobian，并避开关节坐标的环绕不连续。有限差分误差应随 $\Delta t$ 先减小；当步长过小时又会受浮点误差影响。若只在静止姿态测试，$\omega\times v$ 和 $\dot J\dot q$ 都可能为零，无法发现这类错误，所以验证轨迹必须包含组合关节运动和工具旋转。

Pinocchio 还可以分别计算纯重力项和包含科氏/离心项的非线性项：

Eigen::VectorXd gravity =
    pinocchio::computeGeneralizedGravity(model, data, q);

Eigen::VectorXd nonlinear =
    pinocchio::nonLinearEffects(model, data, q, dq);

其中 gravity 对应 $g(q)$，而 nonlinear 对应 Pinocchio 刚体模型中的 $C(q,\dot q)\dot q+g(q)$。因此常见输出只能根据接口契约选择一种：

// A. 原始力矩接口，只补偿重力
tau_command = tau_impedance + gravity;

// B. 原始力矩接口，控制律需要完整刚体非线性补偿
tau_command = tau_impedance + nonlinear;

// C. 底层已经完成所需补偿
tau_command = tau_impedance;

不能使用 tau_impedance + gravity + nonlinear，因为 nonlinear 已经包含重力。Pinocchio 默认不会替驱动器发送或执行补偿，它只根据模型计算数值；究竟选择 A、B 还是 C，仍由机器人力矩接口和控制律推导决定。

工具与 payload 必须已经体现在用于构建 model 的惯性参数中，否则 gravity 和 nonlinear 都只对应空载或旧负载。运行时切换 payload 时，应构造并批准对应模型版本，避免实时线程临时修改尚未验证的惯性数据。

若已知期望关节加速度，RNEA 可以计算完整逆动力学力矩：

Eigen::VectorXd tau_inverse_dynamics =
    pinocchio::rnea(model, data, q, dq, desired_joint_acceleration);

其理想刚体含义是：

当 desired_joint_acceleration 为零时，RNEA 的刚体部分与 nonLinearEffects 对应；它并不包含 URDF 刚体模型之外的摩擦、线缆力或驱动器内部补偿。

不能把 tau_inverse_dynamics 无条件加到前面已经通过 $J^T\Lambda\ddot x_{cmd}$ 计算出的任务力矩上，因为两条路径都可能包含惯量和非线性补偿。常见正确结构有两类：

1. 在操作空间完整推导 $F_{ctrl}$，再映射为 $J^TF_{ctrl}$，只补充推导中尚未包含的关节空间项。
2. 先把任务目标转换为一致的 $\ddot q_d$，再使用一次 RNEA 计算完整关节力矩。

两种结构都可以成立，但不能把它们各自的“完整输出”相加。代码审查时应给每个力矩分量标注来源，例如 tau_task、tau_gravity、tau_coriolis、tau_null，并写出它是否已经包含在其他分量中。

如果使用其他库，例如 RBDL、Drake、KDL 或厂商动力学 SDK，也可以按同一接口组织：模型层提供 FK、$J$、$\dot J$、$M$ 和非线性项，阻抗控制层只负责误差、目标 wrench、零空间和安全限制。

7.4 一份可审查的参数配置示例

不要把增益和限幅散落在控制代码中。下面的 YAML 展示一种通用组织方式，便于版本管理、离线检查和运行日志关联。可以直接下载示例配置 impedance_example.yaml：

schema_version: 2
config_id: cartesian-impedance-sim-001
model_hash: replace-with-urdf-sha256
tool_id: example-tool-v1
payload_id: empty

frames:
  base: base_link
  controlled: tool0
  task_parameters: tool0
  reference: local_world_aligned
  spatial_order: [linear, angular]
  characteristic_length_m: 0.10
  pose_error: desired_minus_measured
  wrench_sign: environment_on_robot

interface:
  command: raw_joint_torque
  driver_compensation: none
  effort_limit_scope: physical_total_torque

control:
  period_s: 0.001
  formulation: inertia_shaped
  model_compensation: gravity
  stiffness: [300.0, 300.0, 200.0, 12.0, 12.0, 8.0]
  damping_ratio: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
  virtual_mass: [3.0, 3.0, 3.0, 0.15, 0.15, 0.10]
  integral_gain: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

filter:
  joint_velocity_cutoff_hz: 30.0
  external_wrench_cutoff_hz: 20.0

nullspace:
  enabled: false
  stiffness: 0.0
  damping_ratio: 1.0

contact:
  enabled: false
  normal_axis: positive_z
  target_force_magnitude_n: 0.0
  enter_force_magnitude_n: 3.0
  exit_force_magnitude_n: 1.5
  enter_hold_s: 0.03
  exit_hold_s: 0.05
  force_ramp_n_s: 20.0
  allowed_peak_force_n: 20.0
  emergency_force_n: 30.0

safety:
  translation_error_m: [0.02, 0.02, 0.02]
  rotation_error_rad: [0.10, 0.10, 0.10]
  wrench_abs_limit: [20.0, 20.0, 20.0, 3.0, 3.0, 3.0]
  torque_rate_nm_s: 100.0
  state_timeout_s: 0.005
  consecutive_deadline_misses: 3

六维数组顺序与 spatial_order 对应：前三项分别使用 N/m、kg 或 N，后三项分别使用 N·m/rad、kg·m² 或 N·m。该示例刻意关闭积分和零空间，数值只用于仿真演示，不能作为特定机器人的真机推荐参数。逐关节绝对力矩上限应来自机器人手册和风险评估，通常还需要单独配置，不能用一个通用数值替代。

task_parameters 表示 $K/D/M_d$ 主方向所在的 frame，不一定与 controlled 控制点相同；二者不同时必须执行第 4.4 节的完整变换。characteristic_length_m 是第 1.2 节用于六维监控指标的特征长度，不改变控制器本身的物理增益。pose_error 和 wrench_sign 将本文的误差、外力正方向固定到配置中，防止更换实现后只改了一个负号。tool_id、payload_id 与 model_hash 一起描述当前动力学对象，任一项变化都应生成新的配置版本。

effort_limit_scope: physical_total_torque 表示安全限制针对执行器实际承受的总力矩，因此即使某项补偿由驱动器内部添加，也要纳入力矩预算。若硬件 API 明确只限制上层附加命令，可以使用 commanded_additional_torque，但仍需另有机制保护物理总力矩。这两个语义不能通过观察字段名称猜测，必须与厂商接口和实测饱和行为核对。

contact 将第 4.4 节和图中的接触状态参数落实到配置。normal_axis 给出任务 frame 中的期望接触方向，力目标使用非负幅值，实际符号由该方向与 wrench_sign 共同确定。示例关闭接触力控制，因此目标幅值为零，但仍保留检测和安全阈值供回放测试。开启后必须满足：

目标作业力还必须不高于允许峰值。allowed_peak_force_n 用于受控降级或退出，emergency_force_n 用于立即进入经验证的安全路径，两者不能合并。阈值比较通常针对期望法向投影的幅值；反方向力、切向力或合 wrench 超限仍需独立安全检查，不能因法向符号不匹配而被忽略。

driver_compensation 描述驱动器已经完成的补偿，model_compensation 描述本文控制器还要增加的补偿。后者建议只允许 none、gravity 和 nonlinear 三种枚举值。示例表示驱动器接收原始关节力矩，上层仅加入重力补偿。formulation 只允许 direct_wrench 或 inertia_shaped，接口 command 则明确区分 raw_joint_torque 与 compensated_joint_torque。

控制器加载配置时至少应验证：数组长度均为 6、所有元素有限、虚拟质量严格为正、刚度与阻尼比非负、截止频率低于 Nyquist 频率、frame 存在、模型哈希匹配，以及安全限制不超过硬件允许值。若 driver_compensation 已包含重力，则 model_compensation: gravity 或 nonlinear 应判定为冲突并拒绝使能，而不是只打印警告。运行日志应记录最终生效配置的完整内容或哈希，而不只是文件路径，因为同一路径下的文件可能已经改变。

可以下载配置校验脚本 validate_impedance_config.py。脚本依赖 PyYAML，运行方式为：

python3 -m pip install pyyaml
python3 validate_impedance_config.py impedance_example.yaml

正常输出为：

valid: impedance_example.yaml

该示例会检查主要数值约束和补偿互斥关系，但不会读取机器人厂商的真实关节力矩上限，也无法判断 URDF 哈希、frame 和控制接口语义是否真实匹配。工程控制器仍需在加载模型后完成这些上下文检查。

8. 参数如何整定

推荐按以下顺序：

1. 确认重力补偿、关节方向、雅可比和力矩方向正确；
2. 关闭积分和零空间，使用很低的平移/旋转刚度；
3. 将阻尼比从接近临界阻尼开始，观察自由空间阶跃响应；
4. 逐轴提高刚度，每次只改变一个方向；
5. 接触柔软环境，再逐步测试更硬环境；
6. 加入保守的零空间刚度，确认不干扰主任务；
7. 最后才考虑小积分项，并验证饱和恢复与目标切换。

平移刚度单位通常是 $\mathrm{N/m}$，旋转刚度单位是 $\mathrm{N\cdot m/rad}$；两者数值不能直接比较。虚拟质量平移部分单位为 kg，旋转部分对应 $\mathrm{kg\cdot m^2}$。

8.1 从任务指标反推参数：一个算例

假设一个沿工具 $z$ 轴接触的任务提出以下初始指标：

• 期望在 $12\,\mathrm{N}$ 稳态外力下允许约 $6\,\mathrm{mm}$ 位移；
• 希望该方向的名义自然频率约为 $3\,\mathrm{Hz}$；
• 初始阻尼比取 $\zeta=1$；
• 控制器周期为 $1\,\mathrm{ms}$。

先由静态柔顺关系计算刚度：

再由 $k_z=m_{d,z}\omega_n^2$ 反推虚拟质量，其中 $\omega_n=2\pi f_n$：

临界阻尼为：

因此该方向的第一组仿真候选值为 $k_z=2000\,\mathrm{N/m}$、$d_z\approx212\,\mathrm{N\cdot s/m}$、 $m_{d,z}\approx5.63\,\mathrm{kg}$。它们只是由理想二阶模型得到的起点，不能直接视为真机安全值。

如果控制器没有进行精确惯量整形，应先在代表性构型计算该方向的自然操作空间惯量，而不是强行使用上述 $m_{d,z}$。对方向单位向量 $a_z$，可评估：

然后用 $d_z=2\zeta\sqrt{m_{eff,z}k_z}$ 重新计算阻尼。由于 $m_{eff,z}$ 会随构型变化，应在整个任务工作空间采样其范围，并以仿真和真机响应决定是否采用增益调度。

其余方向不应照抄 $k_z$。例如插入轴需要顺应时，轴向刚度可能低于横向对准刚度；旋转刚度还具有不同单位。第一轮真机测试仍应按比例降低全部候选刚度，并同步降低速度、误差上限和力矩上限，再逐步接近设计值。