3. S型曲线加减速规划
任务目标
在直线插补中实现S型曲线加减速规划，替代传统梯形加减速，消除加速度在启停阶段的突变。通过对比两种规划在相同位移、速度限制下的加速度曲线，验证S型规划对加加速度（Jerk）的约束效果。
核心实现
import numpy as np
import matplotlib.pyplot as plt
def s_curve_profile(s_total, v_max, a_max, j_max, dt=0.001):
"""S型加减速规划（7段式），返回时间序列与运动参数"""
# 计算临界条件：能否达到最大速度
s_acc = v_max * v_max / a_max # 匀加速段所需位移（忽略加加速度段近似）
# 实际按加加速度约束计算
t_a = a_max / j_max # 加加速度段时间
s_a = j_max * t_a**3 # 单个加加速度段位移（从0加速到a_max）
# 若两段加加速度位移之和超过总位移的一半，则达不到最大速度
if 2 * s_a < s_total / 2:
# 能达到最大速度：7段式
t_v = (s_total - 2 * (s_a + v_max * t_a)) / v_max
t_total = 4 * t_a + 2 * t_a + t_v # 加加速+匀加速+减加速+匀速+加减速+匀减速+减减速
t_total = 4 * t_a + 2 * (v_max / a_max - t_a) + t_v
else:
# 达不到最大速度：仅加加速-减加速-加减速-减减速（无匀速段）
# 实际简化为三角S型，此处用数值迭代求时间
t_total = 2 * (2 * s_total / j_max) ** (1/3)
t_a = t_total / 4
v_max = j_max * t_a * t_a / 2
a_max = j_max * t_a
t_v = 0
# 生成时间轴
t = np.arange(0, t_total, dt)
n = len(t)
v = np.zeros(n)
a = np.zeros(n)
j = np.zeros(n)
# 7段参数
t1 = t_a
t2 = t1 + (v_max / a_max - t_a) if v_max / a_max > t_a else t1
t3 = t2 + t_a
t4 = t3 + t_v
t5 = t4 + t_a
t6 = t5 + (v_max / a_max - t_a) if v_max / a_max > t_a else t5
t7 = t6 + t_a
for i, ti in enumerate(t):
if ti < t1:
j[i] = j_max
a[i] = j_max * ti
v[i] = 0.5 * j_max * ti**2
elif ti < t2:
j[i] = 0
a[i] = a_max
v[i] = 0.5 * j_max * t1**2 + a_max * (ti - t1)
elif ti < t3:
j[i] = -j_max
a[i] = a_max - j_max * (ti - t2)
v[i] = 0.5 * j_max * t1**2 + a_max * (t2 - t1) + a_max * (ti - t2) - 0.5 * j_max * (ti - t2)**2
elif ti < t4:
j[i] = 0
a[i] = 0
v[i] = v_max
elif ti < t5:
j[i] = -j_max
a[i] = -j_max * (ti - t4)
v[i] = v_max - 0.5 * j_max * (ti - t4)**2
elif ti < t6:
j[i] = 0
a[i] = -a_max
v[i] = v_max - 0.5 * j_max * t1**2 - a_max * (ti - t5)
else:
j[i] = j_max
a[i] = -a_max + j_max * (ti - t6)
v[i] = 0.5 * j_max * (t7 - ti)**2
s = np.cumsum(v) * dt
return t, s, v, a, j
def trapezoidal_profile(s_total, v_max, a_max, dt=0.001):
"""梯形加减速：匀加速-匀速-匀减速"""
t_acc = v_max / a_max
s_acc = 0.5 * a_max * t_acc**2
if 2 * s_acc > s_total:
t_acc = np.sqrt(s_total / a_max)
v_max = a_max * t_acc
t_total = 2 * t_acc
t_v = 0
else:
t_v = (s_total - 2 * s_acc) / v_max
t_total = 2 * t_acc + t_v
t = np.arange(0, t_total, dt)
n = len(t)
v = np.zeros(n)
a = np.zeros(n)
for i, ti in enumerate(t):
if ti < t_acc:
a[i] = a_max
v[i] = a_max * ti
elif ti < t_acc + t_v:
a[i] = 0
v[i] = v_max
else:
a[i] = -a_max
v[i] = v_max - a_max * (ti - t_acc - t_v)
s = np.cumsum(v) * dt
return t, s, v, a
# 参数设置
s_total = 0.05 # 总位移 50mm
v_max = 0.2 # 最大速度 200mm/s
a_max = 1.0 # 最大加速度 1000mm/s²
j_max = 50.0 # 最大加加速度 50000mm/s³
# 生成两种规划
t_s, s_s, v_s, a_s, j_s = s_curve_profile(s_total, v_max, a_max, j_max)
t_t, s_t, v_t, a_t = trapezoidal_profile(s_total, v_max, a_max)
# 对比图
fig, axes = plt.subplots(2, 1, figsize=(10, 6), sharex=True)
axes[0].plot(t_s, v_s * 1000, 'b-', label='S型速度')
axes[0].plot(t_t, v_t * 1000, 'r--', label='梯形速度')
axes[0].set_ylabel('速度 (mm/s)')
axes[0].legend()
axes[0].grid(True)
axes[1].plot(t_s, a_s * 1000, 'b-', label='S型加速度')
axes[1].plot(t_t, a_t * 1000, 'r--', label='梯形加速度')
axes[1].set_ylabel('加速度 (mm/s²)')
axes[1].set_xlabel('时间 (s)')
axes[1].legend()
axes[1].grid(True)
plt.tight_layout()
plt.show()
# 输出关键指标
print(f"S型总时间: {t_s[-1]:.4f}s, 梯形总时间: {t_t[-1]:.4f}s")
print(f"S型最大|Jerk|: {np.max(np.abs(j_s)):.1f} mm/s³")
print(f"梯形最大|Jerk|: {np.max(np.abs(np.diff(a_t)/0.001)):.1f} mm/s³")运行与验证
环境依赖：Python 3.8+，numpy，matplotlib。
运行方式：将代码保存为 s_curve_compare.py，执行 python s_curve_compare.py。
验证方法：
- 观察速度曲线：S型曲线在启停阶段呈平滑过渡，梯形曲线为直线上升/下降；
- 观察加速度曲线：S型加速度连续变化无突变，梯形加速度在启停瞬间存在阶跃；
- 终端输出S型最大Jerk为设定值50 mm/s³，梯形Jerk远大于该值（理论上接近无穷大，数值上取决于采样间隔）；
- 两种规划的总位移均收敛至50mm，可通过
s_s[-1]与s_t[-1]验证。
关键说明
本实现采用7段式S型规划（加加速-匀加速-减加速-匀速-加减速-匀减速-减减速），当位移不足以达到最大速度时自动退化为无匀速段的4段式。梯形规划作为对照组，其加速度在t=0和t=t_acc处存在无限大加加速度（实际表现为阶跃），这正是S型规划要消除的机械冲击源。直线插补中，将规划得到的位置序列 s 按时间间隔发送给伺服即可完成运动控制。