电机与控制算法：康复机械臂控制调试经验

在 KineWeave 康复机械臂中期检查前的调试阶段，控制问题的关注点逐渐从“能不能运动”转向“运动是否稳定、轨迹是否自然、系统是否能够可靠复现”。早期只下发位置指令时，跟随误差较大，运动过程中也容易出现抖动；加入速度前馈并重新处理轨迹采样与加减速后，控制效果得到明显改善。

这段经历说明，电机控制的效果并不只取决于控制器参数。指令中包含什么信息、轨迹如何采样和插补、通信链路能否及时同步，以及机械系统能否准确传递运动，都会共同影响最终表现。

电机控制模式的前提

KineWeave 使用的是关节电机。作为面向关节运动的电机，它本身具备厂家提供的三环 PID 控制能力，包括位置环、速度环和电流环。厂家已经为这类电机提供了相应的控制配置，项目可以在此基础上直接使用，不需要从零开始实现电机底层控制器。

因此，本文所说的“位置控制”描述的是项目当前采用的控制模式，而不是说电机只有位置环。电机接口还可以接收速度前馈以及电流前馈（也可以理解为力矩前馈）等信息；在这段调试中，实际采用的是位置控制模式，并在位置指令的基础上加入速度前馈。

位置控制模式下的速度前馈

早期在位置控制模式下只向电机下发位置指令。电机需要根据当前位置与目标位置之间的误差不断修正运动速度，整个过程更像是“发现偏差后再追赶”。当目标位置持续变化时，这种追赶会反复经历加速、减速和重新稳定，容易带来较大的跟随误差和运动抖动。

加入速度前馈后，位置控制系统同时获得目标位置和期望速度。位置环仍然负责修正误差，速度前馈则提前提供运动趋势，使电机不必完全依靠位置误差去“追”出速度。

项目	原方案	调整后	观察结果
下发内容	仅位置	位置 + 速度前馈	跟随误差下降约 80%-88%
控制方式	根据位置误差被动追赶	位置修正与速度趋势协同	运动过程更加平滑

这里的经验不是用前馈取代反馈，也不是重新实现电机内部的三环 PID，而是在现有位置控制模式下补充更完整的运动信息：反馈负责纠偏，速度前馈负责提供“接下来应该怎么走”的信息。对于连续运动的机械臂，仅有位置指令往往不足以表达完整的运动意图。

抖动需要从采样与轨迹溯源

低速或运动过程中出现抖动时，表面现象通常是“电机在抖”，但根因可能位于电机之外。调试中逐步确认，抖动主要与以下两类问题有关：

根因	具体表现
采样与指令密度不足	原始轨迹点有限，下发间隔较长，电机内部控制循环需要在相邻指令之间持续估计运动趋势
加减速不平滑	轨迹初始或末端存在跳变，插补后可能形成速度突变和加速度尖峰

原始上肢动作数据的采样频率约为 120 Hz，而 35 ms 或 100 ms 的指令下发间隔，相对于电机内部 1 kHz 级的控制循环并不算高频。若轨迹点本身不够密集，或者相邻点之间的速度变化没有处理好，电机得到的就不是一条连续平滑的运动轨迹，而是一组需要不断追赶的离散目标。

在一次离线验证中，将真实 120 Hz 录制数据直接送入五次样条后，原始信号约 6 rad/s² 的加速度特征被放大到 186 rad/s²。默认 6.5 s 回放中有 21/159 个文件校验失败，部分数据在录制结束时仍处于匀速运动状态。这说明插补并不会自动修复采样问题；如果输入轨迹存在速度跳变，插补反而可能把问题放大为明显的加速度尖峰。

轨迹处理不是点数越多越好

针对采样密度和轨迹不平滑的问题，调试中主要采用了重采样、端点平滑和逐级寻优三种处理方式。

重采样与下发频率

将原始约 120 个采样点重新处理为 225 个控制点；
将指令下发间隔从 100 ms 逐步缩短到 35 ms；
将动作回放周期从约 25 s 压缩到 8-9 s；
通过 100 ms、90 ms、80 ms 逐级下调的方式寻找稳定点，而不是一次性跳到最小间隔。

35 ms 最终成为速度与稳定性之间的一个平衡点。继续单纯提高下发频率，并不能保证运动更稳定，反而可能把采样噪声、轨迹跳变或通信压力更快地传递到系统中。

端点平滑与运动模式

采样轨迹的初始和末端如果直接出现较大位移或速度，机械臂会在动作开始和结束时产生突兀变化。因此，面向自然运动的回放需要让轨迹从静止状态平滑进入目标运动，再平滑回到静止状态，也就是采用 rest-to-rest 的端点处理方式。

但端点平滑并不是所有场景的唯一选择：需要复现原始动作时，应尽量贴合录制数据；需要让机械臂自然起停时，则应使用平滑端点。轨迹处理应当根据应用目标切换，而不是固定采用一种模式。

离线模拟还显示，对轨迹控制点进行适度抽稀可以消除插值伪影，中间过程的保真度可控制在 1 度以内，最终 159/159 个文件通过验证。这进一步说明，轨迹优化的重点是消除不连续和伪影，而不是无条件增加点数。

控制问题要沿系统链路排查

这次调试还形成了一个更广泛的排查边界：运动抖动不应只在算法层寻找原因，而应同时观察算法、通信和机械三个层面。

算法层：检查采样精度、轨迹点密度、插补方式、加减速平滑和端点处理。
通信层：检查指令下发频率、总线吞吐和多设备之间的同步性。项目早期使用 CAN 通讯，后续将电机与六维力传感器统一转为 EtherCAT，以支持 1 kHz 级的同步控制；力矩传感器仍通过 CAN 通讯，频率低于 500 Hz，成为当时链路中的限制因素。
机械层：检查装配间隙、结构刚性和力的传递路径。机械间隙可能让部分力无法有效传递，使算法层的抖动与机械层的响应不足叠加出现。

这三层并不是相互独立的：轨迹采样会影响通信负担，通信同步会影响控制响应，机械间隙又会改变控制器实际看到的运动结果。相关的机械结构与传动问题，可结合机械结构与传动：康复机械臂整机调试经验一并理解。

阶段性结论

调试现象	形成的认识	对应经验
只下发位置时误差较大	位置反馈只能在误差出现后进行修正	连续运动中应结合速度前馈表达运动趋势
低速运动存在抖动	抖动可能来自采样密度和轨迹平滑，而不只是电机本体	先检查原始数据、指令间隔和加速度连续性
插补后出现加速度尖峰	插补会放大输入轨迹中的跳变	先处理采样与端点，再选择插补参数
提高频率后效果不一定更好	控制频率需要与数据质量、通信能力和机械响应匹配	通过逐级调参寻找速度与稳定性的平衡点
算法调整后仍有异常响应	机械间隙和通信同步可能继续影响控制效果	沿算法、通信、机械链路共同排查

小结

康复机械臂的运动控制效果，是位置反馈、速度前馈、轨迹质量、通信同步和机械响应共同作用的结果。速度前馈解决了“只追位置”的被动性，轨迹重采样和端点平滑解决了离散数据带来的速度跳变，而跨层排查则避免把所有问题都归因于电机或 PID 参数。

这段调试经验最终沉淀为一个比较稳定的判断顺序：先确认输入轨迹是否合理，再确认指令是否以合适的频率和同步方式送达，最后结合机械结构检查实际运动是否能够被准确执行。只有把这几层放在同一条链路中观察，控制算法的优化才有明确的落点。