2. 直线插补速度规划

任务目标

在直线插补过程中，若各插补周期位移恒定，机械臂启停瞬间会产生加速度突变，造成冲击。本案例为直线插补添加梯形速度规划：给定起始点、终点、最大速度、加速度与插补周期，计算加速段、匀速段、减速段的持续时间，并输出每个插补点的时间戳与位置。

核心实现

import math

def trapezoid_plan(p0, p1, v_max, a, T):
    """
    梯形速度规划直线插补
    p0, p1: 起点/终点坐标 (x, y)
    v_max : 最大速度 (mm/s)
    a     : 加速度 (mm/s^2)
    T     : 插补周期 (s)
    返回: [(t, x, y), ...] 每个插补点的时间戳与位置
    """
    # 1. 计算总位移与方向
    dx = p1[0] - p0[0]
    dy = p1[1] - p0[1]
    dist = math.hypot(dx, dy)
    if dist < 1e-9:
        return [(0.0, p0[0], p0[1])]

    ux, uy = dx / dist, dy / dist

    # 2. 梯形速度规划（标量位移s ∈ [0, dist]）
    #    加速段: s = 0.5*a*t^2, v = a*t
    #    匀速段: v = v_max
    #    减速段: s = dist - 0.5*a*(t_total - t)^2
    t_acc = v_max / a                # 加速到v_max所需时间
    s_acc = 0.5 * a * t_acc * t_acc  # 加速段位移

    if 2 * s_acc >= dist:
        # 无法达到v_max，三角形速度曲线
        t_acc = math.sqrt(dist / a)
        t_total = 2 * t_acc
        s_acc = dist / 2
        has_cruise = False
    else:
        # 梯形速度曲线
        s_cruise = dist - 2 * s_acc
        t_cruise = s_cruise / v_max
        t_total = 2 * t_acc + t_cruise
        has_cruise = True

    # 3. 按插补周期采样
    points = []
    t = 0.0
    while t <= t_total + 1e-9:
        if t <= t_acc:
            s = 0.5 * a * t * t
        elif has_cruise and t <= t_acc + t_cruise:
            s = s_acc + v_max * (t - t_acc)
        else:
            t_rem = t_total - t
            s = dist - 0.5 * a * t_rem * t_rem

        # 防止浮点误差越界
        s = max(0.0, min(dist, s))
        points.append((round(t, 6), round(p0[0] + s * ux, 6), round(p0[1] + s * uy, 6)))
        t += T

    # 确保终点被包含
    if abs(points[-1][0] - t_total) > 1e-6:
        points.append((round(t_total, 6), p1[0], p1[1]))

    return points

# 示例：从(0,0)到(100,0)，v_max=50mm/s，a=100mm/s^2，T=0.01s
if __name__ == "__main__":
    pts = trapezoid_plan((0, 0), (100, 0), 50, 100, 0.01)
    print(f"插补点数: {len(pts)}")
    print("前5个点:", pts[:5])
    print("最后3个点:", pts[-3:])
    # 验证：相邻点位移增量不超过 v_max*T = 0.5mm
    max_step = 0.0
    for i in range(1, len(pts)):
        step = math.hypot(pts[i][1] - pts[i-1][1], pts[i][2] - pts[i-1][2])
        max_step = max(max_step, step)
    print(f"最大单步位移: {max_step:.4f} mm (理论上限 0.5mm)")

运行与验证

运行方式：直接执行 python trapezoid_plan.py（需Python 3，无需第三方库）。

验证方法：

• 程序输出插补点总数与首尾若干点坐标，终点必须精确为(100, 0)。
• 计算相邻点间距，最大单步位移应不超过 v_max × T = 0.5mm，且首末两步位移接近 0.5 × a × T² = 0.005mm，体现加减速过程。
• 时间戳从0递增至总时间 t_total，总时间满足 t_total = 2×(v_max/a) + (dist - v_max²/a)/v_max = 2×0.5 + (100-25)/50 = 2.5s。

预期输出（示例）：插补点数251，首点(0,0,0)，末点(2.5,100,0)，最大单步位移约0.5mm。

关键说明

• 规划在标量位移 s 上进行，再映射回x/y坐标，保证轨迹为直线。
• 当位移过短无法达到最大速度时，自动退化为三角形速度曲线（代码中 has_cruise=False 分支）。
• 插补周期采样后强制追加终点，避免因周期不整除导致终点遗漏。

上图红色折线为速度曲线（梯形），绿色虚线为对应的位移曲线。加速段与减速段斜率由加速度 a 决定，匀速段速度为 v_max，插补点时间戳由周期 T 在总时间轴上等距采样获得。