做运动控制六七年我踩过最深的坑就是加减速。电机一启动“哐当”一声平台一顿一顿走到终点还过冲严重的时候直接丢步。后来把五段式S型算法从原理到代码完整啃了一遍机器的动作才终于像样。这篇笔记是我的个人复盘围绕五段式S型算法的数学推导、Matlab仿真、单片机实现和现场调试展开尽量写得让做CNC、3D打印、机械臂和AGV的同学都能直接参考。我不打算讲太多教科书理论重点放在“我实际怎么用它”这件事上。先泼一盆冷水五段式S型算法不是银弹它解决的是加减速过程中加速度突变带来的冲击问题。如果你的机械结构刚度太差、传动间隙太大算法能帮你掩盖一部分问题但不可能把一台晃得像果冻的机器变成精密设备。算法负责让指令平滑机械负责把指令执行到位两个都得抓。1. 五段式S型算法到底解决什么问题1.1 梯形加减速的硬伤很多入门教程里电机加减速用的都是梯形速度规划速度从0线性升到目标速度保持匀速跑一段再线性减到0。这类实现简单到不能再简单一个状态机三个阶段就能写完但它有一个非常隐蔽的问题——加速度跳变。梯形曲线的加速度是恒定值所以启动瞬间加速度从0直接跳到某个正数A停止瞬间又从A直接跳回0。这个过程在物理上意味着加加速度jerk趋近于无穷大。机械系统对jerk非常敏感轻则产生振动和噪声重则导致步进电机失步、定位偏差甚至把联轴器震松。我打个比方开手动挡起步离合抬得太快车会“窜”一下副驾的人跟着点头离合慢慢抬车就平稳。梯形加减速就是“快抬离合”S型加减速就是“慢慢抬”。你去看高档数控系统的运动指令几乎没有纯梯形的原因就在这里。1.2 S型曲线和“五段式”的由来S型速度规划的核心思想不是限制加速度大小而是限制加速度的变化率。也就是说让加速度从0逐渐上升到最大值再逐渐降下来速度曲线就从斜直线变成了S形。教科书上的标准S型曲线有七段加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。这个名字看起来很吓人其实每一段对应的是加速度的“上升、保持、下降”过程。那五段式是什么严格说它是七段式在特定条件下的自然裁剪。当运动位移不够大跑不到最大速度时匀速段会消失当位移更小连最大加速度都用不上时匀加速段和匀减速段也会消失最后剩下来的就是加加速、减加速、加减速、减减速这四个过渡段加上中间那个速度峰值点实际跑出来的形态就是“五段”。我在工程笔记里习惯叫它五段式但代码结构通常是按七段框架写的只是把T2和T6这类匀加速段时间自动置零。这样做的好处是不管行程长短同一套代码都能覆盖不需要为每种形态单独写一套规划器。1.3 五段式 vs 七段式怎么选很多人一上来就问是不是七段式一定比五段式好我的答案是否定的。七段式在数学上更完整加速度利用率更高但它多出来的匀加速段和匀减速段在实际机械上未必能带来可感知的差异反而让代码复杂度和调试难度上升。我一般这么选行程长、速度高、对轮廓精度要求极高的加工场景用七段式普通步进电机定位、3D打印、小型机械手、AGV这类点到点运动五段式足够。还有一个经验如果设备本身刚度一般比如铝型材机架加同步带传动七段式带来的收益非常小因为机械瓶颈在刚度不在算法。现场调试还有个更现实的问题五段式参数少只有目标速度、最大加速度、加加速度、总位移四个参数出现异常很好排查。七段式多了匀加速时间调参时多一个变量很多时候你根本分不清问题是出在匀加速段太长还是机械共振。2. 数学推导与关键公式先把曲线拆开看2.1 几个基础变量先把变量统一一下后面所有公式都用这套符号J加加速度单位mm/s³决定了加速度变化的快慢Amax最大加速度单位mm/s²Vp峰值速度单位mm/s可能是设定最大速度也可能是距离不够时的实际峰值S_total总位移单位mmT1、T2、T3加速段三个阶段的时间分别对应加加速、匀加速、减加速加加速度这个名字很多初学者一听就懵其实可以理解成“油门变化的快慢”。加速度是速度的变化率加加速度就是加速度的变化率。J越大加速度建立得越快S型曲线就越接近梯形J越小曲线越“圆润”启动越柔和。2.2 从零加速到峰值速度的位移公式这是整个算法的核心公式没有之一。我要分两种情况讨论因为很多实现写错就是错在这里。第一种情况峰值速度较大加速过程中能达到最大加速度。此时加加速段和减加速段的时间相等均为T1 Amax / J。匀加速段时间T2由剩余的速度增量决定T2 Vp / Amax - Amax / J这种情况下从零加速到Vp的位移为S_acc Vp² / (2 * Amax) Amax * Vp / (2 * J)这个公式前半部分很像匀加速直线运动位移公式后半部分是S型过渡多出来的“额外距离”原因是加速度不是一步到位而是逐渐建立的。第二种情况峰值速度较小加速过程中根本达不到最大加速度。此时加速度曲线是三角形只有加加速和减加速两段时间T1 sqrt(Vp / J)位移为S_acc Vp * sqrt(Vp / J)注意这个公式里的J还是那个加加速度但Amax完全没有参与因为系统根本没有碰到Amax上限。很多人在短行程规划这里翻车就是拿第一种情况的公式硬套结果位移算多了电机跑不到位。2.3 总位移决定曲线形态现在有了加速段位移公式整个过程就变成了一道判断选择题。我从零加速到Vp需要S_acc从Vp减速到零也需要同样的距离这里假设起止速度都为0对称处理。如果S_total 2 * S_acc说明距离足够能跑出匀速段匀速时间T_const (S_total - 2 * S_acc) / Vp。如果S_total 2 * S_acc说明距离不够实际峰值速度Vp必须降低。理论上可以解方程求出Vp但这个方程带根号、带分段手算很烦嵌入式里也最好不要解高次方程。实战做法是二分法。二分法思路很简单在0到Vmax之间猜一个Vp根据它算2 * S_acc和总位移比大小。算出来的距离大了说明Vp猜高了算出来小了说明Vp猜低了。迭代四五十次误差已经小到完全可以忽略。你别觉得二分法土实际工程里它又快又稳比费半天劲解解析解靠谱得多。3. 从公式到代码让曲线真正跑起来3.1 先用Matlab把曲线跑起来动手写单片机代码之前我强烈建议先用Matlab把整条曲线画出来看一眼。速度曲线是不是S形、加速度曲线是不是连续、位移终点对不对一眼就能看出来。下面这段代码是核心部分我做了精简但可以直接运行。它接受四个参数总位移S_total、最大速度Vmax、最大加速度Amax、加加速度J输出时间、速度、加速度、位移四个数组。function [t, v, a, s] s_curve_5seg(S_total, Vmax, Amax, J, dt) if S_total 0 || Vmax 0 || Amax 0 || J 0 error(参数必须为正数); end % 判断能否跑到最大速度 S_full dist_to_speed(Vmax, Amax, J); if 2 * S_full S_total Vp Vmax; else Vp bisect_peak_velocity(S_total, Vmax, Amax, J); end % 计算加速段时间参数 if Vp Amax * Amax / J T1 Amax / J; T2 Vp / Amax - Amax / J; else T1 sqrt(Vp / J); T2 0; end T3 T1; T_acc T1 T2 T3; S_acc dist_to_speed(Vp, Amax, J); T_const (S_total - 2 * S_acc) / Vp; if T_const 0 T_const 0; % 数值保护 end T_dec T_acc; t_end T_acc T_const T_dec; t 0:dt:t_end; v zeros(size(t)); a zeros(size(t)); s zeros(size(t)); % 数值积分生成曲线 for k 2:length(t) tk t(k); if tk T1 ac J * tk; elseif tk T1 T2 ac Amax; elseif tk T_acc ac Amax - J * (tk - T1 - T2); elseif tk T_acc T_const ac 0; elseif tk T_acc T_const T1 ac -J * (tk - T_acc - T_const); elseif tk T_acc T_const T1 T2 ac -Amax; else ac -Amax J * (tk - T_acc - T_const - T1 - T2); end a(k) ac; v(k) v(k-1) ac * dt; s(k) s(k-1) v(k) * dt 0.5 * ac * dt * dt; end end function S dist_to_speed(V, Amax, J) if V Amax * Amax / J S V^2 / (2 * Amax) Amax * V / (2 * J); else S V * sqrt(V / J); end end function Vp bisect_peak_velocity(S_total, Vmax, Amax, J) lo 0; hi Vmax; for k 1:50 mid 0.5 * (lo hi); if 2 * dist_to_speed(mid, Amax, J) S_total hi mid; else lo mid; end end Vp 0.5 * (lo hi); end跑完这个函数你会看到速度曲线是一条平滑的S形加速度曲线没有跳变位移曲线终点精确等于S_total。如果你画出来的速度曲线有尖角说明某段时间参数算错了优先检查T2的符号。3.2 单片机状态机实现Matlab验证完搬到单片机上就需要换一种思路。实时计算三角函数或开方不是不行但很多低成本的MCU跑起来吃力而且中断里做浮点运算容易出问题。我的做法是初始化时算好所有段时间参数存到结构体里然后在定时器中断里根据当前运行时间用状态机判断当前落在哪一段输出对应的目标加速度再积分出速度和目标位置。C语言核心结构体可以这么定义typedef struct { float T1, T2, T3; // 加速段三段 float T_const; // 匀速段时间 float T_acc; // 总加速时间 float Amax, J; // 最大加速度、加加速度 float Vp; // 实际峰值速度 float time_elapsed; // 已运行时间 float velocity; // 当前速度 float position; // 当前目标位置 } SCurve;每次定时器中断里执行一次更新void timer_isr(void) { sc.time_elapsed DT; float ac s_curve_get_acc(sc, sc.time_elapsed); sc.velocity ac * DT; sc.position sc.velocity * DT 0.5f * ac * DT * DT; // 用 sc.position 作为位置环或脉冲发生器的目标值 }其中DT是定时器中断周期s_curve_get_acc函数根据当前时间返回目标加速度逻辑很简单就是一组if判断。提前把T_acc算好中断里避免除法整个ISR执行时间可以控制在几个微秒内。3.3 离散化与精度控制单片机里有个容易被忽略的问题离散化误差。Matlab仿真用很小的dt看不到误差但嵌入式里DT通常要取1ms甚至更大。速度是按DT累加的位置再按速度累加两个积分下来误差会被放大。解决办法有两个方向。如果精度要求不是极端直接用位置闭环用编码器反馈把累积误差吃掉。如果要求开环定位精确就得在生成位置指令时用解析式而不是纯积分。比如加速段每一时刻的目标位置用公式算匀速段用线性位置减速段再切换回解析式。这样每个阶段末端的位置都能和理论值严格对齐。我的习惯是先用纯积分跑通然后看终点误差。实测下来DT1ms、行程100mm时纯积分的终点误差可能到0.1mm级别对步进电机开环来说不一定能接受。加上位置闭环或者改用解析式之后这个问题就消失了。4. 常见问题与排查技巧实录4.1 曲线发尖、启动冲击大这是最典型的调参问题。速度曲线画出来不是S形更像是“两段直线夹一个圆弧”启动时还是能听到明显的“咔”声。基本可以断定加加速度J设置太大加速度建立太快曲线几乎退化成梯形。我一般从大到小调J直到电机啸叫消失、平台不再抖为止。J并不是越大越好也不是越小越好太小的话整个加速过程变得很慢加工效率下降。一个比较实用的起点是让加加速段时间T1 Amax / J 落在20ms到50ms之间。你手头电机性能越好、负载越轻这个时间可以越短。4.2 匀速段速度达不到设定值这个问题主要在计算T2的时候出错。T2 Vp / Amax - Amax / J如果算出来是负数说明Vp在当前Amax和J的组合下根本达不到匀加速段不存在。很多人的代码没做这个判断硬用负数T2去算位移结果速度曲线在中间断了一截。排查方法很简单把Vp、Amax、J三个值代进去检查Vp是否大于等于Amax² / J。不满足就把T2置零同时把峰值速度标成Vp而不是Vmax。另外二分法求峰值速度时也要注意hi初始值必须大于等于可能的Vp否则二分范围不对结果偏小。4.3 定位不准、终点过冲终点过冲最常见的两个原因一是位置累加用了纯速度积分离散误差累积二是减速段距离算少了到终点时速度还没减下来。第二个原因在短行程场景特别容易出现因为你用了“加速段距离等于减速段距离”的对称假设但实际如果起始速度或终止速度不为零这个假设不成立。排查时可以打印一段速度曲线看终点时刻速度是否接近0。如果最后还有速度就是减速段时间或位移算得不够。把起止速度都纳入计算用完整的非对称S型规划这个问题就解掉了。至于位置误差我在前面说过上位置闭环是最省心的方式。4.4 现场调参经验速查调参顺序也有讲究。我习惯先固定J把Amax调到电机不失步上限的八成左右曲线形状对不对先不管保证能定位。然后调J从大往小直到整机震动明显降低。最后再根据实际节拍要求决定是否放宽Amax来缩短加减速时间。现象可能原因处理方式启动冲击大J太大降低J让加加速段更长速度曲线中间断了T2为负未处理检查Vp是否达到Amax²/J终点过冲减速距离算少了改用非对称规划终点有残速减速段时间不足增加减速段时间或降低峰值速度电机发热明显加减速过于频繁适当降低Amax和J5. 扩展从单段规划到连续轨迹5.1 段间速度衔接前面讨论的都是单段点到点运动实际应用中很少真的只跑一段就停。比如CNC雕一个矩形的四条边每一条边都是一段独立的路径。如果你让每段到位后都减速到0再启动加工效率低得可怕而且停停走走反而更容易在拐角留下停顿痕迹。处理思路是做速度前瞻。提前读入下一段路径计算这段路径允许的最大起始速度或终止速度。如果下一段方向变化很小可以保持较高速度直接冲过去如果方向变化很大比如直角拐弯就必须在拐角前把速度降下来。五段式S型算法在这里只需要把起始速度和终止速度从默认的0改为非零值整条曲线形态会变成非对称但状态机框架完全不用大改。5.2 从五段式到七段式的扩展很多人在跑通五段式之后觉得平滑度还不够想升级到七段式。其实从代码角度看改动非常小。七段式就是在加速段中间真正启用匀加速段在减速段中间真正启用匀减速段。我已经按T1、T2、T3的框架写了T2平时可能被置零升级时只需要让T2在距离允许时大于0。这种渐进式升级路径我很推荐先用五段式跑通整个系统确认机械、电气、软件都没问题再逐步往七段式靠。一上来就追求数学形式最完整很容易被一堆参数埋进去。5.3 调试心得与习惯最后分享一点个人调试习惯。我每次拿到新的运动控制板都不会直接跑完整算法而是先设一个极低的最大速度比如每秒1mm跑一段10mm的短行程。确认起始、加速、匀速、减速、停止五个阶段都按预期执行再逐步加大速度。这样最容易定位问题出现在哪个阶段。还有个好用的小技巧用逻辑分析仪抓STEP和DIR引脚看脉冲频率随时间的变化。脉冲越来越密就是加速越来越疏就是减速中间平稳就是匀速。这个波形能直观告诉你S型曲线到底有没有生效比看示波器电流直观得多。至于Jerk值我现在习惯把它做成一个可调参数放到配置菜单里。现场调试时从大往小拧直到机器不叫、不抖为止。五段式S型算法的公式谁都能写但只有调过机才知道0.01量级的Jerk变化到了机械上可能就是“顺滑如丝”和“抖成筛子”的区别。
