1. 梯形速度规划基础实现

任务说明

实现梯形速度规划器：给定起点位置 \(p_0\)、终点位置 \(p_1\)、最大速度 \(v_{max}\)、加速度 \(a\) 和减速度 \(d\)，计算总运动时间 \(T\)，并输出位置 \(p(t)\)、速度 \(v(t)\)、加速度 \(a(t)\) 随时间变化的离散曲线。要求验证加减速过程的对称性（即加速段与减速段的时间相等、位移相等）。

核心算法

梯形速度规划将运动分为三个阶段：匀加速、匀速（可能不存在）、匀减速。设总位移 \(D = p_1 - p_0\)，符号方向由 \(D\) 决定。先计算按给定加减速度达到最大速度所需位移：

\[ D_{acc} = \frac{v_{max}^2}{2a}, \quad D_{dec} = \frac{v_{max}^2}{2d} \]

若 \(D_{acc} + D_{dec} \le |D|\)，则存在匀速段；否则实际峰值速度 \(v_{peak} < v_{max}\)，需重新计算：

\[ v_{peak} = \sqrt{\frac{2ad|D|}{a+d}} \]

各阶段时间与位移按运动学公式计算。本实现采用离散时间采样，时间步长 \(\Delta t\) 由用户指定，输出数组长度 \(N = \lfloor T/\Delta t \rfloor + 1\)。

代码实现

import numpy as np

def trap_velocity_profile(p0, p1, vmax, acc, dec, dt=0.001):
    """
    梯形速度规划器
    参数:
        p0, p1: 起点/终点位置 (m)
        vmax:   最大速度 (m/s)
        acc:    加速度 (m/s^2)
        dec:    减速度 (m/s^2)
        dt:     采样时间步长 (s)
    返回:
        t:    时间数组 (s)
        p:    位置数组 (m)
        v:    速度数组 (m/s)
        a:    加速度数组 (m/s^2)
        phase: 阶段标记数组 (1=加速, 2=匀速, 3=减速)
    """
    D = p1 - p0
    sign = 1.0 if D >= 0 else -1.0
    D_abs = abs(D)

    # 计算实际峰值速度
    d_acc = vmax*vmax / (2*acc)
    d_dec = vmax*vmax / (2*dec)
    if d_acc + d_dec <= D_abs:
        v_peak = vmax
    else:
        v_peak = np.sqrt(2*acc*dec*D_abs / (acc+dec))

    # 各阶段时间
    t_acc = v_peak / acc
    t_dec = v_peak / dec
    d_acc = v_peak*v_peak / (2*acc)
    d_dec = v_peak*v_peak / (2*dec)
    d_cruise = D_abs - d_acc - d_dec
    t_cruise = d_cruise / v_peak if v_peak > 0 else 0.0
    T_total = t_acc + t_cruise + t_dec

    # 离散采样
    N = int(np.floor(T_total / dt)) + 1
    t = np.linspace(0, T_total, N)
    p = np.zeros(N)
    v = np.zeros(N)
    a = np.zeros(N)
    phase = np.zeros(N, dtype=int)

    for i, ti in enumerate(t):
        if ti <= t_acc:
            # 加速段
            s = 0.5 * acc * ti*ti
            vel = acc * ti
            accel = acc
            ph = 1
        elif ti <= t_acc + t_cruise:
            # 匀速段
            s = d_acc + v_peak * (ti - t_acc)
            vel = v_peak
            accel = 0.0
            ph = 2
        else:
            # 减速段
            tr = ti - t_acc - t_cruise
            s = d_acc + d_cruise + v_peak*tr - 0.5*dec*tr*tr
            vel = v_peak - dec * tr
            accel = -dec
            ph = 3
        p[i] = p0 + sign * s
        v[i] = sign * vel
        a[i] = sign * accel
        phase[i] = ph

    return t, p, v, a, phase, T_total, t_acc, t_dec, d_acc, d_dec

# 示例输入
p0, p1 = 0.0, 1.0
vmax, acc, dec = 0.5, 1.0, 1.0
dt = 0.001

t, p, v, a, phase, T, ta, td, da, dd = trap_velocity_profile(
    p0, p1, vmax, acc, dec, dt)

# 验证加减速对称性
print(f"总时间 T = {T:.4f} s")
print(f"加速时间 = {ta:.4f} s, 减速时间 = {td:.4f} s")
print(f"加速位移 = {da:.4f} m, 减速位移 = {dd:.4f} m")
print(f"终点位置 = {p[-1]:.4f} m (目标 {p1} m)")
print(f"最大速度 = {np.max(v):.4f} m/s (限制 {vmax} m/s)")

落地与验证

运行方式：将代码保存为 trap_profile.py，在装有 NumPy 的 Python 环境中执行 python trap_profile.py。

验证方法：

1. 检查输出终点位置 p[-1] 是否等于目标位置 p1（误差小于采样步长引入的舍入误差）；
2. 检查最大速度是否不超过 vmax；
3. 加减速对称性验证：当 acc == dec 时，比较加速时间与减速时间、加速位移与减速位移是否相等（数值上应完全一致）；
4. 绘制 v-t 曲线，观察是否呈梯形形状。

判定标准：当 acc = dec = 1.0 时，输出中 ta == td 且 da == dd，终点位置误差小于 \(10^{-6}\) m，即判定通过。

预期输出

对于示例输入（\(p_0=0, p_1=1, v_{max}=0.5, a=d=1.0\)），代码逻辑可唯一推出：

总时间 T = 2.5000 s
加速时间 = 0.5000 s, 减速时间 = 0.5000 s
加速位移 = 0.1250 m, 减速位移 = 0.1250 m
终点位置 = 1.0000 m (目标 1 m)
最大速度 = 0.5000 m/s (限制 0.5 m/s)

该结果满足加减速对称性判定标准，且终点位置精确到达目标点。