基于 CODESYS 的运动控制：多轴插补参数调试与实时性校验

CODESYS SoftMotion 是中小型多轴设备（3–5 轴点胶、雕铣、SCARA、Delta）的主流方案——NC 通道 + SMC_Interpolator 插补 + SMC_Controller 位置环，挂到 CODESYS Control Runtime（Windows/Linux RT）上跑。插补参数没调好，拐角处过冲、小线段抖动；实时性没验，高速小线段下 Axis 跟丢、轨迹锯齿。下面以 4 轴点胶机（X/Y/Z + 点胶阀，总线伺服用 EtherCAT，IPO 4 ms）为主线。

一、插补参数三件套：V / A / Jerk + LookAhead

SMC_Interpolator 的核心可调参数在 SMC_INTERPOLATOR_PAR 结构体里，决定了"给定轨迹"的光滑程度：

VAR

   interpPar : SMC_INTERPOLATOR_PAR;

END_VAR

interpPar.uiVersion := SMC_INTERPOLATOR_PAR_VERSION;

interpPar.fVelMax  := 200.0;   (* X/Y 平面最大合成速度 200 mm/s *)

interpPar.fAccMax  := 800.0;   (* 最大加速度 *)

interpPar.fJerkMax := 4000.0;  (* 加加速度，拐角平滑度关键 *)

interpPar.nLookAhead := 200;   (* 前瞻段数，小线段密集处要 150+ *)

interpPar.fCornerDist := 0.5;  (* 拐角半径 <0.5mm 仍减速，防过冲 *)

SMC_Interpolator(

   pIntr := ADR(interpolator),

   pPar := ADR(interpPar),

   bEn := TRUE

);

三个调参经验：

• Jerk（加加速度）：Jerk 越小拐角越"软"，但加减速拖得长、效率降；Jerk 大拐角锐利但电机抖。点胶机 200 mm/s 下 Jerk=3000–5000 mm/s³ 是甜点，雕铣要高 Jerk（8000+）保效率，但伺服调教要好

• LookAhead（前瞻）：G 代码小线段（每段 2–5 mm）密集时，LookAhead < 50 会让每段都"刹-加"，速度起不来；点胶/雕铣给 150–300 段，Runtime 能吃的就给够

• CornerDist：拐角小于此值仍按"拐角速度"走（不是停，是降速过拐），防过冲。点胶 0.3–0.8 mm，雕铣 0.1–0.3 mm（精度优先）

二、位置环与控制周期

插补周期是 IPO（4 ms），但伺服本身位置环周期更短（EtherCAT DC 通常 1 ms 或 250 μs），CODESYS 侧位置给定更新率 = IPO 周期，伺服内再做细分。SMC_Controller（或你挂 PID 在 Task）的周期要 ≤ IPO：

(* 运动任务，挂到 4ms Task 上 *)

MotionTask: SMC_Controller;

MotionTask(

   Axis := g_axisX,

   fSetPos := interpolator.fX,   (* 插补给的 X 给定 *)

   fActPos := g_axisX.fActPos,

   fVelMax := 200.0,

   fKp := 80.0, fKi := 0.5, fKd := 2.0,

   Cycle := T#4ms

);

⚠ 4 轴 + 前瞻 200 段时，IPO 4 ms 在 Cortex-A7（RK3568 跑 CODESYS RT）上 CPU 单核约 35%；如果换 IPO 2 ms，单核能到 65%——实时性要留余量，别把周期压到极限。

三、实时性校验：Trace + 抖动看三样

CODESYS 自带 Trace（在线跟踪），是验实时性最好用的工具，不用额外示波器：

1. Axis.fActPos vs Axis.fSetPos：抓跟随误差 e = fSetPos - fActPos，匀速段 e < 0.02 mm（丝杠 5 mm/rev、编码器 20 bit 下），拐角处 e 尖峰 < 0.08 mm

2. IPO 任务周期抖动：Trace 里抓 Task.TimeLast）的 min/max/mean，4 ms 任务抖动应 < ±200 μs（Linux RT 打了 PREEMPT_RT 补丁+isolcpus 的情况下）

3. EtherCAT 帧抖动：抓 SMC_EtherCAT.FrameDelay，DC 同步下帧间抖动 < 100 ns，超 500 ns 说明 CPU 抢占了 EtherCAT 主栈

(* Trace 触发条件：拐角过冲检测 *)

IF ABS(g_axisX.fSetPos - g_axisX.fActPos) > 0.1 THEN

   Trace_Trigger(TRUE);

END_IF

Linux RT 侧的配套（以 RK3568 为例）：

• 内核开 PREEMPT_RT，cat /proc/cmdline 看 isolcpus=3（把 CODESYS Runtime 绑核 3）

• CODESYS Control RTE 的 .cfg 里 CpuAffinity=0x08（对应核 3）

• 关 CPU 调频：echo performance > /sys/devices/system/cpu/cpu3/cpufreq/scaling_governor

四、三个翻车高发点

• LookAhead 给 50、G 代码每段 2 mm：速度永远起不来，4 轴点胶机标称 200 mm/s 实测 60 mm/s，因为每段都在刹-加。LookAhead 按"总路径长度 / 平均段长 × 1.5"估，200 段是底线

• Jerk 太大 + 伺服刚性不够：雕铣拐角处 Axis 抖，Trace 里 e 尖峰 0.15 mm，工件拐角有"锯齿"。Jerk 减半 + 伺服 Kp 降 20% 能消，但效率掉——要平衡

• IPO 4 ms 但 EtherCAT DC 1 ms 不同步：插补给定 4 ms 一刷、伺服 1 ms 一算，没问题，但 DC 的 ShiftTime 要设成 IPO 周期的 1/4（1 ms），否则 DC 同步漂移 → 跟随时而跳

高速小线段（如 3D 打印、激光雕）要换 SMC_Interpolator.HighSpeed=ON + LookAhead 500+，并把 IPO 压到 2 ms（RK3588 / i5 这种能扛），否则 100 mm/s 以上拐角会"切圆变多边形"。

CODESYS 多轴插补的真谛：V/A/Jerk + LookAhead 三件套按"拐角平滑 vs 效率"平衡、IPO 周期 4 ms 起步（高速 2 ms）、Trace 抓跟随误差 + 任务抖动 + EtherCAT DC 三样验实时、Linux RT 要 PREEMPT_RT + isolcpus + 关调频。这套走完 4 轴点胶 200 mm/s、拐角 0.5 mm 不溢胶、Trace 跟误差 < 0.02 mm 匀速 / < 0.08 mm 拐角，雕铣 300 mm/s + 小线段也能跑。