经验值的工程化，实现机械臂末端柔顺

如果只关心末端力相对稳定，就可以大幅简化，甚至绕过复杂标定，采用“经验参数 + 被动柔顺 + 主动导纳试凑”的策略。

核心理念

• 利用 MIT 模式的被动柔顺：即使力控不太准，高阻尼和高刚度也能让接触力波动被机械柔顺吸收一部分。
• 力传感器只做最简单的处理：去零漂 + 低通滤波，不做重力/惯性补偿。
• 导纳参数靠手调试凑，不追求理论最优。
• 期望力可以是一个粗略范围，比如刚刚好接触、刷毛压缩量适中。

极简实施步骤

第 1 步：硬件与通信就绪

• 所有关节确认为 MIT 模式，CAN 周期 2ms，能收目标位置和 Kp,Kd,τ_ff。
• 六维力传感器能稳定读到数据（不要求补偿）。
• 机械臂末端装好清洁模组。

第 2 步：设置统一的保守 MIT 参数（不单独整定）

为了避免逐关节调参，采用统一的保守值：

• Kp：取一个中等值，确保机械臂能撑起自重且不抖动。经验值可以是 50~100 Nm/rad（取决于电机大小），可以先从小试起。
• Kd：取 Kp 的 0.02~0.05 倍，或者直接设一个较小固定值如 5~10 Nm/(rad/s)。
• τ_ff：暂时全设 0。

验证：发送一个缓慢的关节正弦摆动，观察是否抖动，若抖动，降低 Kp；若响应太慢，适当增加 Kp，最后取一个不抖的值固化给所有关节。

第 3 步：力传感器处理

1. 零漂去除：清洁模组悬空，记录 1 秒平均值作为偏置，之后每周期减去它。
2. 低通滤波：对力信号做一个简单的移动平均或一阶低通（截止频率 20Hz），滤除高频噪声。

不做重力补偿：这会导致传感器读数包含了清洁模组的自重，没关系，我们后面通过试凑的期望力来包含这个重量。

第 4 步：导纳参数经验取法与试凑

导纳公式只用最简单的一阶形式（忽略加速度项），然后积分得到位置修正量，只整定一个参数 D（阻尼），M 忽略或取 0。

• 期望力 F_des 的确定：通过手动拖动或试贴，找到认为刚好贴紧但不过压的力传感器读数（注意此时传感器包含工具重量，所以 F_des 可能是 5N~10N 之类的值，包含了清洁模组的重力偏置）。记录下这个值作为期望力。
• 阻尼 D 的调试：从很大的 D 开始（例如 500），此时力控很粘稠。执行贴合，观察力传感器读数，如果力波动大，说明太软，可减小 D；如果力响应极慢，可减小 D，慢慢找到一个力波动在可接受范围的 D 值。

第 5 步：贴合运动（极简状态机）

1. 快速趋近：用纯位置模式（MIT 高 Kp，Kd），让刷头移动到大致位置（预先示教或手拖记录几个关键点）。
2. 降速逼近：以低速度（如 10mm/s）向釉面移动，同时开启导纳控制器（期望力设为上面获得的 F_des）。此时，一旦接触，导纳会自动将工具回退以维持力。
3. 恒力清洁：保持导纳开启，沿着规划好的简单轨迹（比如来回直线或螺旋）移动，切向速度保持恒定。
4. 退出：清洁完成，撤去期望力，让导纳自动脱开，然后快速退回。

第 6 步：MIT 被动柔顺

在上述步骤中，即使导纳参数不太理想，MIT 模式的高 Kd 也会提供一定的阻尼，防止振荡。如果接触力突然增大，关节自身的弹性也会有一定退让，起到被动保护作用。

核心原理

• 釉面不是高精度力控任务，更类似保持接触、轻微压力的场景，力波动几牛顿都不会影响清洁效果。
• 通过直接试凑期望力，避免了复杂的传感器补偿，把工具重量当成力控目标的一部分。
• MIT 模式的被动柔顺特性降低了主动力控的调试难度。
• 整套流程不需要任何标定实验，所有参数均可在线手调。

效果评估标准

• 贴合过程中，力传感器读数波动在 ±2N 以内。
• 清洁头无明显弹跳或脱离釉面的现象。
• 清洁后釉面无划伤。

优先采用极简方法，快速实现恒力贴合。当未来需要更精确力控或更高效率时，再逐步引入补偿和标定。这个分步走的策略，最符合软件工程领域的敏捷实践——先跑通闭环，再优化性能。