3. 运动控制基础

运动控制是RTOS插补算法的物理载体与执行基础。在深入插补器设计之前，必须明确被控对象的几何描述方式（坐标系与轴定义）、运动学约束（速度、加速度、加加速度）以及伺服环路的控制模式。这三者决定了插补算法输出的目标值格式、周期以及底层执行器的响应特性。本章将系统梳理这些基础概念，并重点说明它们与RTOS任务调度、插补周期设计之间的直接关联。

3.1 坐标系与轴定义：插补空间的几何基础

3.1.1 核心原理：坐标系的层级与变换

在运动控制系统中，坐标系是描述刀具或工件位置的参考框架。RTOS插补器输出的每一个目标点，都必须在一个统一的坐标系下表达，否则各轴的运动无法合成预期的轨迹。常见的坐标系层级包括：

• 机床坐标系（MCS）：由机床原点（Home点）确定，是参考系。所有轴的机械限位、回零操作均基于此坐标系。
• 工件坐标系（WCS）：编程人员设定的坐标系，通常与工件的装夹基准重合。G54~G59等指令即用于选择不同的WCS。
• 用户坐标系（UCS）：在WCS基础上进一步偏移或旋转，用于多工位或倾斜面加工。
• 工具坐标系（TCS）：定义刀具中心点（TCP）相对于主轴法兰的位置，用于刀尖跟随（TCPC）功能。

插补器在生成轨迹时，首先将编程坐标（WCS/UCS）通过齐次变换矩阵（HTM）转换到MCS，再分解为各物理轴的指令。RTOS中的坐标变换任务通常以固定周期执行，变换矩阵的更新频率与插补周期一致。

3.1.2 轴定义与映射关系

轴定义是将逻辑轴（X、Y、Z、A、B、C）映射到实际伺服驱动器通道的过程。在RTOS运动控制软件中，轴定义通常包含以下数据结构：

typedef struct {
    uint8_t     axis_id;        // 逻辑轴号 0~N-1
    uint8_t     drive_ch;       // 驱动器通道号
    uint8_t     encoder_ch;     // 编码器反馈通道
    float       gear_ratio;     // 电子齿轮比
    float       pitch;          // 丝杠导程 (mm/rev)
    float       max_vel;        // 最大速度 (mm/s)
    float       max_acc;        // 最大加速度 (mm/s²)
    float       max_jerk;       // 最大加加速度 (mm/s³)
    AxisType    type;           // LINEAR / ROTARY / VIRTUAL
} AxisConfig_t;

对于龙门同步轴（如双X轴驱动），需要定义主从关系，插补器只计算主轴的指令，从轴通过电子同步或交叉耦合控制跟随。RTOS中，同步轴的控制任务优先级通常高于普通轴，以保证同步误差在允许范围内。

3.1.3 案例拆解：五轴机床的坐标系变换链路

以AC双摆头五轴机床为例，插补器输出的每个插补点包含X、Y、Z、A、C五个坐标值。实际控制流程如下：

1. 输入：G代码中的刀位点（WCS下的X、Y、Z）和刀轴矢量（I、J、K）。
2. 变换：通过后处理或RTOS内的坐标变换模块，将刀轴矢量转换为A轴和C轴的旋转角度。
3. 映射：将WCS下的X、Y、Z加上旋转轴引起的平移补偿，得到MCS下的各轴位置。
4. 输出：插补器按插补周期（如1ms）输出各轴的目标位置到位置控制模式。

该案例中，RTOS需要管理两个任务：坐标变换任务（优先级高，周期1ms）和插补计算任务（优先级相同，但相位错开）。若坐标变换任务超时，会导致插补点错误，造成轨迹偏差。

3.2 速度、加速度与加加速度：运动学约束的数学表达

3.2.1 核心原理：S曲线规划的必要性

插补器不仅要生成轨迹形状，还要保证运动过程满足动力学约束。速度（Velocity）、加速度（Acceleration）、加加速度（Jerk）是描述运动平滑度的三个关键参数：

• 速度 V：单位时间内位置的变化量，决定加工效率。
• 加速度 A：速度的变化率，影响惯性力大小，受伺服电机峰值力矩限制。
• 加加速度 J：加速度的变化率，直接影响机械振动和轮廓误差。J值过大，机床会明显抖动；J值过小，加减速时间延长。

梯形速度规划（仅限制V和A）在加减速阶段存在加速度突变，导致加加速度无穷大，不适合高精度加工。S曲线规划（限制V、A、J）通过限制J值，使加速度曲线连续变化，从而减小振动。

3.2.2 判断方法与参数计算

在RTOS插补器中，运动学约束用于前瞻规划（Look-ahead）模块。核心判断逻辑如下：

1. 读取约束：从轴配置中获取Vmax、Amax、Jmax。
2. 计算可行速度：对于每个插补段，根据段长L和两端速度，计算满足A和J约束的最大可达速度。
3. 速度前瞻：扫描后续N段轨迹，找出速度谷点（如拐角处），反向规划减速曲线。
4. 输出速度曲线：生成每个插补周期的目标速度，再积分得到位置指令。

关键公式（以S曲线加速段为例）：

// 加加速度阶段 (Jerk phase)
v(t) = v0 + 0.5 * J * t²
a(t) = J * t

// 匀加速阶段 (Constant acceleration phase)
v(t) = v1 + A * t
a(t) = A

// 减加加速度阶段 (De-jerk phase)
v(t) = v2 + A * t - 0.5 * J * t²
a(t) = A - J * t

3.2.3 案例拆解：高速加工中的Jerk限制

某模具加工中心，X轴最大速度Vmax=200mm/s，Amax=2000mm/s²，Jmax=50000mm/s³。若采用梯形规划，从0加速到200mm/s需0.1s，加速度突变产生冲击。采用S曲线规划后：

实际调试中，Jmax并非越大越好。当Jmax超过机械系统的固有频率时，会激发共振。通常通过频谱分析确定机床一阶固有频率，将Jmax对应的激励频率避开该频段。

3.3 位置/速度/力矩控制模式：伺服环路的接口选择

3.3.1 核心原理：三种模式的本质区别

伺服驱动器通常支持三种控制模式，插补器需要根据应用场景选择输出指令的类型：

• 位置模式（周期位置指令）：插补器输出每个周期的目标位置（如脉冲数或总线位置值）。驱动器内部完成位置环、速度环、电流环的闭环。适用于绝大多数点位控制和连续轨迹控制。
• 速度模式（周期速度指令）：插补器输出目标速度（模拟量或总线速度值）。驱动器内部完成速度环和电流环，位置环由上位机（RTOS）完成。适用于需要高动态响应的场合，或上位机需要直接控制速度的场合。
• 力矩模式（电流指令）：插补器输出目标力矩（电流值）。驱动器仅完成电流环。适用于力控制、压铸、打磨等对力有直接要求的场景。

3.3.2 判断方法与选择依据

选择控制模式需考虑以下因素：

3.3.3 案例拆解：RTOS中模式切换的实时性

在复杂应用中，可能需要动态切换控制模式。例如，在机器人打磨场景中，法向力控制需要力矩模式，而轨迹跟踪需要位置模式。切换过程必须平滑，否则会产生冲击。

// RTOS中模式切换任务（周期1ms）
void ModeSwitchTask(void) {
    if (force_control_enabled) {
        // 平滑过渡：当前位置→速度→力矩
        if (current_mode == POSITION_MODE) {
            // 先切换为速度模式，并输出当前速度
            SetDriveMode(SPEED_MODE);
            SetSpeedCmd(current_velocity);
            current_mode = SPEED_MODE;
        } else if (current_mode == SPEED_MODE) {
            // 再切换为力矩模式，并输出当前力矩
            SetDriveMode(TORQUE_MODE);
            SetTorqueCmd(current_torque);
            current_mode = TORQUE_MODE;
        }
    }
}

该案例中，RTOS通过状态机管理切换过程，每个状态停留数个周期，确保驱动器内部环路稳定。切换期间，插补器暂停输出新轨迹，等待切换完成标志。

3.4 常见误区与总结复盘

3.4.1 误区清单

• 误区1：坐标系变换与插补混用同一任务。导致变换耗时过长，插补周期抖动，轨迹不连续。
• 误区2：Jerk值设置过大或过小。过大引起振动，过小降低效率。应通过实际测试或仿真确定。
• 误区3：所有轴使用相同的运动学参数。不同轴的机械特性不同，应独立配置Vmax、Amax、Jmax。
• 误区4：忽视控制模式切换的过渡过程。直接切换会导致驱动器报错或机械冲击。

3.4.2 总结复盘

本章内容为RTOS插补算法提供了三个基础支撑：坐标系与轴定义确定了插补输出的目标空间；速度/加速度/加加速度约束决定了插补器如何规划运动过程；控制模式则明确了插补器与驱动器之间的接口协议。在后续章节中，插补算法（如直线、圆弧、样条插补）将基于这些基础进行设计。实际工程中，建议在RTOS中建立统一的轴配置管理模块，将坐标系变换、运动学参数、控制模式封装为独立组件，便于维护和调试。