运动控制卡底层原理与多轴控制工程实战#

1.1 DSP 与 FPGA 的职责划分#

• DSP（如 TI TMS320C6000 / C2837x 系列）：
  • 负责周期性离散插补运算（插补周期一般为 $250\,\mu\text{s}$、$500\,\mu\text{s}$ 或 $1\,\text{ms}$）。
  • 执行 7 段 S 型加减速曲线递推解算、多轴笛卡尔空间逆解（如 SCARA、Delta 机构）。
  • 运行位置环 PID + 速度/加速度前馈控制算法。
• FPGA（如 Xilinx Spartan / Artix 系列）：
  • 脉冲发生器（PGA）：实现多轴独立或同步的 PUL/DIR、CW/CCW 脉冲发生，最高频率可达 5MHz~10MHz。
  • 正交解码与计数器（QEI）：对接收到的编码器 A/B 相相位差 $90^\circ$ 的差分信号进行 4 倍频解码、硬件滤波与 32 位双向计数。
  • 硬件比较器（PSO）：将 32 位编码器实时计数器与比较寄存器阵列进行硬件级数值匹配，匹配成功瞬间通过 IO 口直接拉高信号，延时通常 $< 50\,\text{ns}$。

1.2 关键接口电路与电气特性#

1. 差分脉冲与方向输出电路：
   • 采用 RS-422 标准差分驱动芯片（如 AM26C31）。在长距离线缆传输时，通过双绞线形成模反相信号（$\text{PULSE+}$ 与 $\text{PULSE-}$），有效抵消共模干扰。
2. 正交编码器输入电路：
   • 接收端采用 AM26C32 差分接收芯片，后接 RC 低通滤波与 Schmitt 触发器，滤除电磁噪声造成的毛刺脉冲。
3. 数字量 I/O 光耦隔离电路：
   • 原点（HOME）、正/负限位（LIMIT+/LIMIT-）及通用数字量使用高速光耦（如 6N137，传输延迟约 75ns）隔离 24V 工业现场信号与 3.3V/5V 板卡逻辑电平。

2.1 脉冲当量计算公式#

$\text{Pulse Equivalent (脉冲当量)} = \frac{\Delta x}{\Delta N} \quad (\text{mm/pls})$

对于螺杆传动系统： $P = \frac{L}{R \cdot N_{\text{enc}}}$

• $L$：滚珠丝杠导程（Pitch，单位：mm/rev）。
• $R$：减速比（电机转速 / 轴转速，无减速机时为 1）。
• $N_{\text{enc}}$：伺服电机编码器分辨率（脉冲/圈，若使能 4 倍频则为编码器线数 $\times 4$）。

计算示例： 若丝杠导程 $L = 10\,\text{mm}$，减速比 $R = 1$，伺服编码器为 17 位绝对值编码器（$2^{17} = 131,072\,\text{pls/rev}$），则： $P = \frac{10}{131072} \approx 0.0000762939\,\text{mm/pls}$

即上位机要求移动 $1\,\text{mm}$，控制卡需发送 $13107.2$ 个脉冲。

3.1 7 段 S 型加减速曲线的连续数学模型#

S型加减速将阶段划分为 7 段：

1. 加加速段 ($[0, t_1]$)：加速度 $a(t)$ 从 $0$ 线性增加到 $a_{\max}$，加加速度 $j(t) = J_{\max}$。
2. 匀加速段 ($[t_1, t_2]$)：$a(t) = a_{\max}$，$j(t) = 0$。
3. 减加速段 ($[t_2, t_3]$)：$a(t)$ 从 $a_{\max}$ 线性减小到 $0$，$j(t) = -J_{\max}$。
4. 匀速段 ($[t_3, t_4]$)：$v(t) = v_{\max}$，$a(t) = 0$，$j(t) = 0$。
5. 加减速段 ($[t_4, t_5]$)：$a(t)$ 从 $0$ 线性增加到 $-a_{\max}$，$j(t) = -J_{\max}$。
6. 匀减速段 ($[t_5, t_6]$)：$a(t) = -a_{\max}$，$j(t) = 0$。
7. 减减速段 ($[t_6, t_7]$)：$a(t)$ 从 $-a_{\max}$ 线性增加到 $0$，$j(t) = J_{\max}$。

3.2 离散插补周期的递推计算公式#

在每一个插补采样周期 $k$（时间时刻 $t_k = k \cdot T_s$）中：

1. 更新加速度： $a_k = a_{k-1} + j_k \cdot T_s$
2. 更新速度： $v_k = v_{k-1} + a_k \cdot T_s + \frac{1}{2} j_k \cdot T_s^2$
3. 计算位置增量： $\Delta p_k = v_{k-1} \cdot T_s + \frac{1}{2} a_k \cdot T_s^2 + \frac{1}{6} j_k \cdot T_s^3$
4. 当前目标位置： $p_k = p_{k-1} + \Delta p_k$

控制卡将解算出的 $\Delta p_k$ 转换为该插补周期内 FPGA 需发出的脉冲个数或 EtherCAT 报文的目标位置（Target Position）。

4.1 CiA 402 核心对象字典 (Object Dictionary)#

• 0x6040 (Controlword)：主站控制字，用于切换伺服状态机。
• 0x6041 (Statusword)：从站状态字，反映驱动器当前运行状态。
• 0x6060 (Mode of Operation)：运行模式设置（6: 周期同步位置 CSP 模式；1: PP 位置模式；3: PV 速度模式；6: 回零 Homing 模式）。
• 0x607A (Target Position)：周期同步位置模式（CSP）下，主站每周期发送的目标位置。
• 0x606C (Velocity Actual Value)：实际运行速度。
• 0x6077 (Torque Actual Value)：实际转矩负载率。

4.2 CiA 402 驱动器使能状态机流程#

驱动器上电后必须按照规定的状态转变顺序写入 0x6040 控制字：

1
[ Not Ready to Switch On ]
2
           │ (自动过渡)
3
           ▼
4
[ Switch On Disabled ] ──(写 Controlword = 0x0006)──► [ Ready to Switch On ]
5
                                                               │
6
                                                       (写 Controlword = 0x0007)
7
                                                               │
8
                                                               ▼
9
[ Operation Enabled (伺服使能就绪) ] ◄──(写 Controlword = 0x000F)─── [ Switched On ]

1
// C# CiA 402 状态机使能示例代码
2
public bool EnableServo(ushort slaveIndex)
3
{
4
    ushort status = ReadSDO<ushort>(slaveIndex, 0x6041, 0x00);
5

6
    // 检查是否处于 Switch On Disabled 状态 (xxxx xxxx x0xx 0000)
7
    if ((status & 0x004F) == 0x0040)
8
    {
9
        WriteSDO<ushort>(slaveIndex, 0x6040, 0x00, 0x0006); // 转换到 Ready to Switch On
10
        Thread.Sleep(20);
11
    }
12

13
    status = ReadSDO<ushort>(slaveIndex, 0x6041, 0x00);
14
    if ((status & 0x006F) == 0x0021)
15
    {
16
        WriteSDO<ushort>(slaveIndex, 0x6040, 0x00, 0x0007); // 转换到 Switched On
17
        Thread.Sleep(20);
18
    }
19

20
    status = ReadSDO<ushort>(slaveIndex, 0x6041, 0x00);
21
    if ((status & 0x006F) == 0x0023)
22
    {
23
        WriteSDO<ushort>(slaveIndex, 0x6040, 0x00, 0x000F); // 使能成功 (Operation Enabled)
24
        return true;
25
    }
26
    return false;
27
}

5.1 PSO 硬件原理与偏差计算#

当运动轴以速度 $v = 500\,\text{mm/s}$ 移动时，若软件触发延迟 $\tau = 2\,\text{ms}$，产生的空间位置偏差为：

$\Delta x = v \cdot \tau = 500\,\text{mm/s} \times 0.002\,\text{s} = 1.0\,\text{mm}$

这在微米级 AOI 检测中是不可接受的。

PSO 利用 FPGA 硬件比较器直接监控编码器 AB 相计数：

$\text{Comparator Match Trigger} \Longrightarrow \text{Pulse Output (Delay } < 50\,\text{ns})$

硬件触发的时序偏差为： $\Delta x_{\text{hardware}} = 500\,\text{mm/s} \times 50 \times 10^{-9}\,\text{s} = 0.025\,\mu\text{m}$

5.2 线性比较与表比较#

• 等间距线性比较（Linear PSO）： 设置起始位置 $P_{\text{start}}$，触发间隔 $\Delta P$，比较次数 $N$。当编码器位置累加到 $P_{\text{start}} + k \cdot \Delta P$ 时硬件自动触发脉冲。
• 非等间距表比较（Table PSO）： 将任意非定距坐标数组预先写入控制卡的内存 FIFO 缓冲区中，FPGA 依次加载下一个目标坐标进行比较。

6.1 位置环 PID 与前馈 (Feedforward) 调谐#

在控制卡的 PID 控制回路中：

$u(t) = K_p e(t) + K_i \int e(t)dt + K_d \frac{de(t)}{dt} + V_{ff} \cdot v_{\text{cmd}}(t) + A_{ff} \cdot a_{\text{cmd}}(t)$

• 比例增益 $K_p$：决定系统的响应刚度。$K_p$ 过小会导致跟随误差（Following Error）增大；过大会引起机械发尖锐哮叫和高频振荡。
• 速度前馈 $V_{ff}$：根据当前周期的规划速度指令直接叠加输出，不依赖跟随误差。合理配置 $V_{ff}$（通常调至 90%~99%）可以显著减小匀速段的跟随误差。
• 加速度前馈 $A_{ff}$：在加减速阶段直接补充克服惯性所需的转矩，显著减小加减速阶段的动态滞后。

6.2 反向间隙 (Backlash) 与螺距误差补偿#

1. 反向间隙补偿： 由于滚珠丝杠与轴承存在机械间隙，当轴改变运动方向时，电机旋转但工作台短时间内不移动。
   • 补偿方法：测定反向间隙量 $\delta$（如 $0.015\,\text{mm}$）。控制卡在检测到方向切换指令的第一个插补周期，自动在输出脉冲中叠加 $\delta$ 对应的脉冲量。
2. 螺距误差补偿（Pitch Error Compensation）： 使用双频激光干涉仪沿轴全行程按固定步距（如 $10\,\text{mm}$）测量实际位置误差。
   • 生成螺距补偿表加载至控制卡驱动中。控制卡在运动过程中实时插值查表，自动修正脉冲输出。