4. 梯形速度规划

任务目标

实现单轴梯形速度规划器：给定总位移 \(S\)、最大速度 \(V_{max}\)、加速度 \(A\) 和减速度 \(D\)，计算加速、匀速、减速三段的时间与位移，并输出完整的速度-时间曲线采样点，用于后续运动控制插补。

核心算法

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

• 能达到最大速度（\(S \geq S_{tri}\)）：三段完整，匀速段时间 \(t_c > 0\)。
• 达不到最大速度（\(S < S_{tri}\)）：只有加速和减速段，实际峰值速度 \(V_{peak} < V_{max}\)。

其中三角形临界位移 \(S_{tri} = \frac{V_{max}^2}{2A} + \frac{V_{max}^2}{2D}\)。若达不到最大速度，则实际峰值速度 \(V_{peak} = \sqrt{\frac{2 \cdot S \cdot A \cdot D}{A + D}}\)。

完整实现

import numpy as np

def trapezoidal_profile(S, Vmax, A, D, dt=0.001):
    """
    梯形速度规划
    参数:
        S: 总位移 (正数)
        Vmax: 最大速度
        A: 加速度
        D: 减速度
        dt: 采样周期
    返回:
        dict: 包含各段时间/位移及速度曲线数组
    """
    assert S > 0 and Vmax > 0 and A > 0 and D > 0, "参数必须为正"

    # 加速段时间与位移
    ta = Vmax / A
    sa = 0.5 * A * ta**2

    # 减速段时间与位移
    td = Vmax / D
    sd = 0.5 * D * td**2

    # 三角形临界位移
    Stri = sa + sd

    if S >= Stri:
        # 情况1: 能达到最大速度
        sc = S - sa - sd
        tc = sc / Vmax
        v_peak = Vmax
    else:
        # 情况2: 达不到最大速度，只有加速+减速
        v_peak = np.sqrt(2 * S * A * D / (A + D))
        ta = v_peak / A
        td = v_peak / D
        sa = 0.5 * A * ta**2
        sd = 0.5 * D * td**2
        tc = 0.0
        sc = 0.0

    # 生成速度曲线采样
    t_total = ta + tc + td
    n = int(np.ceil(t_total / dt)) + 1
    t_arr = np.linspace(0, t_total, n)
    v_arr = np.zeros_like(t_arr)
    s_arr = np.zeros_like(t_arr)

    for i, t in enumerate(t_arr):
        if t < ta:
            v = A * t
            s = 0.5 * A * t**2
        elif t < ta + tc:
            v = v_peak
            s = sa + v_peak * (t - ta)
        else:
            t_dec = t - ta - tc
            v = v_peak - D * t_dec
            s = sa + sc + v_peak * t_dec - 0.5 * D * t_dec**2
        v_arr[i] = v
        s_arr[i] = s

    return {
        "ta": ta, "tc": tc, "td": td,
        "sa": sa, "sc": sc, "sd": sd,
        "v_peak": v_peak, "t_total": t_total,
        "t_arr": t_arr, "v_arr": v_arr, "s_arr": s_arr
    }

# 示例调用
if __name__ == "__main__":
    # 例1: 能达到最大速度
    r1 = trapezoidal_profile(S=0.1, Vmax=0.2, A=0.5, D=0.4)
    print(f"例1: ta={r1['ta']:.3f}s, tc={r1['tc']:.3f}s, td={r1['td']:.3f}s")
    print(f"     sa={r1['sa']:.4f}m, sc={r1['sc']:.4f}m, sd={r1['sd']:.4f}m")
    print(f"     总位移验证: {r1['sa']+r1['sc']+r1['sd']:.4f}m (目标0.1m)")

    # 例2: 达不到最大速度
    r2 = trapezoidal_profile(S=0.02, Vmax=0.2, A=0.5, D=0.4)
    print(f"例2: ta={r2['ta']:.3f}s, tc={r2['tc']:.3f}s, td={r2['td']:.3f}s")
    print(f"     实际峰值速度: {r2['v_peak']:.3f} m/s (Vmax=0.2)")
    print(f"     总位移验证: {r2['sa']+r2['sc']+r2['sd']:.4f}m (目标0.02m)")

运行与验证

直接运行脚本：

python trapezoidal_profile.py

通过条件：

1. 例1中三段位移之和等于0.1m（误差<1e-6），且 \(t_c > 0\)；
2. 例2中三段位移之和等于0.02m，且 \(v_{peak} < 0.2\)，\(t_c = 0\)；
3. 速度曲线末点速度趋近于0，位移末点等于目标位移。

可视化

上图展示了两种规划结果：实线为位移充足时的完整梯形（加速-匀速-减速），虚线为位移不足时的三角形速度曲线（无匀速段）。规划器自动判断并切换两种模式。

常见问题

• 位移为负或零：本实现要求 \(S>0\)，反向运动可对位移取值后规划，再对速度取反。
• 加减速度不相等：代码已分别处理 \(A\) 和 \(D\)，无需额外修改。
• 采样点数过多：若 \(t_{total}\) 较大且 \(dt\) 很小，数组会很长。实际使用时可根据需要降低采样频率，或改为逐点生成方式。