3. 直线插补加减速控制

任务目标

在直线插补过程中，控制运动轴从起点到终点时速度按梯形曲线变化：匀加速、匀速、匀减速三段。给定总位移、最大速度、加速度，计算各阶段位移与时间，并输出每个插补周期（如1ms）的目标位置序列。

核心算法

梯形速度曲线规划的关键是判断是否达到最大速度。设总位移为 S，最大速度为 Vmax，加速度为 A（加减速值相同）。若 S < Vmax² / A，则无法达到最大速度，实际峰值速度 Vp = sqrt(S·A)，此时只有加速和减速段；否则 Vp = Vmax，包含匀速段。加速段位移 Sa = Vp²/(2A)，减速段位移与加速段相同，匀速段位移 Sc = S - 2·Sa。

完整实现

import math

def trapezoidal_profile(S, Vmax, A, T_cycle=0.001):
    """
    梯形加减速直线插补规划
    参数:
        S: 总位移 (单位: mm 或 pulse)
        Vmax: 最大速度 (单位/s)
        A: 加速度 (单位/s²)
        T_cycle: 插补周期 (s)
    返回:
        (positions, times, phases)
        positions: 每个周期的目标位置列表
        times: 对应时间点列表
        phases: 每段对应的阶段标签 ('accel'/'cruise'/'decel')
    """
    # 判断是否可达最大速度
    S_accel_min = Vmax * Vmax / (2 * A)  # 加速到Vmax所需位移
    if S < 2 * S_accel_min:
        # 三角形速度曲线：达不到最大速度
        Vp = math.sqrt(S * A)
        Sa = S / 2
        t_accel = Vp / A
        t_total = 2 * t_accel
        t_cruise_start = t_accel
        t_cruise_end = t_accel  # 无匀速段
    else:
        # 梯形速度曲线
        Vp = Vmax
        Sa = S_accel_min
        t_accel = Vmax / A
        Sc = S - 2 * Sa
        t_cruise = Sc / Vmax
        t_total = 2 * t_accel + t_cruise
        t_cruise_start = t_accel
        t_cruise_end = t_accel + t_cruise

    # 生成插补点
    positions = []
    times = []
    phases = []
    t = 0.0
    pos = 0.0
    while t <= t_total + 1e-9:
        if t < t_accel:
            # 加速段: s = 0.5*A*t²
            pos = 0.5 * A * t * t
            phase = 'accel'
        elif t < t_cruise_end:
            # 匀速段: s = Sa + Vp*(t - t_accel)
            pos = Sa + Vp * (t - t_accel)
            phase = 'cruise'
        else:
            # 减速段: s = S - 0.5*A*(t_total - t)²
            t_remain = t_total - t
            pos = S - 0.5 * A * t_remain * t_remain
            phase = 'decel'
        positions.append(pos)
        times.append(t)
        phases.append(phase)
        t += T_cycle

    # 确保终点精确
    if positions:
        positions[-1] = S
    return positions, times, phases

# 最小调用示例
if __name__ == "__main__":
    S = 100.0      # 总位移 100mm
    Vmax = 200.0   # 最大速度 200mm/s
    A = 500.0      # 加速度 500mm/s²
    T = 0.001      # 1ms插补周期

    pos_list, time_list, phase_list = trapezoidal_profile(S, Vmax, A, T)

    # 输出关键规划结果
    print(f"总点数: {len(pos_list)}")
    print(f"首点: pos={pos_list[0]:.4f}, t={time_list[0]:.4f}")
    print(f"末点: pos={pos_list[-1]:.4f}, t={time_list[-1]:.4f}")

    # 验证加减速对称性
    accel_end = max(i for i, p in enumerate(phase_list) if p == 'accel')
    decel_start = min(i for i, p in enumerate(phase_list) if p == 'decel')
    print(f"加速段结束于索引 {accel_end}, 位置 {pos_list[accel_end]:.4f}")
    print(f"减速段开始于索引 {decel_start}, 位置 {pos_list[decel_start]:.4f}")
    print(f"中点位置: {pos_list[len(pos_list)//2]:.4f} (理论中点 {S/2:.4f})")

运行与验证

直接运行上述代码（Python 3，无需额外依赖）。程序会输出插补总点数、首末点位置及加减速切换点。验证通过条件：

• 末点位置精确等于总位移 S（100.0）
• 加速段结束位置与减速段开始位置关于中点对称，即两者之和等于 S
• 中点位置接近理论中点 S/2（因离散化存在≤半个周期的误差）
• 相邻点位移差（即瞬时速度）先增后减，且峰值不超过 Vmax

关键说明

本实现采用时间基插补，每个周期计算一次位置。实际工程中需将位置增量 Δpos = pos[i] - pos[i-1] 转换为脉冲频率或速度指令发送给驱动器。若需更高精度，可改用位置增量累加方式避免浮点累积误差。对于三角形曲线（无法达到最大速度）的情况，代码已自动处理，无需额外分支调用。