导纳MDK 跟传统 PID 控制算法的对比

导纳控制和常规 PID 控制不是同一个原理，但在特定条件下，它们在行为上有相似之处。下面从原理、区别、以及如何确定那三个参数（质量 M、阻尼 D、刚度 K）三个方面来系统说明。

一、原理上的根本不同

1. 常规 PID 控制

PID 是一种基于误差校正的通用控制器，它不依赖被控对象的模型。原理是：

控制输出 = Kp * e + Ki * ∫e dt + Kd * de/dt

• 输入是“期望值”与“实际值”之间的误差 e（位置误差、力误差等）。
• 输出直接是执行器的指令（电机力矩、速度、阀门开度等）。
• 核心思想：直接根据误差的大小、累积和变化趋势来强行修正，直到误差为零。

如果用在力控制上，就是一个“力 PID”：期望力减实测力得到力误差，PID 输出直接变成机器人关节力矩或末端位置修正量（不加任何模型），直接驱动。

2. 导纳控制

导纳控制是一种基于动力学模型的交互控制，它的原理是人为构造一个“弹簧-阻尼-质量”的虚拟力学系统：

M * Δẍ + D * Δẋ + K * Δx = F_ext - F_des

• 左边是虚拟机械系统的动力学，右边是力误差（实测外力减期望力）。
• 求解这个微分方程，得到位置修正量 Δx（或速度、加速度），然后把这个 Δx 叠加到当前目标位置上，发给机器人内部的位置伺服环去执行。
• 核心思想：不是在直接消除力误差，而是让机器人在外力误差下，表现得像一个指定的物理系统（有质量、有阻尼、有弹簧）。力误差不是被“控制没了”，而是被转化成了符合物理规律的运动。

简单对比：

• PID 直接说：“力小了，给我往前顶”。
• 导纳控制则说：“假设我是一个有质量有弹簧的滑块，现在受了一个不平衡力，我会怎么动？机器人就那样动。”

二、关键区别

一句话总结： PID 是一个“强硬的纠错器”；导纳控制是“赋予机器人一个虚拟的柔顺人格”，它本质上是一个轨迹生成器 + 位置环，而 PID 是误差驱动。

三、如何确定导纳控制的三个参数

清洁任务中，主要是法向恒力贴合，典型工况是工具需要以恒力压在釉面上，并跟随曲面移动。此时导纳控制器的目标不是提供刚度恢复力，而是消除稳态力误差，因此通常设刚度 K = 0。这样方程式简化为：

M * Δẍ + D * Δẋ = F_des - F_ext

这变成了一阶导纳（对速度积分是位置），或者如果你对速度做导纳控制，输出速度修正。现在需要确定的就是 虚拟质量 M 和虚拟阻尼 D。

1. 参数 M（虚拟质量）

• 物理意义：表征机器人在受到力误差时，加速度响应的“惯性”大小。M 越大，运动越迟钝，但力超调越小；M 太小，容易对力噪声敏感，引起抖动。
• 确定原则：
  • 下限：至少是机器人末端实际有效惯量的 1~2 倍，太低会导致不稳定（内环位置控制器会与导纳动力耦合）。
  • 上限：受期望的力响应时间限制。假设你希望从接触力变化到位置修正基本完成在 0.1 秒 内，则带宽应 > 10 Hz。简化估算：一阶系统时间常数 τ ≈ M/D。但考虑二阶特性，可先预估。
  • 经验起点：在 1~5 kg 量级（取决于你的末端模组重量），例如对轻载清洁头，可选 M = 2 kg。

2. 参数 D（虚拟阻尼）

• 物理意义：决定系统对力误差响应的“粘滞感”和能量耗散。D 越大，运动越平缓，但达到稳态力的时间越长。
• 与 M 配合决定动态响应： 定义阻尼比 ζ = D / (2√(M * K_eq))。因为你设 K=0，理论阻尼比用不了，更直接看时间常数。 近似为一阶惯性环节时，力响应时间常数 τ ≈ M/D。要达到 0.1 秒内完成90%响应，则需 τ ≈ 0.03 秒，即 D ≈ M / 0.03。若 M=2 kg，则 D ≈ 66 Ns/m。可根据实际振荡情况增大。
• 下限约束：保证系统稳定，尤其接触环境有刚度 Ke（釉面+刷毛）。即使 K=0，导纳闭环与环境的交互也可能振荡。稳定性条件近似：D > Ke / ω_pos 之类的（ω_pos 是位置内环带宽）。通常取较大阻尼比是安全的。
• 起点推荐：先取 D 使得系统在自由空间阶跃力响应呈临界阻尼或过阻尼，没有超调。在贴合场景，宁可稍有过阻尼，也不要有超调冲击釉面。例如 M=2, D=100 Ns/m。

3. 参数 K（虚拟刚度）

• 在恒力贴合中通常设为 0，因为我们需要稳态时即使有位置偏移，力也恒定（F_ext = F_des）。如果 K > 0，工具会像弹簧，在偏离参考点时产生恢复力，无法做到恒力。
• 什么情况会用 K > 0？如果你要工具在无接触时回到某个参考位置，可以用很小的 K 做回位倾向，但贴合时要切换为 K=0。

4. 实际整定步骤（结合你的机器人）

1. 自由空间测试：机器人不接触，给一个虚拟的阶跃力指令（如 F_des 从 0 跳到 3N），观察末端位置修正是否平滑、有无振荡。调整 M 和 D，使速度没有过冲，力传感器跳动能被滤除。
2. 轻接触测试：用软泡沫代替，以低速接近，观察接触瞬间力尖峰。如果力超过期望 50% 以上，增大 D 或减小 M。
3. 真实釉面贴合：在平直釉面区域运行，逐渐提高切向移动速度，看力控能否维持 ±0.5 N。若跟不上曲面变化（力误差累积），可略微减小 M 和 D 以提高响应；若出现抖动，则增大 D。
4. 利用双俯仰冗余：将小行程俯仰关节配置为更高带宽的导纳修正端，M 可更小、D 稍低，负责高频补偿；大俯仰跟随曲面慢变部分，其导纳参数可设得更“重”一些（M 大、D 大）。

因为有高带宽位置内环，导纳参数可以在很大范围内稳定。“保守过阻尼，无超调”是保护釉面的首要原则，可以预先计算：选定期望的力稳定时间 T_s（如 0.2s），由一阶近似取 D = M / (T_s/3)，然后确保 D 大于某个由环境最大刚度和位置环延迟决定的稳定下限。