4. 梯形速度规划下的直线插补

任务目标

在笛卡尔空间直线插补中引入梯形速度规划，给定起点、终点、最大速度、加速度与插补周期，计算每个插补周期内的位移增量与插补点坐标，使机器人末端沿直线运动且速度曲线呈梯形（加速—匀速—减速）。

实现要点

梯形速度规划的关键是确定加速段、匀速段、减速段的时间边界。设总位移为L，最大速度为v_max，加速度为a。若v_max² / a ≤ L，则能达到最大速度，存在匀速段；否则为三角形速度曲线（无匀速段）。每个插补周期T内，根据当前时刻所处阶段计算位移增量，再按直线方向比例映射到各坐标轴。

完整代码

import numpy as np

def trapezoid_velocity_profile(L, v_max, a, T):
    """
    梯形速度规划，返回每个插补周期的位移增量列表。
    若无法达到v_max，自动退化为三角形速度曲线。
    """
    # 加速到v_max所需时间与位移
    t_acc = v_max / a
    s_acc = 0.5 * a * t_acc**2

    # 判断是否存在匀速段
    if 2 * s_acc >= L:
        # 三角形曲线：加速到峰值后立即减速
        t_acc = np.sqrt(L / a)
        t_total = 2 * t_acc
        s_cruise = 0.0
        t_cruise = 0.0
    else:
        # 梯形曲线
        s_cruise = L - 2 * s_acc
        t_cruise = s_cruise / v_max
        t_total = 2 * t_acc + t_cruise

    # 按插补周期离散化
    increments = []
    t = 0.0
    while t < t_total - 1e-9:
        t_next = min(t + T, t_total)
        dt = t_next - t
        t_mid = t + dt / 2  # 取中点速度近似该周期平均速度

        # 根据中点时刻判断所处阶段
        if t_mid < t_acc:
            v = a * t_mid
        elif t_mid < t_acc + t_cruise:
            v = v_max
        else:
            v = a * (t_total - t_mid)

        increments.append(v * dt)
        t = t_next

    # 修正累积误差，确保总位移精确等于L
    total = sum(increments)
    if total > 0:
        scale = L / total
        increments = [inc * scale for inc in increments]
    return increments, t_total

def line_interpolation_with_trapezoid(start, end, v_max, a, T):
    """
    直线插补：起点start、终点end为n维坐标（numpy数组）。
    返回插补点列表（含起点，不含终点）与每周期位移增量。
    """
    start = np.array(start, dtype=float)
    end = np.array(end, dtype=float)
    delta = end - start
    L = np.linalg.norm(delta)
    if L < 1e-12:
        return [start], []

    direction = delta / L
    increments, t_total = trapezoid_velocity_profile(L, v_max, a, T)

    # 生成插补点
    points = [start.copy()]
    s = 0.0
    for inc in increments:
        s += inc
        points.append(start + direction * s)

    # 确保最后一个点精确等于终点
    points[-1] = end.copy()
    return points, increments

# ===== 示例：3D空间直线插补 =====
if __name__ == "__main__":
    start = np.array([0.0, 0.0, 0.0])
    end = np.array([100.0, 50.0, 20.0])  # 单位：mm
    v_max = 50.0    # mm/s
    a = 100.0       # mm/s²
    T = 0.01        # 插补周期 10ms

    points, increments = line_interpolation_with_trapezoid(start, end, v_max, a, T)

    # 验证：总位移与终点坐标
    total_dist = sum(increments)
    final_point = points[-1]
    print(f"插补点数: {len(points)}")
    print(f"总位移: {total_dist:.3f} mm")
    print(f"终点坐标: ({final_point[0]:.3f}, {final_point[1]:.3f}, {final_point[2]:.3f})")
    print(f"理论直线距离: {np.linalg.norm(end - start):.3f} mm")

    # 验证速度曲线形状（打印前5个与后5个位移增量）
    print(f"前5个增量: {[f'{x:.4f}' for x in increments[:5]]}")
    print(f"后5个增量: {[f'{x:.4f}' for x in increments[-5:]]}")

运行与验证

运行方式：直接执行python 本文件.py，需安装NumPy（pip install numpy）。

验证方法：

• 程序输出的总位移应等于理论直线距离（约114.018 mm），误差小于0.001 mm。
• 终点坐标应精确等于(100, 50, 20)。
• 前5个位移增量应逐渐增大（加速段），后5个逐渐减小（减速段），中间段接近恒定值v_max × T = 0.5 mm，证明速度曲线为梯形。

关键说明

梯形速度规划中，若位移过短导致无法加速到最大速度，代码自动退化为三角形速度曲线。插补点数量由t_total / T向上取整决定，最后一个周期可能不满T，代码通过中点速度近似处理。末尾的等比缩放修正了离散化累积误差，确保终点精确落在目标位置。