开头我先说个现象很多接触运动控制的人第一次看速度规划以为把梯形速度曲线做出来就算入门了结果一上机就发现电机咯噔一声机械结构跟着振加工表面还留振纹。问题不在速度曲线本身而在加速度发生了突变。真正让运动丝滑的是让加速度的变化也连续起来这就是S型曲线算法的核心价值。而S型落到工程实现上最常见也最实用的一种形态就是五段式S型算法。这篇笔记不搞抽象理论直接把我做运动控制时对五段式的理解、公式推导、C代码和实测中踩过的坑一次说清楚。适合谁看做3D打印机、CNC、伺服电机定位、机器人关节控制的工程师以及自动化专业正在学运动规划的学生。你看完能直接照着写出一个可用的五段式速度规划器并且知道它在什么条件下成立、什么条件下会失效。1. 从梯形曲线到S型曲线一次加速度突变引发的问题1.1 梯形曲线为什么硬梯形速度规划的逻辑很直白速度先以恒定加速度上升到达目标速度后匀速运行再以恒定加速度下降。三段速度曲线画出来是标准的梯形实现起来三行代码就能写。但它有一个致命问题加速度在启动瞬间、速度切换瞬间、停止瞬间都是阶跃变化的。想象一辆车起步时油门瞬间踩到底加速度从0直接跳到Amax停车时刹车瞬间踩死加速度从- Amax直接跳回0。车上的乘客会猛地后仰、前冲机械结构承受的冲击力也远大于稳态值。在运动控制里这个冲击的度量叫加加速度jerk是加速度对时间的导数单位m/s³。梯形曲线的jerk在加速度切换点上等于无穷大严格说是阶跃没有导数这是所有刚性冲击的根源。1.2 S型曲线做了什么S型曲线做的事情是把加速度的跳变改成连续过渡。加速度从0开始按一定斜率J逐步增加到Amax再按-J逐步降回0。这样jerk成了有限值甚至在某些分段里jerk恒定速度曲线从直线变成了抛物线和直线拼接的S形冲击明显变小。完整S型曲线通常有七段加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。教科书上几乎都讲七段式但实际工程里有个常见场景不需要匀加速和匀减速段——那就是目标速度较低、或者目标位移较短速度根本达不到匀加速段存在的条件。此时S型曲线自动坍缩成五段加加速、减加速、匀速、加减速、减减速。1.3 五段式不是凑出来的是边界条件逼出来的我最早学S型算法时一度以为五段式是七段式的简化版是为了省代码故意砍掉了两段。后来自己推边界条件才发现这个认知是反的五段式是S型曲线在自己适用域内的完整形态七段式才是五段式的扩展。具体来说加速阶段能不能完整分成加加速匀加速减加速三段取决于目标速度Vmax和加速能力之间的大小关系。如果Vmax不大速度增量在加加速减加速两段里就能完成匀加速段的时长挤成负数它自然就消失了。换句话说五段式是系统参数约束下的数学必然不是拍脑袋的简化。这个认知很重要因为很多现成的S型算法模板默认按七段来写如果用不加判断的统一代码在速度上限较低时会算出一堆负数或者NaN最后还得回头补判断。先理解五段式的存在条件再去看代码设计逻辑就顺了。2. 五段式S型的分段公式与边界条件的完整推导2.1 分段结构与剖面特征五段式S型速度规划按时间顺序分为五段我习惯用符号统一记T1加加速段、T2减加速段、T3匀速段、T4加减速段、T5减减速段。加加速、减加速这两段共同完成从起始速度到匀速段速度的过渡匀速段保持速度恒定加减速、减减速对称地完成减速。对应的加速度剖面是这样的第一段jerk为正加速度从0线性增加到Amax第二段jerk为负加速度从Amax线性降到0第三段加速度保持0第四段jerk为负加速度从0线性降到- Amax第五段jerk为正加速度从- Amax线性升回0。注意一个关键点第一段和第二段之间没有匀加速段加加速段刚把加速度推到Amax马上进入减加速段让加速度立刻往回降。这也是五段式名称的来源——加速度剖面只包含五个时间区间其中前两个都是三角形区。2.2 各段位移、速度公式推导下面我从零开始推一遍加速阶段的公式减速阶段完全对称可以直接镜像。设定加加速度绝对值为J最大加速度为Amax起始速度为0。先看加加速段时间从0到T1a(t) J * t v(t) (1/2) * J * t^2 s(t) (1/6) * J * t^3当t T1时加速度到达Amax所以T1 Amax / J。此时速度v1和位移s1分别为v1 (1/2) * J * T1^2 Amax^2 / (2J) s1 (1/6) * J * T1^3 Amax^3 / (6J^2)再看减加速段时间从T1到2T1。这段的局部时间记为ττ从0到T1加速度表达式是a(τ) Amax - J * τ对加速度积分得到速度再加上v1作为初始速度v(τ) v1 Amax * τ - (1/2) * J * τ^2当τ T1时v2 v1 Amax * T1 - 0.5 * J * T1^2。代入T1 Amax/J得到v2 Amax^2 / (2J) Amax^2 / J - Amax^2 / (2J) Amax^2 / J这个v2就是加速阶段能够达到的峰值速度。我给这个速度起个名字叫临界速度Vcrit它是判断五段式是否成立的核心参数。继续算减加速段的位移。对v(τ)积分s2 s1 v1 * T1 (1/2) * Amax * T1^2 - (1/6) * J * T1^3把已知量代入化简最后得到整个加速阶段的总位移s_acc s1 s2 Amax^3 / J^2也可以用更快的验算方式加速阶段总时间2T1速度从0上升到一个峰值Vcrit速度曲线近似对称等效平均速度是Vcrit/2总位移就等于(Vcrit/2) * 2T1 Vcrit * T1 (Amax²/J) * (Amax/J) Amax³/J²。结果一致。减速阶段完全对称减速位移s_dec s_acc Amax³/J²减速总时间也是2T1。2.3 五段式成立的两个前提条件推导到这一步五段式的成立条件就很清晰了。第一个条件目标速度Vmax必须等于临界速度Vcrit。如果目标速度大于Vcrit说明不加匀加速段撑不到目标速度需要七段式如果目标速度小于Vcrit说明实际运行时加速度根本到不了Amax速度就得开始下降这叫不完全S型或三角形S型只有三四段。所以五段式严格对应的是Vmax Amax²/J也就是速度刚好被这两段三角形加速度区推到上限的场景。第二个条件目标位移S必须足够容纳完整的加速和减速过程。五段式总位移包括加速段位移、匀速段位移、减速段位移三部分S s_acc s_cons s_dec 2 * Amax^3 / J^2 Vmax * T3其中T3是匀速段时间。所以必须满足S 2 * Amax³/J²否则连完整加速再减速都做不完需要退化成三角形S型曲线。2.4 已知目标位移反解各段时间实际工程里我们遇到的问题一般是给定目标位移S、最大加速度Amax、加加速度J求每段执行时间。这里要分两步。第一步算临界速度Vcrit Amax²/J。如果S 2 * Amax³/J²则说明可以按五段式规划计算出匀速段时间T3 (S - 2 * Amax^3 / J^2) / Vcrit加加速段时间T1 Amax/J减加速段时间T2 T1加减速段T4 T1减减速段T5 T1。总时间T_total 4 * T1 T3。如果S 2 * Amax³/J²五段式用不了需要走三角形S型分支。三角形S型没有匀速段速度峰值Vpeak小于Amax²/J要通过解一个三次方程或者二分法求Vpeak。我在第4章会专门讲这个降级方案。顺便说一句很多工程实现里不会硬性要求Vmax刚好等于Vcrit而是把它当作算法的一个工作点你给出位移和参数它计算出对应能达到的峰值速度。系统允许的最高速度只是上限如果Vcrit超过上限算法自动降级到七段式或者限制速度这才是完整规划器要做的事情。3. 工程实现的代码骨架规划器结构与C语言实现3.1 数据结构设计写规划器之前先把数据结构定好我用C语言实现结构体里包含输入参数、规划结果和状态标记typedef struct { double v0; // 起始速度 double vmax; // 系统允许的最大速度 double amax; // 系统允许的最大加速度 double jerk; // 加加速度 double target; // 目标位移有符号 double dir; // 运动方向1 或 -1 double T1, T3; // 加加速时间、匀速段时间 double vcrit; // 临界速度 double s_acc; // 单侧加减速位移 double total_time; int status; // 0: 五段式 1: 需降级 2: 参数非法 } SProfile;把v0、vmax、amax、jerk、target全放在结构体里规划函数和插补函数都通过结构体指针传参这样方便在调试时整体打印状态。3.2 五段式规划函数规划函数做的事就是把我上面推导的公式变成代码逻辑按顺序来int s_profile_plan(SProfile *p) { double S fabs(p-target); p-dir (p-target 0) ? 1.0 : -1.0; if (p-amax 0 || p-jerk 0 || S 0) { p-status 2; return -1; } // 计算临界速度和单侧加减速位移 p-vcrit p-amax * p-amax / p-jerk; p-s_acc p-amax * p-amax * p-amax / (p-jerk * p-jerk); // 如果临界速度超过系统速度上限需要降级到七段式 if (p-vcrit p-vmax) { p-status 1; return -2; } // 如果位移不够容纳完整五段需要降级到三角形S型 if (S 2.0 * p-s_acc) { p-status 1; return -3; } // 五段式规划 p-T1 p-amax / p-jerk; p-T3 (S - 2.0 * p-s_acc) / p-vcrit; p-total_time 4.0 * p-T1 p-T3; p-status 0; return 0; }这里我特意把降级条件分成两个超速降级和位移不足降级。因为它们的处理方式完全不同。超速降级意味着你的目标速度比系统能跑到的更高应该走七段式加上匀加速段位移不足降级意味着目标位置太近速度根本爬不到Amax需要走三角形S型。两种场景不能混为一谈否则代码混沌不清。3.3 实时位置插补函数规划完成之后运动控制核心在每一次周期中断里调用插补函数输入当前运行时间t输出该时刻的位置值。这个函数要处理五段各自的分段逻辑实现时用局部时间变量简化double s_profile_pos(const SProfile *p, double t) { double T1 p-T1; double J p-jerk; double pos 0.0; if (t 0) t 0; if (t p-total_time) t p-total_time; if (t T1) { // 加加速段s 0.5*J*t^3/3 - 1/6*J*t^3 pos (1.0 / 6.0) * J * t * t * t; } else if (t 2 * T1) { // 减加速段局部时间tau double tau t - T1; double v1 p-amax * p-amax / (2.0 * J); double s1 p-amax * p-amax * p-amax / (6.0 * J * J); pos s1 v1 * tau 0.5 * p-amax * tau * tau - (1.0 / 6.0) * J * tau * tau * tau; } else if (t 2 * T1 p-T3) { // 匀速段 double tau t - 2 * T1; pos p-s_acc p-vcrit * tau; } else if (t 3 * T1 p-T3) { // 加减速段减速过程关于中点对称 double tau t - (2 * T1 p-T3); double v_dec p-vcrit; pos p-s_acc p-vcrit * p-T3 v_dec * tau - 0.5 * p-amax * tau * tau (1.0 / 6.0) * J * tau * tau * tau; } else { // 减减速段 double tau t - (3 * T1 p-T3); double s4 0; // 第四段结束时的位置用对称性简化直接由总位移减最后一段 // 更稳妥的做法直接用总位移反推 double s_total 2.0 * p-s_acc p-vcrit * p-T3; double s_rem s_total - (p-s_acc p-vcrit * p-T3 p-s_acc); // 这段算出的是整个最后一段已走之外 // 我实际更常写成下面这种对称式 // 最后一段从总长往回减如果用正向积分则 // s5(tau) J*tau^2*(T1/2 - tau/6) v4*tau s4其中v4是第四段末速度这里v4p-vcrit-Amaz*T1J*T1^2/2代入T1化简可得0 // 为了代码清晰直接用总长减剩余距离计算更省心 double v4 0.0; // 第四段结束时速度正好为0 double s5 0.0; // 简化直接用对称性最后阶段位置 总位移 - 还没走完的剩余对称部分 double rem_time p-total_time - t; double rem_pos (1.0 / 6.0) * J * rem_time * rem_time * rem_time; pos (2.0 * p-s_acc p-vcrit * p-T3) - rem_pos; s5 pos; (void)s4; (void)v4; pos (p-s_acc p-vcrit * p-T3); // 上面这行是为了让编译器忽略未用变量实际我建议用反向剩余法 // 重写为 double total 2.0 * p-s_acc p-vcrit * p-T3; double rem p-total_time - t; pos total - (1.0 / 6.0) * J * rem * rem * rem; } return p-dir * pos; }我坦白说上面这个函数在最后一段的处理绕了一点。实际工程里我更推荐用剩余时间对称法来算最后一段的位置也就是用总位移减去从当前时刻到终点这段反向对称加速产生的位移。因为减速末段和起点加速段在数学上完全对称这样写思路最简洁代码也最少。上面的代码里我已经把最终逻辑整理了就是最后那个total减剩余部分的写法跑测试时这个逻辑是对的。3.4 一个简单的测试主函数写个简单的main测试一下规划结果输入位移和参数打印各段参数和几个关键时间点的位置#include stdio.h #include math.h int main(void) { SProfile sp; sp.v0 0.0; sp.vmax 1000.0; sp.amax 500.0; sp.jerk 50000.0; sp.target 20.0; int ret s_profile_plan(sp); if (ret ! 0) { printf(plan failed: %d\n, ret); return 1; } printf(T1%f T3%f total%f vcrit%f\n, sp.T1, sp.T3, sp.total_time, sp.vcrit); for (double t 0; t sp.total_time; t sp.total_time / 10.0) { printf(t%f pos%f\n, t, s_profile_pos(sp, t)); } return 0; }这段代码跑通之后你会看到一个标准的S型位移曲线位置从0一路平滑增加到20中间没有加速度突变。我建议你拿到代码后先不急着改参数跑一遍在坐标纸上画出来把速度曲线和加速度曲线也画出来直观感受一下S型曲线和梯形曲线的差异这个印象比看十篇文章都有用。4. 实测定点运动时需要注意的边界情况4.1 位移不足时的降级处理最常踩的坑就是位移太小五段式根本跑不满。例如你设定目标位移只有1毫米但Amax和J都很大算出来的2 * s_acc可能远超这个位移。此时算法不能直接报错要降级到三角形S型。三角形S型的特点是没有匀速段速度从0爬升到一个峰值Vpeak后又立即下降回0Vpeak小于Vcrit。整个加速度剖面只有四段加加速、减加速、加减速、减减速或者说对称的四个三角形区。求解Vpeak的方法是解一个方程。总位移S等于加速部分和减速部分的位移之和而两部分的距离都是Vpeak²/J形式。我直接给结论S 2 * Vpeak^2 / J所以Vpeak sqrt(S * J / 2)算完Vpeak后加速度峰值Apeak sqrt(Vpeak * J)它其实小于Amax说明系统没有用到最大加速度。各段时间T 2 * sqrt(Vpeak / J)。这个公式我建议直接背下来位移不足的场景在短距离点动、点位运动中太常见了。不处理这个分支设备轻则顿挫重则报警停机。4.2 初速度不为零怎么办很多运动控制的规划器默认初速度为0但实际设备经常在运动中接到新指令比如上一段还在匀速前进突然要给一个新的相对位移目标。这时候初速度v0不为零五段式的边界条件要修正。核心变化在加速阶段的起始速度。加速阶段结束时速度要达到vcrit速度增量变成vcrit - v0。如果v0 0加加速和减加速两段的三角形面积之和要等于vcrit - v0如果v0接近vcrit加速段只占很小一段。我在实际项目中处理初速不为零的策略比较简单先判断vcrit和v0的大小关系。如果v0等于vcrit根本没有加速段直接进入匀速或减速段。如果v0小于vcrit把加速段的位移公式从s_acc vcrit * T1改为s_acc (v0 vcrit) / 2 * (2 * T1) (v0 vcrit) * T1也就是用平均速度乘以加速段时间。这是因为在纯三角形加速度剖面下速度变化近似线性平均速度可以用首尾速度平均。这样处理的精度对绝大多数点位运动足够。4.3 指令变更时的规划重置运动控制里另一个隐蔽问题上一条指令还没走完下一条指令就来了。很多新手把新指令直接传给规划器重算结果当前位置和速度发生跳变电机瞬间暴走。我的习惯是先用插补函数算了当前时刻的位置和速度再把它们作为下一条规划的起点参数。用结构体里的v0字段承接当前速度target设为剩余目标位移重新调用规划函数。这样两条指令之间速度连续、位置连续唯一的代价是每段规划的起点在变化但这对实时控制系统完全可接受。另外一个细节规划器内部不能只规划绝对位置必须支持相对位移。否则每次新指令都要用绝对坐标重算一遍容易在累积误差上出问题。我上面给的代码用target作为相对位移输入就是这个目的。4.4 实测中的浮点误差与位置补偿浮点误差在纯计算中不起眼但在长时间连续运动中可能积累。S型规划器每个中断周期算一次位置如果只用浮点累加做位置输出一天跑下来可能误差几十个脉冲。我的做法是位置输出永远基于规划器的解析公式而不是累加增量。规划器本身输入的是距离目标还有多远输出的是当前时刻应该在哪不依赖上一个周期的结果所以不存在累积误差。这个思路几乎适用于所有运动规划器推荐你架构时就按这个来。还有一个工程小细节许多电机驱动器要求位置指令是整数脉冲数。规划器输出浮点位置后我建议不要直接截断而是做四舍五入并把舍入误差保存在一个变量里下个周期补上确保整段位移的脉冲总数精确等于目标值。5. 从能用到好用参数整定与实测体会5.1 jerk和加速度的匹配关系五段式里最核心的参数是jerk它决定加速度变化快慢。jerk过大加速度突变虽然从数学上被平滑了但宏观上仍然表现为很强的冲劲机械振动明显jerk过小加速过程拖得太长同样位移下总时间增加生产效率下降。我个人的整定经验是从加速度的5到10倍开始试。例如Amax 500 mm/s²时jerk取5000到10000 mm/s³比较稳妥。然后看速度曲线上升沿的斜率如果电机有明显的冲刺感就降jerk如果整个加速过程软绵绵浪费时间就升jerk。5.2 匀速段太短的异常形态五段式的一个隐患当目标位移刚好比2 * s_acc大一点点匀速段时间T3就会非常短短到可能只有几十毫秒。此时速度曲线变成两个S形贴在一起中间一个极短的平台视觉上几乎看不出来。这种形态在振动抑制上没什么问题但有个实际麻烦如果你用固定周期定时器做速度采样可能整个匀速段被一个采样周期跳过插补函数显示速度从上升直接切到下降看起来像加速度跳变。解决方法是采样周期要远小于T3或者干脆在规划时给T3设一个下限比如不小于5个采样周期。如果算出来太短就把峰值速度压一点让T3延长。这个方法在工程上叫时间整形效果比硬调参数好得多。5.3 速度上限与临界速度的关系如何取舍前面推导过五段式的匀速段速度是Vcrit Amax²/J。这意味着一旦选定Amax和J五段式能跑到的最高速度就被固定了不能随意设置。如果你的应用要求一个指定的Vmax而Vmax大于Vcrit就必须上七段式。如果Vmax小于Vcrit算法会掉进三角形S型分支。所以五段式实际上是S型曲线家族的一个特定档位我把它理解为用最大能力跑中低速位移的模式。对于大多数3D打印和轻载CNC这种模式反而很合适打印头位移通常在几毫米到几十厘米速度需求不高五段式计算量小、参数少、实时性好。用好了速度和加速度双向连续机械寿命和表面质量都有明显提升。5.4 我把jerk调大的那一次惨痛教训最后分享一个真实案例。我之前调试一台高速贴片机型的X轴为了缩短节拍把jerk从8000一口气提到50000想着反正S型曲线已经平滑了参数大点没事。结果上电一跑导轨末端发出明显的哐一声虽然加速度曲线看起来还是连续的但jerk导致的机械激振能量被结构放大刚性不足的联轴器直接暴露了共振问题。后来我把jerk降回20000同时把Amax从1500降到1200节拍只慢了不到3%但整机噪音和振动大幅下降。这个案例让我明白了一个道理S型曲线解决的是加速度突变带来的冲击但它不是万能药机械系统的固有频率和刚度决定了jerk的上限。算法层面能做的是在参数整定时对jerk保持敬畏而不是一味追快。如果你刚开始在自己的设备上做S型速度规划我建议你按这个路径走一遍先跑通五段式核心代码画出位移、速度、加速度三条曲线确认连续然后接上电机从小jerk开始向上调留意噪音和振动最后再做位移不足降级和指令变更重置两个分支逻辑整套规划器才算完整可用。这样一轮下来你对S型曲线的理解会比看多少篇论文都扎实。