2. S型速度规划（7段式）
任务说明
实现7段式S型速度规划器，输入起始位置、目标位置、最大速度、最大加速度及最大加加速度，输出完整运动轨迹（时间、位置、速度、加速度序列），并正确处理距离过短导致无法达到最大速度或最大加速度的边界条件。
7段式S型速度规划原理
7段式S型速度规划将运动过程分为7个阶段：加加速（T1）、匀加速（T2）、减加速（T3）、匀速（T4）、加减速（T5）、匀减速（T6）、减减速（T7）。各阶段加速度变化率（加加速度jerk）分别为：j、0、-j、0、-j、0、j。当目标距离不足以完成完整7段时，需要退化处理：先判断能否达到最大速度，若不能则取消匀速段（T4=0）；再判断能否达到最大加速度，若不能则进一步退化，使加速度峰值低于设定值。
核心代码实现
import numpy as np
class SCurvePlanner:
def __init__(self, q0, q1, vmax, amax, jmax, dt=0.001):
"""
q0: 起始位置
q1: 目标位置
vmax: 最大速度
amax: 最大加速度
jmax: 最大加加速度
dt: 采样周期
"""
self.q0 = q0
self.q1 = q1
self.vmax = abs(vmax)
self.amax = abs(amax)
self.jmax = abs(jmax)
self.dt = dt
self.direction = 1 if q1 >= q0 else -1
self.L = abs(q1 - q0) # 总位移距离
# 规划结果
self.times = [] # 各段时间
self.t_total = 0
self.v_peak = 0 # 实际峰值速度
self.a_peak = 0 # 实际峰值加速度
def plan(self):
"""执行7段式S型规划，自动处理边界条件"""
# 步骤1：计算能到达的最大速度（考虑加速段和减速段对称）
# 若距离足够，可达到vmax；否则需要降低峰值速度
# 加速段距离（从0加速到v，再减速到0）= v^2 / amax + v*amax/jmax
# 简化：先假设能达到vmax，计算所需距离
L_need_vmax = self.vmax**2 / self.amax + self.vmax * self.amax / self.jmax
if self.L >= L_need_vmax:
# 情况A：可以达到最大速度，有匀速段
self.v_peak = self.vmax
self._plan_with_cruise()
else:
# 情况B：无法达到最大速度，需要降低峰值速度
# 求解方程：v^2/amax + v*amax/jmax = L
# 先判断能否达到最大加速度
L_need_amax = self.amax**3 / self.jmax**2 # 三角形速度曲线（无匀速）达到amax所需距离
if self.L >= L_need_amax:
# 情况B1：能达到最大加速度，但达不到最大速度
# 解方程 v^2/amax + v*amax/jmax = L
# 令 x = v/amax，则 x^2*amax + x*amax^2/jmax = L
# x^2 + (amax/jmax)*x - L/amax = 0
a = 1.0
b = self.amax / self.jmax
c = -self.L / self.amax
x = (-b + np.sqrt(b**2 - 4*a*c)) / (2*a)
self.v_peak = x * self.amax
self.a_peak = self.amax
self._plan_no_cruise_with_amax()
else:
# 情况B2：连最大加速度都达不到，需要降低加速度峰值
# 此时速度曲线为三角形，加速度峰值 a_peak 满足：
# L = a_peak^3 / jmax^2 （推导：v_peak = a_peak^2/jmax, 距离 = v_peak^2/a_peak）
self.a_peak = (self.L * self.jmax**2) ** (1.0/3.0)
self.v_peak = self.a_peak**2 / self.jmax
self._plan_triangle()
# 生成轨迹点
return self._generate_trajectory()
def _plan_with_cruise(self):
"""情况A：完整7段或6段（无匀加速/匀减速）"""
# 加速段时间（T1=T3）
Ta = self.vmax / self.amax
# 加速段距离
La = self.vmax * Ta # 实际 = 0.5*amax*Ta^2 + vmax*Ta - 0.5*amax*Ta^2 = vmax*Ta
# 若加速段距离*2 > 总距离，说明没有真正的匀速段，但这种情况已在plan()中排除
# 匀速段时间
Tc = (self.L - 2*La) / self.vmax
# 各段时间
T1 = Ta / 2
T2 = Ta / 2
T3 = Ta / 2
T4 = Tc
T5 = Ta / 2
T6 = Ta / 2
T7 = Ta / 2
self.times = [T1, T2, T3, T4, T5, T6, T7]
self.t_total = sum(self.times)
def _plan_no_cruise_with_amax(self):
"""情况B1：无匀速段，但能达到最大加速度"""
# 加速到v_peak再减速到0，无匀速
# 加速段时间 Ta = v_peak/amax + amax/jmax （实际分两段：加加速+匀加速+减加速）
# 但更精确：加加速时间 T1 = amax/jmax，匀加速时间 T2 = v_peak/amax - amax/jmax
# 减加速时间 T3 = amax/jmax
T1 = self.amax / self.jmax
T2 = self.v_peak / self.amax - T1
T3 = T1
T4 = 0
T5 = T1
T6 = T2
T7 = T1
# 验证：若T2为负，说明amax/jmax > v_peak/amax，即v_peak < amax^2/jmax
# 这种情况实际是情况B2，但plan()中已通过L_need_amax判断排除
if T2 < 0:
T2 = 0
T6 = 0
self.times = [T1, T2, T3, T4, T5, T6, T7]
self.t_total = sum(self.times)
def _plan_triangle(self):
"""情况B2：三角形速度曲线，加速度峰值低于设定值"""
# 加加速时间 = 减加速时间 = a_peak/jmax
T1 = self.a_peak / self.jmax
T2 = 0
T3 = T1
T4 = 0
T5 = T1
T6 = 0
T7 = T1
self.times = [T1, T2, T3, T4, T5, T6, T7]
self.t_total = sum(self.times)
def _generate_trajectory(self):
"""根据各段时间生成轨迹点"""
t = 0
times = []
positions = []
velocities = []
accelerations = []
# 当前状态
pos = 0
vel = 0
acc = 0
# 各段参数
T1, T2, T3, T4, T5, T6, T7 = self.times
j = self.jmax
# 段1：加加速 (0 <= t < T1)
t_seg = 0
while t_seg < T1:
acc = j * t_seg
vel = 0.5 * j * t_seg**2
pos = (1.0/6.0) * j * t_seg**3
times.append(t)
positions.append(pos)
velocities.append(vel)
accelerations.append(acc)
t_seg += self.dt
t += self.dt
# 段2：匀加速 (T1 <= t < T1+T2)
if T2 > 0:
acc_start = j * T1
vel_start = 0.5 * j * T1**2
pos_start = (1.0/6.0) * j * T1**3
t_seg = 0
while t_seg < T2:
acc = acc_start
vel = vel_start + acc_start * t_seg
pos = pos_start + vel_start * t_seg + 0.5 * acc_start * t_seg**2
times.append(t)
positions.append(pos)
velocities.append(vel)
accelerations.append(acc)
t_seg += self.dt
t += self.dt
# 段3：减加速 (T1+T2 <= t < T1+T2+T3)
t_seg = 0
T3_start = T1 + T2
acc_start = j * T1
vel_start = 0.5 * j * T1**2 + acc_start * T2
pos_start = (1.0/6.0) * j * T1**3 + vel_start * T2 + 0.5 * acc_start * T2**2
while t_seg < T3:
acc = acc_start - j * t_seg
vel = vel_start + acc_start * t_seg - 0.5 * j * t_seg**2
pos = pos_start + vel_start * t_seg + 0.5 * acc_start * t_seg**2 - (1.0/6.0) * j * t_seg**3
times.append(t)
positions.append(pos)
velocities.append(vel)
accelerations.append(acc)
t_seg += self.dt
t += self.dt
# 段4：匀速
if T4 > 0:
vel_cruise = self.v_peak
pos_start = positions[-1] if positions else 0
t_seg = 0
while t_seg < T4:
acc = 0
vel = vel_cruise
pos = pos_start + vel_cruise * t_seg
times.append(t)
positions.append(pos)
velocities.append(vel)
accelerations.append(acc)
t_seg += self.dt
t += self.dt
# 段5：加减速
t_seg = 0
pos_start = positions[-1] if positions else 0
vel_start = velocities[-1] if velocities else 0
while t_seg < T5:
acc = -j * t_seg
vel = vel_start - 0.5 * j * t_seg**2
pos = pos_start + vel_start * t_seg - (1.0/6.0) * j * t_seg**3
times.append(t)
positions.append(pos)
velocities.append(vel)
accelerations.append(acc)
t_seg += self.dt
t += self.dt
# 段6：匀减速
if T6 > 0:
acc_start = -j * T5
vel_start = velocities[-1] if velocities else 0
pos_start = positions[-1] if positions else 0
t_seg = 0
while t_seg < T6:
acc = acc_start
vel = vel_start + acc_start * t_seg
pos = pos_start + vel_start * t_seg + 0.5 * acc_start * t_seg**2
times.append(t)
positions.append(pos)
velocities.append(vel)
accelerations.append(acc)
t_seg += self.dt
t += self.dt
# 段7：减减速
t_seg = 0
pos_start = positions[-1] if positions else 0
vel_start = velocities[-1] if velocities else 0
acc_start = -j * T5 # 与段5结束时的加速度相同
while t_seg < T7:
acc = acc_start + j * t_seg
vel = vel_start + acc_start * t_seg + 0.5 * j * t_seg**2
pos = pos_start + vel_start * t_seg + 0.5 * acc_start * t_seg**2 + (1.0/6.0) * j * t_seg**3
times.append(t)
positions.append(pos)
velocities.append(vel)
accelerations.append(acc)
t_seg += self.dt
t += self.dt
# 转换为实际方向
positions = [self.q0 + self.direction * p for p in positions]
return {
'time': np.array(times),
'position': np.array(positions),
'velocity': np.array(velocities) * self.direction,
'acceleration': np.array(accelerations) * self.direction,
't_total': self.t_total,
'v_peak': self.v_peak,
'a_peak': self.a_peak,
'segments': self.times
}
# ============ 测试用例 ============
if __name__ == '__main__':
# 测试1：完整7段（距离足够）
planner1 = SCurvePlanner(q0=0, q1=100, vmax=10, amax=5, jmax=20)
traj1 = planner1.plan()
print(f"测试1 完整7段: 总时间={traj1['t_total']:.4f}s, 峰值速度={traj1['v_peak']:.2f}, 峰值加速度={traj1['a_peak']:.2f}")
print(f" 各段时间: {[f'{t:.4f}' for t in traj1['segments']]}")
print(f" 终点位置: {traj1['position'][-1]:.4f} (目标100)")
# 测试2：无匀速段（距离较短）
planner2 = SCurvePlanner(q0=0, q1=20, vmax=10, amax=5, jmax=20)
traj2 = planner2.plan()
print(f"\n测试2 无匀速: 总时间={traj2['t_total']:.4f}s, 峰值速度={traj2['v_peak']:.2f}, 峰值加速度={traj2['a_peak']:.2f}")
print(f" 各段时间: {[f'{t:.4f}' for t in traj2['segments']]}")
print(f" 终点位置: {traj2['position'][-1]:.4f} (目标20)")
# 测试3：三角形速度曲线（距离很短）
planner3 = SCurvePlanner(q0=0, q1=2, vmax=10, amax=5, jmax=20)
traj3 = planner3.plan()
print(f"\n测试3 三角形: 总时间={traj3['t_total']:.4f}s, 峰值速度={traj3['v_peak']:.2f}, 峰值加速度={traj3['a_peak']:.2f}")
print(f" 各段时间: {[f'{t:.4f}' for t in traj3['segments']]}")
print(f" 终点位置: {traj3['position'][-1]:.4f} (目标2)")
# 测试4：反向运动
planner4 = SCurvePlanner(q0=50, q1=0, vmax=8, amax=4, jmax=15)
traj4 = planner4.plan()
print(f"\n测试4 反向: 总时间={traj4['t_total']:.4f}s, 峰值速度={traj4['v_peak']:.2f}")
print(f" 终点位置: {traj4['position'][-1]:.4f} (目标0)")
边界条件处理逻辑
规划器通过三级判断处理边界条件：首先计算达到最大速度所需距离，若总距离不足则降低峰值速度；其次判断在降低速度后能否达到最大加速度，若不能则进一步降低加速度峰值；最终退化为三角形速度曲线。每种情况均通过解析公式直接计算各段持续时间，无需迭代求解。
验证方法
运行上述测试代码，观察输出。判定标准：
- 各测试终点位置与目标位置误差小于0.001（由采样步长引入）
- 测试1应包含7段且T4>0；测试2的T4=0但T2、T6>0；测试3的T2=T4=T6=0
- 全程速度不超过设定vmax，加速度值不超过设定amax（或退化后的峰值）
- 速度曲线连续无跳变，加速度曲线连续（S型特征）
轨迹验证SVG
运行与验证
将代码保存为 s_curve_planner.py，直接运行 python s_curve_planner.py。程序会输出4组测试的规划结果。验证时重点检查：各测试终点位置误差是否在采样步长允许范围内；测试1的7段时间是否均大于0；测试2的T4是否为0；测试3的T2、T4、T6是否为0；所有速度值是否不超过对应测试的峰值速度。若输出满足上述条件，则规划器实现正确。