4. 轨迹规划基础：梯形速度曲线、S形速度曲线、T型与S型对比、多项式插值
各位同学，大家好。今天我们来聊聊轨迹规划。这是Delta机器人高速分拣的核心技术之一。说白了，就是让机器人知道怎么从A点走到B点，走得更快、更稳、更准。
我刚开始做Delta机器人时，觉得轨迹规划不就是算个路径吗？后来发现，这里面门道很深。一个不好的轨迹，轻则抖动、定位不准，重则直接触发过载报警。嗯，今天我把这块掰开揉碎了讲清楚。
4.1 梯形速度曲线（T型曲线）
梯形速度曲线，也叫T型曲线。它是最基础的轨迹规划方式。为什么叫梯形？你想想看，速度随时间的变化图，就像一个梯形——先匀加速，再匀速，最后匀减速。
它的数学表达很简单：
加速段：v(t) = v0 + a*t
匀速段：v(t) = v_max
减速段：v(t) = v_max - a*t其中v0是初速度，v_max是最大速度，a是加速度。
优点：实现简单，计算量小。我在早期的一个项目中，用STM32跑T型曲线，CPU占用率不到5%。
缺点：加速度在拐点处突变。你想想看，从加速突然切换到匀速，加速度从a瞬间变成0。这会导致什么？冲击！机器会抖一下。
4.2 S形速度曲线
S形速度曲线，就是为了解决T型曲线的冲击问题。它把加速度也做了平滑处理——加速度从0开始，逐渐增加到最大值，再逐渐减小到0。
完整的S形曲线分为7段：
- 加加速度段（jerk为正，加速度增加）
- 匀加速段（加速度恒定）
- 减加速度段（jerk为负，加速度减小）
- 匀速段
- 加减速度段（jerk为负，加速度反向增加）
- 匀减速段
- 减减速度段（jerk为正，加速度反向减小）
这里引入了一个新概念——加加速度（Jerk）。它是加速度的变化率。说白了，就是加速度的变化有多快。
核心公式：
位置：s(t) = s0 + v0*t + (1/2)*a0*t² + (1/6)*J*t³
速度：v(t) = v0 + a0*t + (1/2)*J*t²
加速度：a(t) = a0 + J*t
其中J是加加速度（Jerk），单位m/s³
我个人习惯用S形曲线做高速分拣。虽然计算量大了点，但胜在平滑。我建议，如果你的机器人运行速度超过200次/分钟，优先考虑S形曲线。
4.3 T型与S型对比
咱们直接上对比表，一目了然：
| 对比项 | T型曲线 | S形曲线 |
|---|---|---|
| 加速度变化 | 阶跃突变 | 连续平滑 |
| Jerk | 无穷大（拐点处） | 有限值，可控 |
| 运动时间 | 较短（同等条件下） | 稍长（约5-10%） |
| 机械冲击 | 大 | 小 |
| 计算复杂度 | 低 | 中高 |
| 适用场景 | 低速、低精度 | 高速、高精度 |
你可能会问：既然S形曲线这么好，为什么还要用T型？原因很简单——实时性。在一些低成本控制器上，S形曲线的计算量可能吃不消。我见过一个项目，用8位单片机跑S形曲线，结果每周期计算时间超过1ms，根本跑不起来。
💡 我的经验：如果控制器算力够，无脑选S形。如果算力紧张，可以试试"简化版S形"——只做3段（加加速、匀速、减加速），效果介于T型和完整S形之间。
4.4 多项式插值
除了T型和S型，还有一种更灵活的轨迹规划方式——多项式插值。它用多项式函数来拟合位置-时间曲线。
常用的多项式插值有：
- 三次多项式：s(t) = a0 + a1*t + a2*t² + a3*t³
- 五次多项式：s(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴ + a5*t⁵
- 七次多项式：s(t) = a0 + a1*t + a2*t² + ... + a7*t⁷
次数越高，能约束的条件越多。三次多项式可以约束起点和终点的位置、速度。五次多项式还能约束加速度。七次多项式连加加速度（Jerk）都能约束。
举个例子，用五次多项式做轨迹规划：
已知起点位置s0、速度v0、加速度a0
终点位置sf、速度vf、加速度af
总时间T
求解系数：
a0 = s0
a1 = v0
a2 = a0/2
a3 = (20*h - 8*vf*T - 12*v0*T - 3*a0*T² + af*T²) / (2*T³)
a4 = (30*h + 14*vf*T + 16*v0*T + 3*a0*T² - 2*af*T²) / (2*T⁴)
a5 = (12*h - 6*vf*T - 6*v0*T - a0*T² + af*T²) / (2*T⁵)
其中 h = sf - s0多项式插值的优点是灵活性极高。你可以任意指定起点和终点的状态。缺点是计算量大，而且如果次数太高，容易出现"龙格现象"——曲线在端点附近剧烈振荡。
⚠️ 注意：多项式插值不适合做长距离运动。我试过用七次多项式跑500mm的位移，结果中间段速度波动很大。后来改用分段多项式（比如分成3段），效果就好多了。
4.5 知识体系总览
下面这张图，是我整理的轨迹规划知识体系。你可以看到，T型、S型、多项式插值，它们之间不是互斥的，而是互补的。
从这张图你可以看到，三种方法各有侧重。T型曲线简单粗暴，适合对冲击不敏感的场合。S型曲线平滑优雅，是高速分拣的主力。多项式插值灵活多变，适合有特殊约束的场景。
4.6 实际选型建议
说了这么多，到底怎么选？我给大家一个实用指南：
- 分拣速度 < 100次/分钟：T型曲线就够了。别折腾，简单就是美。
- 分拣速度 100-200次/分钟：建议用S型曲线。我一般用5段S形（去掉匀加速和匀减速段），效果不错。
- 分拣速度 > 200次/分钟：必须用完整的7段S形曲线。这时候Jerk的控制至关重要。
- 有特殊路径约束：比如必须经过某个中间点，或者起点终点速度不为零，用多项式插值。
💡 一个小技巧：在实际项目中，我经常把T型和S型结合起来用。粗定位用T型快速到位，精定位用S型平滑微调。这样既保证了速度，又兼顾了精度。
好了，轨迹规划的基础就讲到这里。记住一句话：没有最好的轨迹，只有最合适的轨迹。根据你的实际工况，选择最适合的方案。
公众号:蓝海资料掘金营，微信deep3321