关节MIT 模式，导纳算法实现柔顺贴合

在没有 CSP 的情况下，MIT 模式凭借其可编程阻抗的本性，能够通过高刚度设置完美接管位置伺服任务，同时保留了未来向更高级力控与安全功能升级的潜力，是当前最优的选择。

若只有 PP、PV、MIT 三种模式，目标是实现基于末端力传感的导纳贴合，这需要驱动器能够作为一个高响应的位置执行器来工作，让上位机输出的位置修正量能被精确跟踪。

我们先分析三种模式的特点，再给出选择方案及其理由。

一、三种模式的核心区别

贴合任务的需求：

• 上位机以 1 kHz 频率运行导纳控制器，输出末端位置修正量。
• 修正量通过逆运动学转换为关节目标，需要驱动器实时、平滑地跟踪这一连续变化的位置指令，且不允许有位置累积误差。

二、PP 模式：不适合

• 指令方式：每次发送一个完整的位置、速度、加速度 profile，驱动器自己完成插补，通常用于点到点运动。
• 致命缺陷：无法接受高频的、微小连续位置更新，如果每 1 ms 都发一个新目标，驱动器会反复启动新的插补，导致运动卡顿，甚至报错。
• 结论：无法用于导纳外环的实时位置流架构。

三、PV 模式：次选方案

• 指令方式：可周期性发送目标速度，驱动器维持速度闭环。
• 能否模拟位置控制？ 上位机可做位置闭环：将导纳输出的位置修正量转换为速度指令（如比例控制），发送给 PV 模式，但从位置到速度是开环积分，存在漂移，且动态位置精度难以保证。接触力控需要精确的位置保持，PV 模式在零速附近容易受静摩擦影响，容易出现粘滑或力波动。
• 风险：位置无保证，安全边界（如限位）完全依赖上位机速度积分，有失控风险。
• 结论：勉强可用，但工程复杂度高、稳定性差，不建议作为主方案。

四、MIT 模式：最佳方案

MIT 模式的本质是带阻抗特性的力矩控制，但它可以通过参数设置退化为虚拟位置模式，这能够完美模拟 CSP 行为，甚至更好。

1. 用 MIT 模拟 CSP

将 MIT 控制律中的参数设置为高刚度：

• 设定 Kp为一个很高的值（接近关节物理刚度的极限，如几百 Nm/rad）。
• 设定 Kd为对应的临界阻尼值（2√KpJ），保证无振荡。
• 设定 τff=0（或用于补偿重力和摩擦）。

这样，驱动器内部的力矩环会竭尽全力让关节跟踪 qdes，其行为极其逼近一个纯位置伺服，任何位置误差都会被高增益放大成大力矩，迅速纠正。这就完全取代了 CSP 的位置闭环。

2. 完整控制架构（导纳 + MIT 虚拟位置环）

[力传感器] → 导纳控制器 → ΔX (笛卡尔)
                              ↓
[轨迹规划] → X_ref → (+) → X_cmd → [逆运动学] → q_des (关节)
                              ↓
                           [MIT 模式驱动器] (Kp 高, Kd 高)
                              ↓
                            关节实际运动 → 末端运动 → 接触力

• 导纳外环：不变，仍输出位置修正量。
• MIT 内环：接收 qdes（期望关节角）和 q˙des（期望速度，可设为零或由差分提供），以高刚度执行位置跟踪。
• 效果：与原先的 CSP 模式几乎一致，因为都是位置指令 + 高增益闭环。

3. MIT对比 CSP

MIT 模式是 CSP 的超集，它带来额外好处：

• 拖动示教：需要时可将 Kp,Kd设为零，仅用 τff 补偿重力，即可实现零力拖动。
• 碰撞柔顺：可临时降低刚度，让关节在意外碰撞时退让，而不必依赖末端力传感器触发。
• 参数在线调整：可以根据清洁面的特点，微调关节刚度来抑制特定频率的振动，这是标准 CSP 做不到的。

五、MIT对比标准 CSP 方案

六、方案落地

采用“导纳外环 + MIT 模式（高刚度虚拟位置环）”作为贴合控制方案。

1. 单关节整定：在无负载情况下，对每个关节的 MIT 模式进行 PD 参数整定，目标：使关节对阶跃位置指令的响应快速、无超调（临界阻尼），记录 Kp,Kd基准值。
2. 摩擦力补偿：辨识各关节的库仑+粘滞摩擦，将其作为 τff 的一部分，以减小位置静差。
3. 重力补偿：通过运动学实时计算各关节重力矩，也加入到 τff 中，使得在零导纳修正时机械臂也能自主悬浮。
4. 集成导纳外环：将原先 CSP 架构中的 q_cmd 计算逻辑直接复用，作为 MIT 模式的 q_des 输入。q˙des可由位置差分得到（并滤波），或直接设为零（依赖 PD 自己产生阻尼）。
5. 稳定性验证：先在自由空间测试轨迹跟踪，再在真实釉面上调优导纳参数，方法与 CSP 架构相同。
6. 扩展柔顺功能：在非贴合阶段（如区域切换），可动态降低 Kp 让手臂更柔顺，提高安全性。