ST数学函数库：SQRT、SIN、COS在运动控制中的计算

ST（Structured Text）是IEC 61131-3标准定义的高级文本编程语言，广泛应用于PLC（可编程逻辑控制器）开发，尤其在运动控制、伺服定位、多轴同步等高精度场景中承担核心计算任务。其内置数学函数库虽简洁，但若理解偏差或调用不当，极易引发定位抖动、速度突变、轨迹失真等严重问题。本文聚焦三个高频且易错的基础函数：SQRT（平方根）、SIN（正弦）、COS（余弦），结合真实运动控制需求，逐层拆解其数值特性、工程约束、典型误用点及安全调用范式，所有内容均可直接用于TIA Portal、Codesys、Automation Studio等主流平台。

一、为什么运动控制特别依赖这三个函数？

运动控制的本质是将目标位置/速度/加速度，通过物理模型映射为驱动器可执行的电信号。该过程常涉及：

• 圆弧插补：需实时计算 X = R * COS(θ)、Y = R * SIN(θ)
• 向量分解：如将合速度 V 按角度 α 分解为 Vx = V * COS(α)、Vy = V * SIN(α)
• 加速度限幅：根据当前速度 v 和最大加速度 a_max 计算剩余安全距离 s = v² / (2 * a_max) → 关键含 SQRT 的逆运算
• 相位同步：多轴间保持固定相位差（如 θ₂ = θ₁ + π/2），SIN/COS 互换实现正交信号

这些计算均要求：
✅ 毫秒级响应（ST函数为原生指令，无调用开销）
✅ 确定性结果（无浮点异常中断，如除零、溢出）
✅ 输入域严格受控（否则返回无效值 NaN 或 ±INF，导致后续逻辑崩溃）

二、SQRT：平方根函数——安全调用的生死线

SQRT(x: REAL): REAL 计算非负实数 x 的算术平方根。其危险性在于：PLC不自动校验输入符号，负数输入直接触发未定义行为。

常见误用场景与后果

安全调用四步法（必须逐条执行）

1. 前置符号净化：对所有可能含符号的变量，强制取绝对值

   v_abs := ABS(v_current); // 非negate，是ABS！

2. 分母零保护：对除法操作单独设最小阈值

   a_limited := MAX(0.001, ABS(a_actual)); // 最小有效加速度0.001 m/s²

3. 输入域钳位：明确限定 SQRT 输入范围

   x_clamped := MAX(0.0, v_abs * v_abs / (2.0 * a_limited));

4. 结果有效性检查：SQRT 返回后立即验证

   s_safe := SQRT(x_clamped);
   IF NOT (s_safe > 0.0 AND s_safe < 1E6) THEN
       s_safe := 0.0; // 或触发报警标志
   END_IF;

✅ 正确范式：s_safe := SQRT(MAX(0.0, (ABS(v_current))^2 / (2.0 * MAX(0.001, ABS(a_cmd)))));
❌ 禁止范式：s_safe := SQRT(v_current^2 / (2.0 * a_cmd));（无任何防护）

三、SIN/COS：三角函数——角度制陷阱与周期性风险

SIN(x: REAL): REAL 和 COS(x: REAL): REAL 的输入 x 单位为弧度（radians），而非工程惯用的度（°）。这是90%初学者调试失败的根源。

关键事实清单

• SIN(π/2) = 1.0，但 SIN(90.0) ≈ 0.894（因90.0被解释为90弧度 ≈ 5156.6°，即 5156.6 mod 360 = 236.6°，sin(236.6°) ≈ -0.83）
• π 在ST中无内置常量，必须手动定义：PI := 3.14159265358979323846;
• SIN/COS 对大角度输入存在精度衰减：当 |x| > 1E6 弧度时，多数PLC库因模 2π 计算误差，结果偏差可达 1E-3 量级

运动控制中的典型应用与防护

场景1：圆弧插补坐标计算
目标：以圆心 (cx, cy)、半径 R=100.0、起始角 θ_start=30°、终止角 θ_end=120° 插补圆弧
错误写法：

theta := 30.0; // 度数！
x := cx + R * COS(theta); // 错！COS(30) ≠ cos(30°)

正确写法：

PI := 3.14159265358979323846;
theta_deg := 30.0;
theta_rad := theta_deg * PI / 180.0; // 显式转弧度
x := cx + R * COS(theta_rad);
y := cy + R * SIN(theta_rad);

场景2：相位同步中的周期性溢出
多轴协同时，主轴位置 pos_master 以脉冲计数，需生成相位偏移 φ = 90° 的从轴信号：

// 危险：pos_master 增大至1E9时，theta_raw = pos_master * K 超出精度范围
theta_raw := pos_master * 0.017453292519943; // 1°=0.01745...rad，累加后易超
phase_shifted := SIN(theta_raw + PI/2); // 结果漂移

安全方案：

// 方案A：模2π截断（推荐）
theta_norm := MOD(theta_raw, 2.0 * PI);
IF theta_norm < 0.0 THEN theta_norm := theta_norm + 2.0 * PI; END_IF;
phase_shifted := SIN(theta_norm + PI/2);

// 方案B：增量式计算（更高精度）
// 每次只计算微小角度增量 Δθ，避免大数累加
delta_theta := delta_pos * K; // delta_pos为本次位置变化量
theta_cum := theta_cum + delta_theta;
theta_cum := MOD(theta_cum, 2.0 * PI); // 每次更新后立即归一化

四、组合应用：直线-圆弧连续轨迹的平滑过渡算法

工业机器人画圆时，常需从直线段无缝切入圆弧段。关键在曲率连续（即切线方向一致），需精确计算切入点处的法向量。

已知条件：

• 直线终点 P1 = (x1, y1)，方向向量 D = (dx, dy)
• 圆弧圆心 C = (cx, cy)，半径 R
• 要求：找到圆弧上离 P1 最近的点 P2，且 P1→P2 与圆弧在 P2 处的切线垂直

数学推导：
向量 CP2 为半径向量，必与切线垂直，故 CP2 与直线方向 D 平行。设 CP2 = k * D，则：
$$ |CP2| = R \Rightarrow |k| \cdot \sqrt{dx^2 + dy^2} = R \Rightarrow |k| = \frac{R}{\sqrt{dx^2 + dy^2}} $$
取 k 符号使 P2 位于 P1 向圆心一侧：
$$ k = \text{SIGN}((cx - x1) \cdot dx + (cy - y1) \cdot dy) \cdot \frac{R}{\sqrt{dx^2 + dy^2}} $$
则 P2 = C - k * D

ST安全实现：

// 1. 计算方向向量模长（含SQRT防护）
len_d_sq := dx * dx + dy * dy;
len_d := SQRT(MAX(0.0, len_d_sq)); // 防负输入

// 2. 防零除：若直线退化为点，跳过计算
IF len_d < 1E-6 THEN
    p2_x := cx; p2_y := cy; // 退化处理
ELSE
    // 3. 计算k的符号：点积判断方向
    dot_product := (cx - x1) * dx + (cy - y1) * dy;
    k_sign := 1.0;
    IF dot_product < 0.0 THEN k_sign := -1.0; END_IF;

    // 4. 计算k（含分母保护）
    k := k_sign * R / MAX(1E-6, len_d); // 防len_d=0

    // 5. 输出P2坐标
    p2_x := cx - k * dx;
    p2_y := cy - k * dy;
END_IF;

此算法全程规避了 SIN/COS，仅用 SQRT 和基础四则，却实现了几何上最严格的过渡点计算。

五、性能与精度实测对比（基于Siemens S7-1500）

在1ms任务周期内，连续调用1000次各函数，记录平均执行时间与最大误差：

结论：

• SQRT/SIN/COS 本身性能卓越，瓶颈在输入数据质量；
• 误差主要源于大角度输入未归一化和负数输入未拦截；
• 所有防护措施（钳位、ABS、MAX）增加耗时 < 15ns，远低于1ms周期裕量。

六、终极检查清单（部署前必做）

1. SQRT输入：是否存在任何路径可能传入负数？是否对 v² 类表达式使用 ABS？
2. SIN/COS输入：所有角度变量名是否含 _rad 后缀（如 theta_rad）？是否存在 *180/PI 混淆？
3. 大数归一化：所有累计角度变量是否每周期执行 MOD(theta, 2*PI)？
4. 零值防护：所有分母是否用 MAX(ε, ABS(x)) 代替直接 x？
5. 结果验证：SQRT 后是否检查 > 0？SIN/COS 后是否检查 >= -1.0 AND <= 1.0？

// 一行式终极防护模板（可复用）
safe_sqrt := 
    IF x < 0.0 THEN 0.0 
    ELSIF x > 1E12 THEN 1E6 
    ELSE SQRT(x) 
    END_IF;

safe_sin := 
    IF theta_abs > 1E6 THEN 
        SIN(MOD(theta_abs, 2.0*PI)) 
    ELSE 
        SIN(theta_abs) 
    END_IF;