📚 课程目录第3章 XTS动子位置与速度感知

3. XTS动子位置与速度感知

在XTS系统中，碰撞避免与间距控制的所有算法都建立在一个共同前提上：控制器必须准确、及时地知道每个动子此刻在哪里、以多快速度运动、加速度是否正在变化。如果位置感知存在延迟或噪声，即使间距计算逻辑本身正确，也可能因为"看到的是过去的位置"而做出错误决策。本节围绕XTS动子的位置、速度、加速度感知链路，讲解从编码器反馈到TwinCAT中可用数据的完整过程。

一、感知链路的整体结构

XTS的每个动子（Mover）内部装有永磁体阵列，导轨（Track）上分布着若干电机模块（Motor Module），每个电机模块包含驱动线圈和位置传感器。动子经过某个电机模块时，该模块的传感器检测动子磁场位置，产生原始位置信号。这些信号由电机模块本地处理，通过EtherCAT分布式时钟同步后上传至TwinCAT控制器。控制器中的XTS功能库（Tc3 XTS）将原始数据转换为每个动子的全局位置、速度和加速度。

整个链路可以概括为：传感器原始信号 → 电机模块本地处理 → EtherCAT过程数据 → TwinCAT XTS功能库 → 用户可读的动子状态。理解这条链路，才能知道延迟和误差可能出现在哪个环节。

二、编码器反馈与位置原始值

XTS的位置传感器并非传统意义上的旋转编码器，而是基于磁场检测的线性位置传感系统。每个电机模块内部有多个霍尔或磁阻传感元件，当动子磁极经过时，传感器输出与磁场强度相关的模拟信号。电机模块的本地处理器将这些信号转换为该模块范围内的绝对位置值，单位通常为微米（μm）或增量计数。

由于动子可能同时被两个相邻电机模块检测到，系统需要进行"交接"处理：当动子从一个模块移动到下一个模块时，位置值必须连续过渡，不能出现跳变。Tc3 XTS功能库自动处理这种交接，对外输出统一的全局位置。用户通过Axis接口或Mover结构体读取时，看到的是已经拼接好的连续位置。

在TwinCAT中，动子位置通常以LREAL类型返回，单位为毫米（mm）。例如，moverPosition[3] := 1250.847;表示3号动子位于导轨1250.847 mm处。这个数值的更新周期与EtherCAT周期一致，典型值为1 ms或更短。

三、状态字解析：判断数据是否可信

仅仅读取位置数值是不够的。每个动子的状态字（Status Word）包含了数据有效性和动子状态的关键信息。Tc3 XTS中，动子状态通常通过MoverStatus或类似结构体暴露，其中几个关键位需要重点关注：

PositionValid：位置数据是否有效。如果动子尚未被任何电机模块检测到，或处于交接异常区域，该位为FALSE，此时位置值不可用于碰撞判断。
Enabled：动子是否处于使能状态。未使能的动子可能被手动移动，其位置变化不代表受控运动。
InPosition：动子是否已到达目标位置。在间距控制中，该位可用于判断动子是否已完成定位。
Error：是否存在驱动或传感错误。一旦置位，位置和速度数据可能不可信。

实际编程中，建议在每次读取位置前先检查PositionValid和Error。例如：

IF mover[i].Status.PositionValid AND NOT mover[i].Status.Error THEN
    actualPos := mover[i].Position;
    actualVel := mover[i].Velocity;
ELSE
    // 数据不可信，触发安全间距策略
    actualPos := lastValidPos[i];
    actualVel := 0;
END_IF

这种"先验证、后使用"的习惯，是碰撞避免逻辑可靠运行的基础。

四、速度与加速度的获取

XTS动子的速度并非直接测量，而是由位置对时间微分得到。Tc3 XTS功能库在控制器内部完成这一计算，用户可以直接读取Velocity字段。速度单位通常为mm/s。加速度则是对速度再微分，或由功能库根据位置二阶差分计算，单位为mm/s²。

需要注意的是，微分运算会放大噪声。如果位置信号存在微小抖动，速度值会出现明显波动，加速度波动更大。因此，在读取速度和加速度时，必须关注滤波设置。

典型参数示例：假设EtherCAT周期为1 ms，动子以500 mm/s匀速运动，则每个周期位置增量为0.5 mm。若位置噪声为±0.01 mm，速度噪声约为±10 mm/s，相对误差2%。若加速度由速度差分得到，噪声可能达到±10000 mm/s²，完全淹没真实加速度信号。因此，加速度通常需要更强的滤波或仅用于趋势判断。

五、位置数据滤波方法

TwinCAT中常用的滤波方式有两种：一阶低通滤波和移动平均滤波。一阶低通滤波实现简单，适合实时性要求高的场景：

// 一阶低通滤波示例
filteredPos := filteredPos + alpha * (rawPos - filteredPos);
// alpha = Tc / (Tc + Tf)
// Tc = 周期时间，Tf = 滤波时间常数

假设周期Tc = 1 ms，希望滤波时间常数Tf = 5 ms，则alpha = 1/(1+5) = 0.1667。这意味着每个周期只吸收约16.7%的新数据，输出变化平缓。滤波越强，延迟越大。对于碰撞避免，延迟必须控制在安全距离允许范围内。例如，若动子最大速度2000 mm/s，滤波引入5 ms延迟，则位置滞后约10 mm。安全间距必须大于这个滞后量。

移动平均滤波则对最近N个周期取平均，适合周期性噪声。但N越大，延迟越大。实际工程中，建议先测量原始信号的噪声水平，再选择滤波参数，而不是盲目加大滤波。

实用建议：在调试阶段，可以先将滤波关闭，用TwinCAT Scope记录原始位置、速度、加速度波形，观察噪声幅度和频率。然后逐步增加滤波，直到波形平滑且延迟可接受。记录下此时的alpha值或N值，作为正式参数。

六、数据更新与同步

XTS动子数据的更新与EtherCAT分布式时钟（DC）同步。所有电机模块在同一时钟基准下采样，控制器在同一周期内读取所有动子数据。这意味着不同动子的位置数据具有相同的时间戳，可以直接用于间距计算，不需要额外的时间对齐。

但需要注意，TwinCAT任务周期与EtherCAT周期可能不同。如果PLC任务周期为2 ms，而EtherCAT周期为1 ms，则PLC每两个周期才读取一次动子数据。此时，间距计算使用的数据可能不是最新的。建议将碰撞避免逻辑放在与EtherCAT周期相同的任务中，或至少确保任务周期不大于EtherCAT周期的两倍。

此外，Tc3 XTS功能库提供了GetMoverData或类似接口，可以一次性获取所有动子的位置、速度、状态，减少多次调用带来的不一致风险。在碰撞避免逻辑中，应尽量在同一周期内完成所有动子数据的读取，再统一计算间距。

七、从感知到决策的衔接

获取到可靠的位置、速度、加速度后，下一步才是间距计算和碰撞判断。但感知层的质量直接决定决策层的可靠性。一个常见的误区是：在位置数据尚未稳定时就开始计算间距，导致误报警或漏报警。正确的做法是：在每个控制周期开始时，先检查所有动子的PositionValid和Error，确认数据可信后，再进行间距计算。如果某个动子数据无效，应将其视为"未知位置"，并采取保守策略，如降低所有动子速度或暂停运动。

总结来说，XTS动子位置与速度感知的核心在于：理解编码器反馈到TwinCAT数据的完整链路，学会解析状态字判断数据可信度，掌握速度与加速度的微分计算及其噪声特性，并根据实际噪声水平选择合适的滤波参数。只有感知层可靠，碰撞避免与间距控制才有坚实的基础。