📚 课程目录第3章 S曲线速度规划

3. S曲线速度规划

任务目标

实现S形速度规划器：输入运动距离 D、最大速度 Vmax、最大加速度 Amax 和加加速度 J，输出满足所有约束的速度曲线（离散时间序列），并验证加速度与加加速度不超限。

核心算法

S形规划将运动分为7段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。关键计算步骤：

计算达到 Vmax 所需的理论距离 D_acc（加速段距离）与理论时间；
若 2 × D_acc > D，则无法达到 Vmax，需降低峰值速度 V_peak；
根据实际峰值速度计算各段时间，生成分段速度曲线。

完整实现（Python）

import numpy as np

def s_curve_plan(D, Vmax, Amax, J, dt=0.001):
    """
    S形速度规划（7段式）
    返回: (t, v, a) 时间、速度、加速度序列
    """
    # 1. 计算加速段参数（假设能达到Vmax）
    Ta = Amax / J                    # 加加速段时间
    Da = J * Ta**3 + Amax * Ta**2    # 加速段总距离（加加速+匀加速+减加速）
    Va = J * Ta**2                   # 加速段结束速度（=Amax*Ta）
    
    # 2. 检查是否能达到Vmax
    if 2 * Da <= D:
        V_peak = Vmax
        # 加速段各段时间
        t1 = t3 = t5 = t7 = Ta
        t2 = (Vmax - Va) / Amax      # 匀加速时间
        t6 = t2                      # 匀减速时间
        # 匀速段时间
        D_acc = Da + Vmax * t2       # 加速段总距离
        t4 = (D - 2 * D_acc) / Vmax
    else:
        # 无法达到Vmax，计算实际峰值速度
        # 解方程: 2*(J*t1^3 + V_peak*t2) = D, 其中 t1=sqrt(V_peak/J), t2=V_peak/Amax - t1
        # 简化：使用二分法求V_peak
        lo, hi = 0, Vmax
        for _ in range(100):
            mid = (lo + hi) / 2
            t1 = np.sqrt(mid / J)
            t2 = mid / Amax - t1
            if t2 < 0:
                t2 = 0
            d_acc = J * t1**3 + mid * t2
            if 2 * d_acc <= D:
                lo = mid
            else:
                hi = mid
        V_peak = (lo + hi) / 2
        t1 = t3 = t5 = t7 = np.sqrt(V_peak / J)
        t2 = t6 = V_peak / Amax - t1
        if t2 < 0:
            t2 = t6 = 0
        t4 = 0  # 无匀速段
    
    # 3. 生成时间序列
    t_seg = [t1, t2, t3, t4, t5, t6, t7]
    t_total = sum(t_seg)
    n = int(t_total / dt) + 1
    t = np.linspace(0, t_total, n)
    v = np.zeros_like(t)
    a = np.zeros_like(t)
    
    # 4. 分段计算速度与加速度
    for i, ti in enumerate(t):
        # 确定当前时间段
        cum = 0
        for seg_idx, seg_t in enumerate(t_seg):
            if ti <= cum + seg_t or seg_idx == 6:
                t_local = ti - cum
                break
            cum += seg_t
        
        # 根据段号计算
        if seg_idx == 0:      # 加加速
            a[i] = J * t_local
            v[i] = 0.5 * J * t_local**2
        elif seg_idx == 1:    # 匀加速
            a[i] = Amax
            v[i] = Va + Amax * t_local
        elif seg_idx == 2:    # 减加速
            a[i] = Amax - J * t_local
            v[i] = V_peak - 0.5 * J * (t1 - t_local)**2
        elif seg_idx == 3:    # 匀速
            a[i] = 0
            v[i] = V_peak
        elif seg_idx == 4:    # 加减速
            a[i] = -J * t_local
            v[i] = V_peak - 0.5 * J * t_local**2
        elif seg_idx == 5:    # 匀减速
            a[i] = -Amax
            v[i] = V_peak - Va - Amax * t_local
        else:                 # 减减速
            a[i] = -Amax + J * t_local
            v[i] = 0.5 * J * (t7 - t_local)**2
    
    return t, v, a

# 使用示例
if __name__ == "__main__":
    D, Vmax, Amax, J = 0.5, 0.3, 0.8, 2.0
    t, v, a = s_curve_plan(D, Vmax, Amax, J)
    
    # 验证约束
    print(f"最大速度: {np.max(v):.4f} (限制 {Vmax})")
    print(f"最大加速度: {np.max(np.abs(a)):.4f} (限制 {Amax})")
    print(f"最大加加速度: {np.max(np.abs(np.diff(a)/np.diff(t))):.4f} (限制 {J})")
    print(f"总位移: {np.trapz(v, t):.4f} (目标 {D})")

运行与验证

直接运行脚本，控制台输出各约束的实测最大值。通过条件：

最大速度 ≤ Vmax（误差 < 1%）；
最大加速度 ≤ Amax；
最大加加速度 ≤ J；
位移积分与目标 D 误差 < 1%。

可视化验证

上图示意速度曲线（红色实线）与加速度曲线（绿色虚线）的典型形状。实际运行时可用 matplotlib 绘制精确曲线：

import matplotlib.pyplot as plt
plt.figure(figsize=(10,4))
plt.subplot(1,2,1); plt.plot(t, v); plt.title("速度曲线")
plt.subplot(1,2,2); plt.plot(t, a); plt.title("加速度曲线")
plt.tight_layout(); plt.show()

常见问题

距离过短：当 D 小于最小可行距离时，规划器会输出异常。最小距离约为 2×J×T³，其中 T = Amax/J。
参数单位：确保 D(m)、Vmax(m/s)、Amax(m/s²)、J(m/s³) 单位一致。
离散化误差：dt 越小精度越高，但计算量增大。实际应用中 dt 取控制周期（如 1ms）。