调试一台点胶机的时候我遇到一个很典型的现象机头在高速来回点胶时换向位置有明显顿挫感点胶轨迹末端偶尔还会出现一小段波纹。最开始怀疑是机械装配间隙后来把伺服波形拉出来一看速度指令从最高速直接以最大加速度往下压加速度曲线是标准矩形——典型的梯形加减速加速度在切换瞬间跳变机械上表现为冲击。把算法换成S形曲线速度规划之后顿挫感几乎消失。但真正让我决定写这篇文章的是后面另一台设备换成标准7段S形后加减速时间反而变长了设备节拍跟不上。后来我把匀加速段、匀减速段去掉改成5段式S形速度规划问题才彻底解决。这篇文章把这套东西完整复盘一遍包括5段式为什么会出现、公式怎么推、实际怎么算、代码怎么写以及在现场最容易踩的几个坑。1. 梯形加减速的痛点与S形曲线的出场逻辑1.1 加速度跳变是怎么引起机械冲击的速度规划本质是回答一个问题从A点到B点速度随时间怎么变化。很多初版程序都用梯形加减速匀加速、匀速、匀减速三段实现简单代码不超过二十行。但它有个很难绕过去的毛病——加速度突变。梯形曲线的速度是连续的在加速段和匀速段的交界处速度没有跳变但加速度有。加速度从 a_max 瞬间变成 0对应的加加速度jerk是无穷大。实际机构有弹性不会真产生无穷大冲击但那些力会以机械振动的形式释放出来——表现就是末端抖动、轨迹波纹、螺丝松动严重时加工表面出现振纹。这里可以做一个生活化类比坐车时如果司机一脚油门踩到最大然后保持身体会猛地后仰这是加速度突变的体感优秀司机踩油门有个过程推背感是逐渐出现的这就是 jerk 被限制了。S形曲线就是让加速度按斜坡变化而不是按台阶变化。S形速度规划把加速度的上升和下降过程也规划出来让 jerk 保持一个有限值。速度曲线不再是折线而是一条S形所以叫 S形曲线速度规划。标准S形曲线通常有7段加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速段、减减速段也就是常说的七段式。1.2 7段式到5段式少的到底是谁七段式里加加速段是加速度从0按jerk增大到Amax匀加速段是保持Amax不变减加速段是加速度从Amax减到0。减速侧有对称的三段中间夹一个匀速段。所谓5段式就是把匀加速段和匀减速段去掉加速度爬升到峰值之后立刻开始下降加速度轮廓从梯形变成三角形。为什么能去掉因为很多实际工况里位移距离并不长或者速度切换根本不需要达到系统允许的Amax。举个例子一台XY平台做短行程高速往复运动行程只有5mm运动过程可能70%时间在加减速。如果按7段式规划匀加速段只存在极短时间甚至不存在算法反而复杂还要处理某些段时间接近0的退化分支。5段式用更少的参数覆盖了绝大多数加减速场景代价只是最大速度利用率略低。需要说清楚5段式不是比7段式低级而是针对特定工况的简化。是位移约束主导还是加速度约束主导决定选哪种。简单判断方式加加速段结束时的速度增量如果已经超过目标速度变化量说明加速过程根本不需要匀加速段5段式足够反过来则需要7段式。2. 5段式S形曲线的数学结构与关键公式推导2.1 五个阶段的划分我习惯把5段式划分为第1段加加速段加速度从0线性增加到峰值Ajerk J第2段减加速段加速度从A线性减小到0jerk -J第3段匀速段速度保持Vc不变第4段加减速段加速度从0反向线性增加到 -Ajerk -J第5段减减速段加速度从 -A 反向减小到0jerk J加速侧速度和加速度符合二次抛物线关系。设两段半段时间各为T加加速段结束时速度 v1 1/2 · J · T²减加速段结束时速度 v2 J · T²。所以加速侧的速度增量Δv J · T²峰值加速度A J · T联立消去T得到一个非常关键的式子A sqrt(J · Δv)这个式子决定了5段式能不能用。如果目标速度变化量 Δv 已知jerk 已知那么要达到这个速度变化加速度峰值必须是这个值。只要算出来的A小于等于系统允许的Amax说明5段式能在 jerk 约束内完成任务不需要匀加速段如果算出来大于Amax说明加速度还没到峰值就要超限必须退回7段式或者降低jerk、降低目标速度增量。2.2 位移公式记牢这两个就够了继续推导位移。加加速段的位移s1 1/6 · J · T³减加速段的位移s2 5/6 · J · T³所以加速侧总位移s_a J · T³用峰值加速度A表示T A/J则s_a A³ / J²用速度增量Δv表示T sqrt(Δv/J)则s_a Δv^(3/2) / sqrt(J) Δv · T如果起始速度不是0还要加上起始速度在加速时间内匀速走过的底部位移s_a v0 · 2T Δv^(3/2) / sqrt(J)这两个公式是后面所有计算的基础建议直接记下来。我当年第一次实现时用积分一步一步推浪费了不少时间后来发现只要把这两个公式吃透整个5段式规划器的代码量可以压到一百行以内。减速侧是对称的。从 v2 减速到 ve速度变化量 Δv_d v2 - ve减速时间为2T_dT_d sqrt(Δv_d / J)。减速位移表示为s_d v2 · 2T_d - Δv_d^(3/2) / sqrt(J)2.3 为什么jerk是分段常数而不是连续变化5段式的jerk是分段常数加加速阶段保持J减加速阶段保持 -J。也就是说在段与段交界的地方jerk本身会发生跳变但加速度是连续变化的。物理上jerk跳变意味着加速度曲线出现折点这比梯形曲线的加速度跳变已经柔和了一个数量级。有些控制器追求更高阶的平滑度会采用正弦函数或者高次多项式来规划让jerk也连续。但实际现场调试中5段式对绝大多数设备已经够用。jerk跳变带来的残余激励一般通过提高段切换时的插值精度、或者在伺服环里做低通滤波来吸收后面第5章会说。3. 从目标位移与速度约束反算各段时间长距离和短距离两种工况3.1 判断能否到达设定最大速度实际规划时我们手里有的是起点速度 v0、终点速度 ve、规划最大速度 Vmax、系统最大加速度 Amax、加加速度 J、总位移 S。要反推出每一段的时间和整段运动时间。第一步不把位移代入先判断5段式能否把速度从 v0 加到 VmaxA_req_acc sqrt(J · (Vmax - v0))同样判断减速侧A_req_dec sqrt(J · (Vmax - ve))如果这两个值都小于等于 Amax说明加速和减速都能在 jerk 约束下完成。再算两侧需要的位移如果总位移 S 足够就是长距离模式中间有一段匀速如果不够就是短距离模式根本到不了 Vmax需要反算一个峰值速度 Vp。3.2 长距离工况匀速段怎么分配长距离工况下加速侧半段时间T1 A_req_acc / J加速侧总时间 2T1加速侧位移s_acc v0 · 2T1 (Vmax - v0)^(3/2) / sqrt(J)减速侧对称。匀速段位移s_c S - s_acc - s_dec匀速段时间T3 s_c / Vmax这里有一个边界必须注意如果 s_c 恰好等于0说明总位移刚好够加速到Vmax马上减速也就是临界距离如果 s_c 小于0说明应该进入短距离模式不能硬把匀速段时间算成负数。3.3 短距离工况对称时存在解析解起点终点速度都是0的对称工况是最常见的短行程场景。设峰值速度为 Vp加速侧从0到Vp减速侧从Vp到0两侧各有半段时间 Tp sqrt(Vp / J)。每侧位移s_side Vp^(3/2) / sqrt(J)总位移等于两侧之和S 2 · Vp^(3/2) / sqrt(J)反解Vp (S · sqrt(J) / 2)^(2/3)然后峰值加速度A_p sqrt(J · Vp)算完检查 A_p ≤ Amax 是否成立。成立说明5段式在约束内可行不成立说明位移太短但速度目标太高需要降低Vmax或者降低jerk否则连5段式也保不住只能退化到更简单的梯形斜坡。3.4 非对称工况迭代求解的思路现场经常遇到起点终点速度不为0的情况比如两个运动段衔接时上一段结束时还有速度这一段不能从0开始。这时对称假设失效。工程上有两种处理方式。简单场景用数值迭代假设一个Vp算出加速侧位移和减速侧位移加总与S比较修正Vp一般迭代两三次就收敛因为函数单调性好。复杂场景可以拆成两段分别规划引入一个中间衔接速度作为变量用运动连续条件求解。如果对实时性要求不高直接二分法或者牛顿迭代都是可行的现代MCU跑这个完全无压力。3.5 数值算例给一个完整的数值算例方便你验证自己写的代码。例子某直线轴jerk J 2000 mm/s³最大加速度 Amax 2000 mm/s²最大速度 Vmax 500 mm/s起点终点速度都为0。先看能否到达VmaxA_req sqrt(2000 × 500) 1000 mm/s²小于Amax加速侧可行。加速半段时间 T A_req / J 0.5s。 加速侧位移 s_acc J·T³ 2000 × 0.125 250 mm。减速侧对称也是250mm。两侧加起来正好500mm。如果总位移 S 600mm匀速段位移100mm匀速段时间 100 / 500 0.2s总时间 0.5×4 0.2 2.2s。如果总位移 S 400mm到不了Vmax按短距离解Vp (400 × sqrt(2000) / 2)^(2/3) ≈ 431 mm/sTp sqrt(431 / 2000) ≈ 0.464s总时间 4 × 0.464 ≈ 1.856s可以对比如果用梯形加减速最大加速度可以跑到接近常量加速时间会短一些但冲击大得多。S形用多出来的零点几秒换来了机构稳定性值不值看设备要求。4. 一个可落地的5段式速度规划器实现4.1 规划数据结构与模式判定我习惯用C语言写运动控制底层。先定义规划结果结构体typedef struct { float J; /* 加加速度mm/s^3 */ float A_max; /* 最大加速度mm/s^2 */ float V_max; /* 最大速度mm/s */ float V0, V1; /* 起止速度mm/s */ float T1; /* 加速侧半段时间 */ float T3; /* 匀速段时间 */ float T5; /* 减速侧半段时间 */ float S_acc; /* 加速侧位移 */ float S_dec; /* 减速侧位移 */ float S_c; /* 匀速段位移 */ float V_peak; /* 实际峰值速度 */ int mode; /* 1长距离 0短距离 */ } FiveSegPlan;初始化函数关键是模式判定和参数分配int five_seg_plan_init(FiveSegPlan *p, float v0, float v1, float vmax, float amax, float jerk, float dist) { float a_acc, a_dec, s_acc, s_dec, vp, s_sum; p-V0 v0; p-V1 v1; p-V_max vmax; p-A_max amax; p-J jerk; /* 先判断加减速两侧能否到达Vmax */ a_acc sqrtf(jerk * (vmax - v0)); a_dec sqrtf(jerk * (vmax - v1)); if (a_acc amax a_dec amax) { /* 到达Vmax的位移 */ p-T1 a_acc / jerk; p-T5 a_dec / jerk; s_acc v0 * 2 * p-T1 powf(vmax - v0, 1.5f) / sqrtf(jerk); s_dec vmax * 2 * p-T5 - powf(vmax - v1, 1.5f) / sqrtf(jerk); s_sum s_acc s_dec; if (dist s_sum - 1e-6f) { /* 长距离模式有匀速段 */ p-mode 1; p-V_peak vmax; p-S_acc s_acc; p-S_dec s_dec; p-S_c dist - s_acc - s_dec; p-T3 p-S_c / vmax; return 0; } } /* 短距离模式反算峰值速度这里实现v0v10的对称解 */ vp powf(dist * sqrtf(jerk) / 2.0f, 2.0f / 3.0f); if (sqrtf(jerk * vp) amax) return -1; /* 约束不可满足 */ p-mode 0; p-V_peak vp; p-T1 sqrtf(vp / jerk); p-T5 p-T1; p-S_acc vp * 2 * p-T1 - powf(vp, 1.5f) / sqrtf(jerk); p-S_dec p-S_acc; p-S_c 0; p-T3 0; return 0; }4.2 运行期取位置和速度指令规划是一次性的运行期要反复根据当前时间取位置或速度指令。我通常直接按时段判断float five_seg_get_position(FiveSegPlan *p, float t) { float s; if (p-mode 1) { /* 加速侧 */ if (t p-T1) { return p-V0 * t (1.0f / 6.0f) * p-J * t * t * t; } if (t 2 * p-T1) { float tau t - p-T1; /* 用减加速段公式 */ s (p-V0 0.5f * p-J * p-T1 * p-T1) * tau 0.5f * p-J * p-T1 * tau * tau - (1.0f / 6.0f) * p-J * tau * tau * tau; return p-S_acc s; } ... } }实际项目里我通常还会做一个统一的运动段定时器在当前段结束时自动切换到下一段。代码不复杂关键是每个段的位置公式要和推导一致尤其是减加速段很多人在这里符号搞反导致速度曲线在峰值处对不上。4.3 离散化周期的影响上面公式是连续时间表达式但控制器是周期采样比如1ms或者125us。处理方式有两种一种是把t带入连续公式直接算位置点简单段切换点会有很小的速度量化误差另一种是每一周期累加速度和位置v a * dt; s v * dt; a jerk * dt;这种增量式实现天然贴合实时系统但误差会漂移。我推荐混合方案位置用连续公式计算避免累积误差速度和加速度只用于状态判断和设备监控。这样既准确又简单。4.4 代码里的量纲和符号约定写代码前必须统一单位。常用内部单位建议全部用 mm 和 s不要用 mm/min 或者 rpm 混合否则 pow、sqrt 算出来的数全是错的。反向运动可以规划完后再整体取负号不要在规划器内部到处处理方向容易出bug。5. 实测调参经验与常见坑位5.1 jerk到底怎么选这是现场第一个问题。jerk太小加减速过程长节拍跟不上jerk太大S形退化成梯形冲击没有实质改善。经验上jerk可以按这个范围起步J ≈ (8~20) × Amax / T_expected其中T_expected是你希望加速度爬升的时间。比如希望0.2s内把加速度从0爬升到2000mm/s²那么J ≈ 10000 mm/s³。然后现场听声音、看轨迹波纹逐步微调。刚性好的铸铁机身可以把jerk调高轻量铝型材悬臂结构要降低。设备共振是最直接的参考。如果某个速度段出现明显振纹多半是速度规划激励到了机械固有频率这时候降低jerk比降低最大速度更有效。5.2 参数没超限但依然振动很多人遇到这种情况所有参数都在Amax和Vmax以内为什么还会过冲大概率是5段式实际产生的加速度峰值高于你预期的Amax。看公式 A sqrt(J·Δv)如果你给的位移很短但起点终点速度差很大为了追上速度差加速度峰值会被顶上去。它可能超过Amax但你在代码里只检查了梯形匀速段约束没检查峰值加速度。解决办法初始化函数里一定要加 A_p ≤ Amax 的检查不满足就报错或者降额。不要侥幸以为Amax会天然限定加速度。5.3 终点位置余差离散控制下规划结束时位置和理论终点之间总有一点余差。连续公式算位置时浮点误差和段切换时刻的量化会导致终点差几个脉冲。处理办法是在最后一个减减速段结束后做一次软急停微调直接把剩余位置按很小的速度斜坡走完或者把剩余位置作为step误差补偿给伺服环。另外要小心不要在每个控制周期都对位置取整那样会积累成一个周期性抖动。最终位置指令最好用浮点只有在发给驱动器的最后一步才转成脉冲数取整。5.4 多轴联动时不能逐轴独立规划做XY平台或者龙门双驱时如果每根轴各自调用一个5段式规划器速度峰值、加减速时间可能不一致合成的轨迹会歪尤其在拐角处。正确做法是先根据合成路径长度、限制最大的单轴速度分量做一次整体规划得到一个公共的运动时间再把总位移按各轴比例映射回去。换句话说规划器只规划一个虚拟主轴各轴距离按比例跟从。5段式在时间上是对称的映射起来比7段式简单很多。5.5 和伺服驱动的加速度前馈叠加现代伺服驱动器有加速度前馈功能如果你在上位机做了S形规划驱动侧又同时开启较大的加速度前馈两者叠加可能导致实际电流过大甚至触发过流报警。这时候优先关掉驱动器的加速度前馈只用一个很小的前馈比例让S形规划自己主导加减速过程。这个细节在展会设备和高精度设备上很容易踩。5.6 5段式的扩展思考跑完5段式之后如果你想往更高阶走可以在段切换点用二次曲线替代jerk跳变变成7段甚至9段或者直接用正弦加加速度做速度规划曲线更光滑。但我要说一句实在话对大多数以点胶、贴片、搬运、CNC进给为目标的设备5段式的性能已经够用瓶颈往往在机械刚性和伺服环路上不在规划精度上。把5段式吃透、调顺能解决80%以上的加减速相关问题。最后再分享一个调试习惯我在每次调完一组规划参数后会把速度、加速度、加加速度三条曲线同时记录到日志里和实际机构的振动信号对比。很多看似玄学的机械问题看曲线就能定位到具体是哪个阶段切换点出了问题。实际操作中这个习惯帮我省下的调试时间远远超过写这段文字的时间。