3. S型曲线加减速规划

任务目标

实现S型曲线加减速规划器：输入运动距离、最大速度、最大加速度和最大加加速度，输出满足约束的位置-时间序列。S型曲线通过限制加加速度（Jerk）的突变，使加速度连续变化，从而减小机械冲击。

核心算法

S型规划将运动过程分为7个阶段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。算法核心是：根据约束条件计算各阶段持续时间，若距离不足以达到最大速度，则自动降速规划（三角形速度曲线）。

完整实现

import numpy as np

def s_curve_plan(dist, vmax, amax, jmax, dt=0.001):
    """
    S型曲线加减速规划
    参数:
        dist: 总位移 (m), 要求 > 0
        vmax: 最大速度 (m/s)
        amax: 最大加速度 (m/s^2)
        jmax: 最大加加速度 (m/s^3)
        dt:   采样周期 (s)
    返回:
        t_arr: 时间序列 (s)
        p_arr: 位置序列 (m)
        v_arr: 速度序列 (m/s)
        a_arr: 加速度序列 (m/s^2)
    """
    # 1. 计算匀加速段能达到的最大速度
    v_lim = vmax
    # 加速到vmax所需时间（无匀速段时）
    t_acc = vmax / amax
    # 加加速段时间
    t_j = amax / jmax
    
    # 若加速段无法达到梯形（无匀加速段），则修正
    if t_acc < 2 * t_j:
        # 三角形加速度曲线：vmax无法达到，实际最大速度受限
        v_lim = np.sqrt(amax * amax / jmax * 2)  # 实际可达最大速度
        # 重新计算
        t_j = np.sqrt(v_lim / jmax)
        t_acc = 2 * t_j
        # 匀加速段时间为0
        t_const_acc = 0.0
    else:
        t_const_acc = t_acc - 2 * t_j
    
    # 2. 计算加速段位移
    # 加加速段位移: (1/6)*jmax*t_j^3
    s_j = (1.0/6.0) * jmax * t_j**3
    # 匀加速段位移: v1*t + 0.5*amax*t^2, v1 = 0.5*jmax*t_j^2
    v1 = 0.5 * jmax * t_j**2
    s_const_acc = v1 * t_const_acc + 0.5 * amax * t_const_acc**2
    # 减加速段位移: 同加加速段
    s_acc_total = 2 * s_j + s_const_acc
    
    # 3. 判断是否需要匀速段
    if 2 * s_acc_total <= dist:
        # 完整7段S型
        s_const = dist - 2 * s_acc_total
        t_const = s_const / v_lim
        t_total = 2 * t_acc + t_const
        has_const = True
    else:
        # 距离不足，无法达到v_lim，需要降速规划（三角形速度）
        has_const = False
        # 二分法求实际最大速度v_peak
        v_low, v_high = 0.0, v_lim
        for _ in range(100):
            v_mid = 0.5 * (v_low + v_high)
            t_j_mid = np.sqrt(v_mid / jmax)
            t_acc_mid = 2 * t_j_mid
            # 加速段位移 = 2 * (1/6)*jmax*t_j^3 + 0.5*amax*t_const_acc^2
            # 但此时无匀加速段，位移 = 2 * (1/6)*jmax*t_j^3 = (1/3)*jmax*t_j^3
            s_acc_mid = (1.0/3.0) * jmax * t_j_mid**3
            if 2 * s_acc_mid < dist:
                v_low = v_mid
            else:
                v_high = v_mid
        v_peak = 0.5 * (v_low + v_high)
        t_j = np.sqrt(v_peak / jmax)
        t_acc = 2 * t_j
        t_const_acc = 0.0
        t_const = 0.0
        t_total = 2 * t_acc
        v_lim = v_peak
    
    # 4. 生成时间序列
    n = int(np.ceil(t_total / dt)) + 1
    t_arr = np.linspace(0, t_total, n)
    p_arr = np.zeros(n)
    v_arr = np.zeros(n)
    a_arr = np.zeros(n)
    
    # 5. 各阶段时间节点
    t1 = t_j
    t2 = t_j + t_const_acc
    t3 = 2 * t_j + t_const_acc
    t4 = t3 + t_const
    t5 = t4 + t_j
    t6 = t4 + t_j + t_const_acc
    t7 = t_total
    
    # 6. 逐点计算（向量化）
    for i, t in enumerate(t_arr):
        if t <= t1:
            # 加加速段
            a = jmax * t
            v = 0.5 * jmax * t**2
            p = (1.0/6.0) * jmax * t**3
        elif t <= t2:
            # 匀加速段
            dt_ = t - t1
            a = amax
            v = v1 + amax * dt_
            p = s_j + v1 * dt_ + 0.5 * amax * dt_**2
        elif t <= t3:
            # 减加速段
            dt_ = t - t2
            a = amax - jmax * dt_
            v = v1 + amax * t_const_acc + amax * dt_ - 0.5 * jmax * dt_**2
            p = (s_j + s_const_acc) + (v1 + amax * t_const_acc) * dt_ + 0.5 * amax * dt_**2 - (1.0/6.0) * jmax * dt_**3
        elif t <= t4:
            # 匀速段
            dt_ = t - t3
            a = 0.0
            v = v_lim
            p = s_acc_total + v_lim * dt_
        elif t <= t5:
            # 加减速段
            dt_ = t - t4
            a = -jmax * dt_
            v = v_lim - 0.5 * jmax * dt_**2
            p = (s_acc_total + v_lim * t_const) + v_lim * dt_ - (1.0/6.0) * jmax * dt_**3
        elif t <= t6:
            # 匀减速段
            dt_ = t - t5
            a = -amax
            v = v_lim - 0.5 * jmax * t_j**2 - amax * dt_
            p = (s_acc_total + v_lim * t_const + (v_lim * t_j - (1.0/6.0)*jmax*t_j**3)) + \
                (v_lim - 0.5*jmax*t_j**2) * dt_ - 0.5 * amax * dt_**2
        else:
            # 减减速段
            dt_ = t - t6
            a = -amax + jmax * dt_
            v = v_lim - 0.5 * jmax * t_j**2 - amax * t_const_acc - amax * dt_ + 0.5 * jmax * dt_**2
            p = dist - (1.0/6.0) * jmax * (t7 - t)**3
        
        a_arr[i] = a
        v_arr[i] = v
        p_arr[i] = p
    
    return t_arr, p_arr, v_arr, a_arr

# 使用示例
if __name__ == "__main__":
    # 参数：距离1m，最大速度0.5m/s，最大加速度1m/s^2，最大加加速度5m/s^3
    t, p, v, a = s_curve_plan(dist=1.0, vmax=0.5, amax=1.0, jmax=5.0, dt=0.001)
    
    # 验证：终点位置、速度、加速度
    print(f"终点位置: {p[-1]:.4f} m (目标1.0)")
    print(f"终点速度: {v[-1]:.6f} m/s")
    print(f"终点加速度: {a[-1]:.6f} m/s^2")
    print(f"最大速度: {np.max(v):.4f} m/s")
    print(f"最大加速度: {np.max(np.abs(a)):.4f} m/s^2")
    print(f"总时间: {t[-1]:.4f} s")
    print(f"点数: {len(t)}")

运行与验证

直接运行脚本即可执行示例参数（距离1m，最大速度0.5m/s，最大加速度1m/s²，加加速度5m/s³）。验证通过条件：

• 终点位置误差小于0.1%（即0.999~1.001m）
• 终点速度与加速度接近0（小于1e-4）
• 全程速度不超过设定最大值，加速度不超过设定最大值

预期输出：终点位置约1.0000m，终点速度与加速度为0，最大速度0.5m/s，最大加速度1.0m/s²。

关键说明

当距离较短时（如0.1m），规划器自动进入三角形速度模式，实际峰值速度低于设定值，代码通过二分法自动求解。若需验证该分支，可将dist改为0.1运行观察。