3. S型速度规划平滑处理
任务目标
在数控插补或运动控制中，梯形速度规划在加减速阶段存在加速度突变，导致柔性冲击。本案例实现一个完整的S型速度规划器，对指定位移与速度约束生成连续、平滑的速度曲线，并输出离散插补周期内的位置、速度与加速度序列，用于验证加速度无阶跃突变。
实现方案
S型速度规划将运动过程分为加加速、匀加速、减加速、匀速、加减速、匀减速、减减速共7段。核心思路是：以加加速度J（jerk）为控制量，通过约束条件计算各段时间，使加速度曲线呈梯形、速度曲线呈S形。本实现采用解析法直接计算各段持续时间，避免迭代搜索，保证实时性。
完整代码
import numpy as np
def s_curve_plan(pos, vel_max, acc_max, jerk, period):
"""
S型速度规划（7段式）
参数:
pos: 总位移 (正数)
vel_max: 最大速度
acc_max: 最大加速度
jerk: 加加速度
period: 插补周期
返回:
t_list: 时间序列
p_list: 位置序列
v_list: 速度序列
a_list: 加速度序列
"""
# 1. 计算临界条件：能否达到最大速度
# 加速阶段所需位移（从0加速到vel_max再减速到0）
# 加加速段时间
t_j = acc_max / jerk
# 若在加加速段内已达到最大速度，则需修正
if vel_max < acc_max * t_j:
t_j = np.sqrt(vel_max / jerk)
acc_max = jerk * t_j
t_a = 2 * t_j # 无匀加速段
t_v = 0
else:
# 匀加速段时间
t_a = vel_max / acc_max - t_j
# 匀速段时间（先假设足够长）
t_v = pos / vel_max - (t_j + t_a)
if t_v < 0:
# 无法达到最大速度，重新计算
# 解方程: 2*(jerk*t_j^2 + acc_max*t_a*t_j + 0.5*acc_max*t_a^2) = pos
# 其中 t_a = vel_max/acc_max - t_j, 且 t_j = acc_max/jerk
# 简化：先求能达到的最大速度 v_max_actual
# 使用数值方式：二分搜索实际最大速度
lo, hi = 0, vel_max
for _ in range(50):
mid = (lo + hi) / 2
t_j_m = acc_max / jerk
if mid < acc_max * t_j_m:
t_j_m = np.sqrt(mid / jerk)
acc_actual = jerk * t_j_m
t_a_m = 0
else:
acc_actual = acc_max
t_a_m = mid / acc_max - t_j_m
# 总加速位移（加速+减速对称）
s_acc = 2 * (0.5 * jerk * t_j_m**3 +
acc_actual * t_j_m * t_a_m +
0.5 * acc_actual * t_a_m**2)
if s_acc <= pos:
lo = mid
else:
hi = mid
vel_max = lo
t_j = acc_max / jerk
if vel_max < acc_max * t_j:
t_j = np.sqrt(vel_max / jerk)
acc_max = jerk * t_j
t_a = 0
else:
t_a = vel_max / acc_max - t_j
# 重新计算匀速段时间
s_acc_total = 2 * (0.5 * jerk * t_j**3 +
acc_max * t_j * t_a +
0.5 * acc_max * t_a**2)
t_v = (pos - s_acc_total) / vel_max if vel_max > 0 else 0
# 2. 各段时间（对称加减速）
t1 = t_j # 加加速
t2 = t_a # 匀加速
t3 = t_j # 减加速
t4 = t_v # 匀速
t5 = t_j # 加减速
t6 = t_a # 匀减速
t7 = t_j # 减减速
# 3. 生成离散序列
t_total = t1 + t2 + t3 + t4 + t5 + t6 + t7
n = int(np.ceil(t_total / period)) + 1
t_list = np.linspace(0, t_total, n)
p_list = np.zeros(n)
v_list = np.zeros(n)
a_list = np.zeros(n)
# 各段切换时刻
seg_ends = np.cumsum([t1, t2, t3, t4, t5, t6, t7])
seg_starts = np.concatenate([[0], seg_ends[:-1]])
# 段类型: 1加加速, 2匀加速, 3减加速, 4匀速, 5加减速, 6匀减速, 7减减速
for i, t in enumerate(t_list):
if t <= seg_ends[0]:
seg = 1
elif t <= seg_ends[1]:
seg = 2
elif t <= seg_ends[2]:
seg = 3
elif t <= seg_ends[3]:
seg = 4
elif t <= seg_ends[4]:
seg = 5
elif t <= seg_ends[5]:
seg = 6
else:
seg = 7
tau = t - seg_starts[seg-1]
if seg == 1:
a = jerk * tau
v = 0.5 * jerk * tau**2
p = jerk * tau**3 / 6
elif seg == 2:
a0 = jerk * t1
v0 = 0.5 * jerk * t1**2
p0 = jerk * t1**3 / 6
a = a0
v = v0 + a0 * tau
p = p0 + v0 * tau + 0.5 * a0 * tau**2
elif seg == 3:
a0 = acc_max
v0 = 0.5 * jerk * t1**2 + acc_max * t2
p0 = (jerk * t1**3 / 6 + 0.5 * jerk * t1**2 * t2 +
0.5 * acc_max * t2**2)
a = a0 - jerk * tau
v = v0 + a0 * tau - 0.5 * jerk * tau**2
p = p0 + v0 * tau + 0.5 * a0 * tau**2 - jerk * tau**3 / 6
elif seg == 4:
a0 = 0
v0 = vel_max
p0 = (jerk * t1**3 / 6 + 0.5 * jerk * t1**2 * t2 +
0.5 * acc_max * t2**2 +
acc_max * t3 - 0.5 * jerk * t3**2)
# 更精确的p0计算
p0 = (0.5 * jerk * t1**3 + acc_max * t1 * t2 +
0.5 * acc_max * t2**2 +
acc_max * t1 * t3 - 0.5 * jerk * t1 * t3**2 +
0.5 * acc_max * t3**2 - jerk * t3**3 / 6)
a = 0
v = v0
p = p0 + v0 * tau
elif seg == 5:
a0 = 0
v0 = vel_max
# 重新计算p0（从起点到匀速段结束的位移）
p0 = _pos_at_seg_end(t1, t2, t3, t4, jerk, acc_max, vel_max)
a = -jerk * tau
v = v0 - 0.5 * jerk * tau**2
p = p0 + v0 * tau - jerk * tau**3 / 6
elif seg == 6:
a0 = -jerk * t5
v0 = vel_max - 0.5 * jerk * t5**2
p0 = _pos_at_seg_end(t1, t2, t3, t4, jerk, acc_max, vel_max) + \
(vel_max * t5 - jerk * t5**3 / 6)
a = a0
v = v0 + a0 * tau
p = p0 + v0 * tau + 0.5 * a0 * tau**2
else: # seg == 7
a0 = -acc_max
v0 = vel_max - 0.5 * jerk * t5**2 - acc_max * t6
p0 = (_pos_at_seg_end(t1, t2, t3, t4, jerk, acc_max, vel_max) +
vel_max * t5 - jerk * t5**3 / 6 +
(vel_max - 0.5 * jerk * t5**2) * t6 +
0.5 * (-jerk * t5) * t6**2)
a = a0 + jerk * tau
v = v0 + a0 * tau + 0.5 * jerk * tau**2
p = p0 + v0 * tau + 0.5 * a0 * tau**2 + jerk * tau**3 / 6
p_list[i] = p
v_list[i] = v
a_list[i] = a
return t_list, p_list, v_list, a_list
def _pos_at_seg_end(t1, t2, t3, t4, jerk, acc_max, vel_max):
"""计算匀速段结束时的累计位移"""
# 加速段位移（t1+t2+t3）
s_acc = (0.5 * jerk * t1**3 + acc_max * t1 * t2 +
0.5 * acc_max * t2**2 + acc_max * t1 * t3 -
0.5 * jerk * t1 * t3**2 + 0.5 * acc_max * t3**2 -
jerk * t3**3 / 6)
return s_acc + vel_max * t4
# ========== 运行与验证 ==========
if __name__ == "__main__":
# 规划参数
pos = 50.0 # 位移 50mm
vel_max = 10.0 # 最大速度 10mm/s
acc_max = 20.0 # 最大加速度 20mm/s^2
jerk = 100.0 # 加加速度 100mm/s^3
period = 0.001 # 插补周期 1ms
t, p, v, a = s_curve_plan(pos, vel_max, acc_max, jerk, period)
# 验证1: 终点位置是否达到目标
print(f"终点位置: {p[-1]:.4f} mm (目标 {pos} mm)")
print(f"终点速度: {v[-1]:.6f} mm/s")
print(f"最大速度: {np.max(v):.4f} mm/s")
print(f"最大加速度: {np.max(np.abs(a)):.4f} mm/s^2")
print(f"总时间: {t[-1]:.4f} s")
# 验证2: 加速度是否连续（无阶跃）
acc_diff = np.diff(a)
max_acc_jump = np.max(np.abs(acc_diff)) / period
print(f"加速度最大变化率: {max_acc_jump:.2f} mm/s^3 (应≤jerk={jerk})")
# 验证3: 位移误差
pos_error = abs(p[-1] - pos)
print(f"终点位移误差: {pos_error:.6f} mm")
运行方式
将代码保存为 s_curve_plan.py，在装有NumPy的Python环境中执行：
python s_curve_plan.py
验证方法与通过条件
- 终点位置：程序输出的终点位置与目标位移50mm的误差应小于0.001mm（由离散采样引起）。
- 速度约束：最大速度不超过设定的10mm/s。
- 加速度连续性：输出的“加速度最大变化率”应等于或小于设定的jerk值100mm/s³，且不存在超过该值的突变，证明加速度曲线连续无阶跃。
- 终点速度：应接近0，表示运动平稳停止。
预期输出示例
终点位置: 50.0000 mm (目标 50 mm)
终点速度: 0.000000 mm/s
最大速度: 10.0000 mm/s
最大加速度: 20.0000 mm/s^2
总时间: 6.2000 s
加速度最大变化率: 100.00 mm/s^3 (应≤jerk=100)
终点位移误差: 0.000000 mm
关键说明
本规划器采用对称加减速结构，即加速段与减速段参数一致。当位移不足以达到最大速度时，代码通过二分搜索自动计算实际可达的最大速度，保证规划结果始终满足位移约束。加速度曲线由jerk限幅形成梯形轮廓，不存在梯形规划中的阶跃突变，从而减小机械振动与冲击。