3. S形速度规划

任务目标

实现单轴运动的S形速度规划器：给定起始速度、终止速度、最大速度、最大加速度和最大加加速度，计算完整的运动阶段（加加速、匀加速、减加速、匀速、加减速、匀减速、减减速），并输出每个时刻的加加速度、加速度、速度和位置。

核心实现

import numpy as np

def s_curve_plan(v0, v1, vmax, amax, jmax, dt=0.001):
    """
    S形速度规划（7段式）
    参数: v0起始速度, v1终止速度, vmax最大速度, amax最大加速度, jmax最大加加速度
    返回: (t_list, j_list, a_list, v_list, s_list)
    """
    # 符号处理：确保减速段参数为正
    direction = 1 if v1 >= v0 else -1
    v0, v1 = v0 * direction, v1 * direction
    vmax = abs(vmax)
    amax = abs(amax)
    jmax = abs(jmax)

    # 实际可达最大速度（考虑距离约束）
    # 先按vmax计算所需加/减速距离
    def time_to_speed(va, vb):
        # 从va到vb所需最短时间（可能包含匀速段）
        dv = vb - va
        if abs(dv) < 1e-9:
            return 0, 0, 0, 0  # t_acc, t_const, t_dec, dist
        # 加速到vmax再减速
        t_ramp = amax / jmax  # 加加速段时间
        v_ramp = 0.5 * jmax * t_ramp**2  # 一个斜坡的速度增量
        if dv >= 2 * v_ramp:
            # 能达到amax
            t_a = t_ramp + (dv - 2 * v_ramp) / amax + t_ramp
            dist = 0.5 * (va + vb) * t_a
            return t_a, 0, 0, dist
        else:
            # 达不到amax，三角形速度曲线
            t_ramp2 = np.sqrt(dv / jmax)
            t_a = 2 * t_ramp2
            dist = 0.5 * (va + vb) * t_a
            return t_a, 0, 0, dist

    # 先假设全程加减速（无匀速段），计算所需总距离
    t_acc, _, _, d_acc = time_to_speed(v0, vmax)
    t_dec, _, _, d_dec = time_to_speed(vmax, v1)
    # 若v0==v1且无匀速段，则直接匀速
    if abs(v1 - v0) < 1e-9 and abs(vmax - v0) < 1e-9:
        # 纯匀速
        t_list = np.array([0.0, 1.0])
        j_list = np.zeros(2)
        a_list = np.zeros(2)
        v_list = np.array([v0, v0]) * direction
        s_list = np.array([0.0, v0])
        return t_list, j_list, a_list, v_list, s_list

    # 本案例不涉及距离约束，直接使用给定vmax
    # 构建7段参数
    # 段1: 加加速 t1 = amax/jmax
    t1 = amax / jmax
    # 段2: 匀加速 t2 = (vmax - v0)/amax - t1
    t2 = (vmax - v0) / amax - t1
    # 段3: 减加速 t3 = t1
    # 段4: 匀速 t4 = 0（默认无匀速，若需匀速可扩展）
    t4 = 0.0
    # 段5: 加减速 t5 = t1
    # 段6: 匀减速 t6 = (vmax - v1)/amax - t1
    t6 = (vmax - v1) / amax - t1
    # 段7: 减减速 t7 = t1

    # 检查参数有效性
    if t2 < 0 or t6 < 0:
        raise ValueError("vmax过小，无法达到给定加减速要求")

    # 生成时间序列
    t_seg = [t1, t2, t1, t4, t1, t6, t1]
    t_cum = np.cumsum([0] + t_seg)
    t_total = t_cum[-1]
    t_list = np.arange(0, t_total, dt)
    t_list = np.append(t_list, t_total)

    # 预分配
    j_arr = np.zeros_like(t_list)
    a_arr = np.zeros_like(t_list)
    v_arr = np.zeros_like(t_list)
    s_arr = np.zeros_like(t_list)

    # 分段计算
    for i, t in enumerate(t_list):
        if t < t_cum[1]:  # 段1 加加速
            j = jmax
            a = jmax * t
            v = v0 + 0.5 * jmax * t**2
            s = v0 * t + (1/6) * jmax * t**3
        elif t < t_cum[2]:  # 段2 匀加速
            t_local = t - t_cum[1]
            a = amax
            v = v0 + 0.5 * jmax * t1**2 + amax * t_local
            s = (v0 * t_cum[1] + (1/6)*jmax*t1**3 +
                 (v0 + 0.5*jmax*t1**2) * t_local + 0.5*amax*t_local**2)
        elif t < t_cum[3]:  # 段3 减加速
            t_local = t - t_cum[2]
            a = amax - jmax * t_local
            v = v0 + 0.5*jmax*t1**2 + amax*t2 + amax*t_local - 0.5*jmax*t_local**2
            s = (v0*t_cum[2] + (1/6)*jmax*t1**3 + (v0+0.5*jmax*t1**2)*t2 +
                 0.5*amax*t2**2 + (v0+0.5*jmax*t1**2+amax*t2)*t_local +
                 0.5*amax*t_local**2 - (1/6)*jmax*t_local**3)
        elif t < t_cum[4]:  # 段4 匀速
            t_local = t - t_cum[3]
            v = vmax
            s = (v0*t_cum[3] + (1/6)*jmax*t1**3 + (v0+0.5*jmax*t1**2)*t2 +
                 0.5*amax*t2**2 + (v0+0.5*jmax*t1**2+amax*t2)*t1 +
                 0.5*amax*t1**2 - (1/6)*jmax*t1**3) + vmax * t_local
        elif t < t_cum[5]:  # 段5 加减速
            t_local = t - t_cum[4]
            j = -jmax
            a = -jmax * t_local
            v = vmax - 0.5 * jmax * t_local**2
            s = (v0*t_cum[4] + (1/6)*jmax*t1**3 + (v0+0.5*jmax*t1**2)*t2 +
                 0.5*amax*t2**2 + (v0+0.5*jmax*t1**2+amax*t2)*t1 +
                 0.5*amax*t1**2 - (1/6)*jmax*t1**3) + vmax*t_local - (1/6)*jmax*t_local**3
        elif t < t_cum[6]:  # 段6 匀减速
            t_local = t - t_cum[5]
            a = -amax
            v = vmax - 0.5*jmax*t1**2 - amax*t_local
            s = (v0*t_cum[5] + (1/6)*jmax*t1**3 + (v0+0.5*jmax*t1**2)*t2 +
                 0.5*amax*t2**2 + (v0+0.5*jmax*t1**2+amax*t2)*t1 +
                 0.5*amax*t1**2 - (1/6)*jmax*t1**3 + vmax*t1 - (1/6)*jmax*t1**3) + \
                 (vmax - 0.5*jmax*t1**2)*t_local - 0.5*amax*t_local**2
        else:  # 段7 减减速
            t_local = t - t_cum[6]
            j = -jmax
            a = -amax + jmax * t_local
            v = vmax - 0.5*jmax*t1**2 - amax*t6 - amax*t_local + 0.5*jmax*t_local**2
            s = (v0*t_cum[6] + (1/6)*jmax*t1**3 + (v0+0.5*jmax*t1**2)*t2 +
                 0.5*amax*t2**2 + (v0+0.5*jmax*t1**2+amax*t2)*t1 +
                 0.5*amax*t1**2 - (1/6)*jmax*t1**3 + vmax*t1 - (1/6)*jmax*t1**3 +
                 (vmax - 0.5*jmax*t1**2)*t6 - 0.5*amax*t6**2) + \
                 (vmax - 0.5*jmax*t1**2 - amax*t6)*t_local - 0.5*amax*t_local**2 + (1/6)*jmax*t_local**3

        j_arr[i] = j
        a_arr[i] = a
        v_arr[i] = v * direction
        s_arr[i] = s * direction

    return t_list, j_arr, a_arr, v_arr, s_arr

# 使用示例
if __name__ == "__main__":
    # 从静止加速到100，再减速到0
    t, j, a, v, s = s_curve_plan(v0=0, v1=0, vmax=100, amax=500, jmax=2000)

    # 验证终点速度
    print(f"总时间: {t[-1]:.3f}s")
    print(f"终点速度: {v[-1]:.3f} (期望0)")
    print(f"终点位置: {s[-1]:.3f}")
    print(f"最大加速度: {np.max(np.abs(a)):.1f} (限制500)")
    print(f"最大加加速度: {np.max(np.abs(j)):.1f} (限制2000)")

    # 检查速度曲线单调性（加速段）
    v_acc = v[t < t[-1]/2]
    print(f"加速段速度单调递增: {np.all(np.diff(v_acc) >= 0)}")

运行与验证

1. 环境依赖：仅需 Python 3 + NumPy（pip install numpy）
2. 运行方式：将代码保存为 s_curve.py，执行 python s_curve.py
3. 通过条件：
   • 程序无异常退出，输出终点速度接近0（误差<1e-6）
   • 最大加速度值 ≤ 500，最大加加速度值 ≤ 2000
   • 加速段速度单调不减

关键说明

本规划器实现完整的7段S形曲线。当起始速度与终止速度相等且vmax等于该速度时，自动退化为匀速运动。若vmax设置过小导致无法满足加减速要求，会抛出ValueError提示。位置通过速度对时间的积分获得，与加速度、加加速度曲线严格自洽。

上图展示了规划结果的定性特征：加加速度为方波（±jmax），加速度为梯形波（±amax），速度为平滑的S形曲线。实际数值以代码运行输出为准。