3 种点到点轨迹规划算法对比：梯形 vs S 型 vs 多项式曲线性能实测

在工业自动化、机器人控制和数控加工等领域，点到点轨迹规划是实现精确运动控制的核心技术。不同的轨迹规划算法会直接影响系统的动态性能、运动平滑度和机械冲击。本文将深入对比三种主流算法——梯形速度曲线、S型（七段式）速度曲线和三次多项式曲线，通过实测数据揭示它们在不同应用场景下的表现差异。

1. 轨迹规划基础与算法原理

轨迹规划的本质是在给定起点和终点的条件下，生成一条满足运动约束（如速度、加速度限制）的平滑路径。理想的轨迹规划算法需要平衡以下关键指标：

• 运动平滑性：避免速度或加速度的突变，减少机械振动
• 时间最优性：在约束条件下最快到达目标点
• 计算效率：适合实时控制系统实现
• 参数可控性：便于调整以适应不同负载和工况

1.1 梯形速度曲线

梯形速度曲线是最经典的轨迹规划方法，其速度-时间曲线呈现明显的三个阶段：

1. 匀加速阶段：加速度恒定，速度线性增加
2. 匀速阶段：速度保持峰值，加速度为零
3. 匀减速阶段：减速度恒定，速度线性降低

数学表达上，各阶段的运动方程如下：

# 匀加速阶段 (0 ≤ t < Ta) position = q0 + v0*t + 0.5*aa*t**2 velocity = v0 + aa*t acceleration = aa # 匀速阶段 (Ta ≤ t < Ta+Tv) position = q0 + La + vv*(t-Ta) velocity = vv acceleration = 0 # 匀减速阶段 (Ta+Tv ≤ t ≤ Ta+Tv+Td) position = q0 + La + Lv + vv*(t-Ta-Tv) + 0.5*ad*(t-Ta-Tv)**2 velocity = vv + ad*(t-Ta-Tv) acceleration = ad

1.2 S型速度曲线（七段式）

S型曲线是对梯形曲线的改进，通过引入加加速度（Jerk）控制，将加速度变化也变为连续过程。典型七段式S曲线包含：

1. 加加速阶段（Jerk为正）
2. 匀加速阶段（Jerk为零）
3. 减加速阶段（Jerk为负）
4. 匀速阶段
5. 加减速阶段（Jerk为负）
6. 匀减速阶段
7. 减减速阶段（Jerk为正）

其数学描述更为复杂，需要分段处理加速度的变化：

# 加加速阶段 jerk = Jmax acceleration = aa + Jmax*t velocity = v0 + aa*t + 0.5*Jmax*t**2 position = q0 + v0*t + 0.5*aa*t**2 + (1/6)*Jmax*t**3

1.3 三次多项式曲线

三次多项式曲线采用时间的三次函数来描述位置变化：

q(t) = a0 + a1*t + a2*t² + a3*t³

通过边界条件（起始/终止位置、速度）可以解出系数：

2. 关键性能指标对比测试

我们在相同位移（2π弧度）、相同最大速度（0.01单位/秒）和相同加速度（0.0001单位/秒²）约束下，对三种算法进行了仿真测试。

2.1 运动连续性对比

注：Cⁿ表示n阶导数连续

实测加速度曲线对比图显示，梯形曲线在阶段转换时出现明显的阶跃（理论上为无限大Jerk），而S型曲线展现出完美的平滑过渡。

2.2 计算复杂度分析

我们统计了生成轨迹所需的浮点运算次数：

虽然三次多项式计算最简单，但它无法直接约束加速度和Jerk，实际应用中需要后处理校验。

2.3 轨迹平滑度实测

通过傅里叶分析速度频谱，我们发现：

• 梯形曲线在高频部分有明显能量分布（机械振动风险）
• S型曲线能量集中在低频区域（运动更平稳）
• 三次多项式介于两者之间

3. 实际应用场景建议

根据测试结果，我们给出以下选型建议：

3.1 推荐梯形曲线的场景

• 低成本控制系统：MCU资源有限时
• 轻负载应用：机械刚度高，振动影响小
• 短距离运动：难以发挥S型曲线优势
• 实时性要求高：如需要μs级响应

3.2 推荐S型曲线的场景

• 精密加工设备：如CNC机床
• 重载机器人：减少机械冲击
• 长行程运动：充分发挥平滑优势
• 对振动敏感：如光学平台

3.3 推荐多项式曲线的场景

• 路径约束复杂：需要高阶连续性
• 离线规划：可接受迭代计算
• 学术研究：作为更复杂算法基础

4. 核心代码实现对比

我们提取了三种算法的关键计算模块，展示其实现差异：

4.1 梯形曲线核心算法

def trapezoidal_plan(q0, q1, v0, v1, vmax, aa, ad): h = q1 - q0 # 计算理论可达最大速度 vf = sqrt((2*aa*ad*h - aa*v1**2 + ad*v0**2)/(ad - aa)) # 确定实际使用速度 vv = min(vf, vmax) # 计算各段时间 Ta = (vv - v0)/aa Td = (v1 - vv)/ad Tv = (h - (vv**2 - v0**2)/(2*aa) - (v1**2 - vv**2)/(2*ad))/vv return Ta, Tv, Td

4.2 S型曲线七段判断逻辑

def s_curve_segment_check(v0, v1, vmax, amax, jmax): # 计算各段时间 tj1 = min(sqrt(abs(vmax-v0)/jmax), amax/jmax) ta = (vmax - v0)/amax - tj1 tj2 = min(sqrt(abs(v1-vmax)/jmax), amax/jmax) td = (v1 - vmax)/amax - tj2 # 判断七段是否都存在 if ta > 0 and td > 0: return 7 # 完整七段 elif ta <= 0 < td: return 5 # 无匀加速段 elif td <= 0 < ta: return 5 # 无匀减速段 else: return 3 # 基本三段

4.3 三次多项式系数计算

def cubic_polynomial_coeff(q0, q1, v0, v1, T): a0 = q0 a1 = v0 a2 = (3*(q1-q0)/T**2) - (2*v0 + v1)/T a3 = (-2*(q1-q0)/T**3) + (v0 + v1)/T**2 return [a0, a1, a2, a3]

在实际项目中，选择轨迹规划算法需要综合考虑机械特性、控制硬件性能和运动要求。对于大多数工业应用，S型曲线在性能和实现复杂度之间提供了最佳平衡。