4. 梯形速度规划

任务目标

实现梯形速度规划器：给定总位移 \(D\)、最大速度 \(v_{max}\)、加速度 \(a\)，计算加速段时间 \(t_a\)、匀速段时间 \(t_c\)、减速段时间 \(t_d\)（本案例中 \(t_a = t_d\)），并生成位置、速度、加速度随时间变化的离散曲线。

核心算法

梯形速度规划分为两种情况：

• 能达到最大速度（\(D > v_{max}^2 / a\)）：加速段与减速段各用时 \(t_a = v_{max}/a\)，匀速段用时 \(t_c = D/v_{max} - t_a\)。
• 达不到最大速度（三角规划，\(D \le v_{max}^2 / a\)）：实际峰值速度 \(v_p = \sqrt{a \cdot D}\)，\(t_a = t_d = v_p/a\)，\(t_c = 0\)。

总时间 \(T = 2t_a + t_c\)。离散化时以固定周期 \(\Delta t\) 采样，逐段计算位置、速度、加速度。

完整实现

import numpy as np

def trapezoidal_profile(D, v_max, a, dt=0.001):
    """
    梯形速度规划
    参数:
        D: 总位移 (正数)
        v_max: 最大速度
        a: 加速度 (正数)
        dt: 采样周期
    返回:
        t: 时间数组
        q: 位置数组
        v: 速度数组
        acc: 加速度数组
    """
    # 判断是否可达最大速度
    if D > v_max**2 / a:
        # 梯形: 加速-匀速-减速
        t_a = v_max / a
        t_c = D / v_max - t_a
        v_p = v_max
    else:
        # 三角: 加速-减速 (无匀速段)
        v_p = np.sqrt(a * D)
        t_a = v_p / a
        t_c = 0.0

    t_d = t_a
    T = 2 * t_a + t_c

    # 离散采样
    n = int(np.ceil(T / dt)) + 1
    t = np.linspace(0, T, n)
    q = np.zeros(n)
    v = np.zeros(n)
    acc = np.zeros(n)

    # 各段边界时间
    t1 = t_a
    t2 = t_a + t_c
    t3 = T

    for i, ti in enumerate(t):
        if ti <= t1:
            # 加速段
            acc[i] = a
            v[i] = a * ti
            q[i] = 0.5 * a * ti**2
        elif ti <= t2:
            # 匀速段
            acc[i] = 0
            v[i] = v_p
            q[i] = 0.5 * a * t1**2 + v_p * (ti - t1)
        elif ti <= t3:
            # 减速段
            tr = ti - t2
            acc[i] = -a
            v[i] = v_p - a * tr
            q[i] = (0.5 * a * t1**2 + v_p * t_c +
                    v_p * tr - 0.5 * a * tr**2)
        else:
            # 结束点
            acc[i] = 0
            v[i] = 0
            q[i] = D

    return t, q, v, acc

# 示例调用
if __name__ == "__main__":
    # 例1: 可达最大速度
    t, q, v, acc = trapezoidal_profile(D=1.0, v_max=0.5, a=1.0)
    print(f"例1: 总时间={t[-1]:.3f}s, 终点位置={q[-1]:.3f}m")

    # 例2: 不可达最大速度 (三角规划)
    t2, q2, v2, acc2 = trapezoidal_profile(D=0.1, v_max=0.5, a=1.0)
    print(f"例2: 总时间={t2[-1]:.3f}s, 终点位置={q2[-1]:.3f}m")

运行与验证

1. 运行：保存为 trapezoidal.py，执行 python trapezoidal.py。
2. 验证方法：检查输出终点位置是否等于目标位移 \(D\)（误差小于 \(v_{max}\cdot dt\) 量级）；速度曲线无突变（梯形）或呈三角形（三角规划）；加速度在 \(\pm a\) 范围内。
3. 预期输出（由代码逻辑直接推出）：
   • 例1：总时间约 2.5s，终点位置 1.000m
   • 例2：总时间约 0.632s，终点位置 0.100m

曲线可视化

以下为梯形规划（例1）的位置、速度、加速度曲线示意，展示三段式特征：

常见问题

• 终点位置偏差：由于离散采样，终点位置与 \(D\) 存在不超过 \(v_p \cdot dt\) 的误差，减小 \(dt\) 可提高精度。
• 加减速不对称：本实现假设 \(a_{acc}=a_{dec}\)，若需非对称加减速，需分别计算 \(t_a\) 与 \(t_d\)，并调整匀速段起始时间。
• 位移为负：本实现仅处理正方向运动，反向运动可对 \(D\) 取值后规划，再反转位置符号。