1. S型速度曲线：为什么运动控制里总绕不开它

S型速度曲线这个词，做运动控制的应该都不陌生。如果你平时用的是步进电机或伺服电机，想让它从一个位置平滑移到另一个位置，又不希望启停瞬间把机械结构“抖”一下，梯形加减速往往不够用，这时候就会想到S型速度曲线。这篇文章我想从一个C语言实现的角度，把S型速度曲线的设计思路、公式推导、核心代码和调试经验完整梳理一遍，特别是那些在文档里不会写、但在实际工程里折腾过的人才知道的细节。

什么是S型速度曲线？简单说，就是速度在加减速过程中不是直线变化，而是像字母S一样有一个平滑的过渡。梯形速度曲线里加速度是恒定不变的，速度变化率（也就是加速度）在启停瞬间会突然跳变，电机和负载之间会产生冲击力。这种冲击在高速高精度的场景下尤其明显，轻则噪音变大，重则机械结构磨损、定位精度下降。S型速度曲线把加加速度（jerk，也就是加速度的变化率）限制在一个可控范围内，让加速度本身连续变化，从根上消掉了那个冲击尖峰。

这个内容适合谁看？我默认读者是有一定嵌入式或自动化基础的人，至少写过单片机程序，知道PWM和定时中断怎么用。如果你只是听说过S型曲线但不知道怎么落地，这篇文章刚好可以帮你把理论和C代码对上号。如果你已经在用梯形加减速，想升级一下运动品质，照着这篇文章的思路把代码改掉就行。哪怕你完全没写过运动控制，只要有C语言基础，跟着把公式理清楚、代码跑起来，也能在开发板上看到一条像模像样的S型速度曲线。

我在实际项目中用过好几个版本的实现，从最开始的查表法到后来的七段解析法，踩过不少坑。先把话说在前面：S型曲线本身不复杂，复杂的是各种边界情况。距离不够怎么办？最大速度还没到就要减速怎么办？实时插补时目标位置改了怎么办？这些问题如果不在设计阶段想清楚，跑起来之后就会变成一堆莫名其妙的问题。

2. 整体设计与选型：为什么我选了七段解析式

2.1 三种常见实现方案对比

S型速度曲线的C语言实现方法，我大致归纳成三条路：查表法、多项式拟合、七段解析式。这个选择会直接影响后续所有代码结构和实时性表现，值得在动手前认真对比一下。

查表法是最容易理解的办法。预先在PC上把速度曲线算好，生成一张表，把每个时间点对应的速度或位置存下来，MCU运行时按节拍查表输出。好处是MCU端代码量特别少，计算时间几乎为零；坏处是灵活性差，每次修改参数都要重新生成表，而且表要占存储空间。做简单Demo可以，做通用运动控制器就比较难受。

多项式拟合的思路是把位移或速度表示成时间的多项式函数，比较常见的是三次或五次多项式。代码紧凑，不需要太多分支判断，但也有明显的问题：多项式的加速度边界条件要人为指定，做不好容易出现过冲；另外对不同的目标位移，多项式系数每次都变，推导过程并不比七段式简单。在MCU上做浮点运算时，这个方案我不是很推荐。

七段解析式是我个人最推荐、也是最后一直沿用的方案。它把整个运动过程按加速度状态拆成七个阶段，每个阶段都有明确的物理含义和时间参数。虽然代码分支多一些，但逻辑清晰、计算可控、边界条件好处理，而且只要推导一遍公式，后面改参数只是改几个初始值的问题。下面这张表是我在选型时做的对比，结论很直观：

2.2 七段式模型的数学基础：从加加速度说起

七段式S型曲线的核心是加加速度jerk，单位是mm/s³或 pulse/s³，它描述的是加速度的变化快慢。有了jerk，加速度就是一条连续折线，速度自然就是一条平滑曲线。

整个运动过程可以拆成这样看：首先是加加速段，jerk保持最大值，加速度从零线性增加到最大值amax；然后是匀加速段，加速度保持amax不变，速度直线上升；接着是减加速段，jerk反向为负，加速度从amax线性降到零，速度达到最大；然后进入匀速段；减速部分则完全对称，依次是加减速段、匀减速段、减减速段，最后速度回到初值。

我之所以强调“对称”，是因为工程上最常见的场景是从静止加速到最大速度，再从最大速度减到静止，两个过程完全对称，计算时只需要算一半，另一半映射过去就行。但如果起始速度和结束速度不同，就需要分别计算加减速段的参数，公式会稍微复杂一点。这篇文章先讲对称的情况，特殊场景放在后面的问题排查里展开。

这样设计的好处是物理意义非常清晰。你给控制器设定最大速度、最大加速度、最大jerk，它就知道每个运动阶段应该持续多久、走过的距离是多少。而且这些时间段全部能用初等公式算出来，不需要迭代求解，实时性有保证。

3. 核心公式推导与参数计算：先把数学算对，再谈代码

3.1 七个阶段的位移、速度、加速度公式

做C语言实现之前，我建议先把公式在纸上推导一遍，哪怕你最后只记住代码里的算式，脑子里也要清楚每一段运动在做什么。我们定义几个基础变量：初速度v0（通常为0），最大速度vmax，最大加速度amax，最大加加速度jmax，目标位移S。

先看加速过程。加速阶段分为加加速段、匀加速段、减加速段三段。假设系统按最大jerk加速，加速度从0升到amax需要的时间是：

Ta = amax / jmax

这个时间里速度增量是：

Δv_j = 0.5 * jmax * Ta² = 0.5 * amax² / jmax

如果vmax比Δv_j大，说明系统有足够的余量加速到最大速度，那么匀加速段一定存在。匀加速段时间为：

Tu = (vmax - v0 - 2 * Δv_j) / amax = (vmax - amax² / jmax) / amax

整个加速阶段的总时间为：

Tacc = 2 * Ta + Tu

加速阶段的总位移也能推导出来，每个小段分别用 s = v0t + (1/2)at² 或 s = v0t + (1/6)jt³ 计算：

Sacc = v0 * Tacc + 0.5 * jmax * Ta³ + (v0 + Δv_j) * Tu + 0.5 * amax * Tu² + 0.5 * jmax * Ta³ + (v0 + Δv_j + amax * Tu) * Ta - (1/6) * jmax * Ta³

这里最后一项的减号是因为减加速段jerk为负。如果你是从零开始运动，v0=0，公式还能简化不少。实际工程里我通常直接用分段位移累加的方式，每一段单独算位移，最后加起来，这样代码更直观，也不容易错。

3.2 边界判断：距离不够时怎么自动降速

这是我踩过最深的坑之一。很多人设计的S型曲线在参数算完后直接跑，结果目标距离很短的时候，速度还来不及升到vmax就已经要走完行程了，最后导致大幅度过冲。解决这个问题必须提前判断：按当前参数算出来的加减速总位移Sacc，是否小于目标位移S。

如果S >= 2 * Sacc（对称运动，加速和减速位移相等），说明有足够的距离跑到最大速度，匀速段存在：

Tv = (S - 2 * Sacc) / vmax

如果S < 2 * Sacc，说明行程不够，系统根本到不了vmax。这时候有两种处理策略。第一种是降低vmax，让减速刚好在目标位置完成；第二种是在vmax不变的情况下缩短匀速段，但如果行程已经短到连匀速段都是负数，就只能降速。降速后的峰值速度vmax_new可以通过二分法或直接解方程来计算，工程上我更喜欢用预先写好的边界解析法，直接算出在给定距离内能到达的实际最大速度。

这里给一个我常用的简化判断逻辑：

• 若加速度能按设计达到amax且能够到达vmax，走完整七段；
• 若加速度能达到amax但不能到达vmax，缩短匀加速段，变成五段（没有匀速段，但仍有匀加减速）；
• 若加速度都达不到amax，变成三段（只有加加速、减加速、减减速、加减速，整体像一个三角形）。

这些判断写进代码后，函数入口只需要传入目标位移、vmax、amax、jmax，剩下的通通由初始化函数自动计算好，使用方不用关心那么多细节。

3.3 实例推演：一个完整参数的计算过程

假设我们要控制一个步进电机移动10mm，规划最大速度为20mm/s，最大加速度为100mm/s²，最大jerk为1000mm/s³。那么Ta = 100/1000 = 0.1s。加速阶段速度增量Δv_j = 0.510000.01 = 5mm/s。因为vmax=20 > 5，匀加速段存在，Tu = (20 - 25)/100 = 0.1s。加速段总时间Tacc = 20.1+0.1 = 0.3s。加速段位移Sacc可以用经验公式估算，约为2.5mm。

因为10mm > 2*2.5=5mm，匀速段时间Tv = (10-5)/20 = 0.25s。整个运动时间就是0.3+0.25+0.3 = 0.85s。如果你的客户只要求0.8s走完，就要反推vmax或者提高amax和jmax。这个过程在工程里通常要反复迭代几次。

我写代码时习惯把计算过程封装成一个函数，输入这些参数后直接输出各阶段的时间和位移，开发调试的时候特别方便，不用每次都在纸面上算。

4. C代码实现：数据结构、状态机与周期更新

4.1 C语言数据结构与初始化逻辑

C语言实现的关键在于定义清晰的数据结构和状态机。我通常用一个结构体保存所有运动参数，一个结构体保存运动状态，然后通过周期调用来推进运动。参数结构体里放的是用户设定和初始化计算出来的各段时间，状态结构体里放的是当前处于第几个阶段、当前位置、当前速度等运行时数据。

#define SC_MAX_STAGES 7 typedef enum { STAGE_ADD_INC = 0, // 加加速段 STAGE_ADD_CONST, // 匀加速段 STAGE_ADD_DEC, // 减加速段 STAGE_VEL_CONST, // 匀速段 STAGE_DEC_INC, // 加减速段 STAGE_DEC_CONST, // 匀减速段 STAGE_DEC_DEC, // 减减速段 STAGE_DONE } SCurveStage; typedef struct { float v0; // 初速度 float vmax; // 最大速度 float amax; // 最大加速度 float jmax; // 最大加加速度 float targetPos; // 目标位置 float T[SC_MAX_STAGES]; // 各阶段运行时间 float S[SC_MAX_STAGES]; // 各阶段运行位移 float totalTime; // 总运行时间 } SCurveProfile; typedef struct { float currentPos; float currentVel; float currentAcc; SCurveStage stage; float stageTime; // 当前阶段中已经过去的时间 float globalTime; // 总运动时间 } SCurveState;

初始化函数是核心，它要做的事情就是根据输入参数计算各段时间和各段位移，并做上一节说的边界判断。这个函数里有两层if嵌套，分别处理“能到vmax”“能到amax”“只能到三角形”三种情况。实际写起来也没有想象中复杂，关键是先把数学情况列全。

int SCurve_Init(SCurveProfile *profile, float v0, float vmax, float amax, float jmax, float targetPos) { float ta = amax / jmax; float dvj = 0.5f * jmax * ta * ta; float s_acc; // 是否能达到最大速度 if (vmax < 2 * dvj) { // 走五段或三段，达不到vmax // 这里按可达峰值速度处理，简化版先用三段逻辑 float vpeak = sqrtf(jmax * targetPos); // 三角形轮廓峰值 if (vpeak > vmax) vpeak = vmax; // ... 根据vpeak重新计算各段时间 } else { float tu = (vmax - 2 * dvj) / amax; profile->T[0] = ta; profile->T[1] = tu; profile->T[2] = ta; s_acc = ... // 累加计算 profile->T[3] = (targetPos - 2 * s_acc) / vmax; profile->T[4] = ta; profile->T[5] = tu; profile->T[6] = ta; } }

这种写法把各段时间预先存好，运动过程中只需要查第几段、时间走完没有，省去大量重复计算。

4.2 周期调用与状态机推进

运动执行部分一般放在定时器中断里，或者放在一个固定周期（比如1ms）的控制循环里。每进来一次，就更新当前速度和位置。状态机的推进逻辑大致是：当前阶段运行时间累加，如果超过该阶段预设时间，就跳到下一阶段，并把stageTime清零。

核心是速度位置更新函数。这个函数要能根据当前阶段和阶段内时间，算出当前加速度、速度和位置增量。写代码时我拆成了两个函数：一个阶段判断，一个阶段内运动计算。

void SCurve_Update(SCurveProfile *profile, SCurveState *state, float dt) { float acc, vel_delta, pos_delta; if (state->stage >= STAGE_DONE) return; state->stageTime += dt; state->globalTime += dt; // 根据当前阶段计算加速度 switch (state->stage) { case STAGE_ADD_INC: acc = profile->jmax * state->stageTime; break; case STAGE_ADD_CONST: acc = profile->amax; break; case STAGE_ADD_DEC: acc = profile->amax - profile->jmax * state->stageTime; break; case STAGE_VEL_CONST: acc = 0.0f; break; case STAGE_DEC_INC: acc = -profile->jmax * state->stageTime; break; case STAGE_DEC_CONST: acc = -profile->amax; break; case STAGE_DEC_DEC: acc = -profile->amax + profile->jmax * state->stageTime; break; default: acc = 0.0f; break; } vel_delta = acc * dt; pos_delta = state->currentVel * dt + 0.5f * acc * dt * dt; state->currentVel += vel_delta; state->currentPos += pos_delta; // 阶段切换判断 if (state->stageTime >= profile->T[state->stage]) { state->stage = (SCurveStage)(state->stage + 1); state->stageTime = 0.0f; } }

这段代码看着简单，但有几个隐藏的坑。第一个坑是acc的计算取了当前阶段的瞬时值，但实际上加速度在一段内是连续变化的，比如加加速段的acc本来就随t线性增长，直接乘以dt做梯形积分，精度会略差。我后来的版本改用解析积分：直接算阶段开始时的速度和位置，加上公式积分结果，这样每个周期算完位置能精确对上，不会出现累积误差。

第二个坑是速度的钳位。如果初始化算得不准，或者用户在运行中改过参数，那速度有可能在加减速切换瞬间超出vmax，甚至变负。稳妥的做法是在更新结束后对速度做一次限幅，并针对“越界但还没到下一阶段”的情况做额外修正，防止积分暴走。

4.3 代码优化与定点化移植

如果你用的是带FPU的处理器，直接float计算完全没问题，我已经在Cortex-M4和M7上跑过，全程没有压力。但如果是普通8位MCU或廉价DSP，浮点运算开销较大，这时候需要把整个计算改成定点数。我的经验是速度和位置可以用Q24或Q16格式，jerk和加速度用Q12，并把所有乘除法转换成移位的组合，同时在每个阶段切换点做一次参数重算，保证精度。

还有一个小技巧：不要在中断里做大量除法。初始化函数里把所有段时间、加速度全部算好并保存，中断里只做累加和乘法，这样时间开销会小很多。我见过有人把powf和sqrtf写到周期更新函数里，然后抱怨系统卡顿，这其实是不必要的。多数情况下，运动控制是低速实时任务，真正需要的是把耗时计算分摊到初始化过程，把周期任务做得足够精简。

5. 实机调试：从示波器看S型曲线

5.1 验证S型曲线是否正确的方法

写代码是一回事，跑起来对不对又是另一回事。我不建议直接把电机接上就全速跑，第一件事是把位置/速度波形用串口发出来，在电脑上看曲线形态。你可以用uart在定时中断里打印当前速度和位置，配合数据处理工具或者直接Excel画图，一眼就能发现问题。

正常的S型速度曲线应该是一条没有折角的平滑曲线，加速度曲线是连续折线，jerk是方波形状。如果速度曲线上有明显的尖点或突变，多半是阶段切换边界没算对，或者是初始化时某个时间段填错了。如果你看到加速度在切换瞬间跳变，那大概率是sign符号处理出错了，减减速段应该是正的jerk，写成了负的那就直接翻车。

还有一种常见情况是实际到达位置和目标位置有偏差。排除了机械丢步之后，问题通常出在我的那段解析积分代码上：如果用欧拉积分近似，当dt较大（比如10ms）时，位移误差会被不断累积。解决办法就是改成解析积分，或者把控制周期缩短到1ms以下。用1ms周期时，你还需要关心定时器的实际抖动，GPIO翻转法测周期就能确认有没有出现周期不稳的情况。

5.2 常见问题速查表

我把调试中经常遇到的问题整理成一张表，放在下面，可以参考着排查：

5.3 几个容易被忽略的工程细节

最后一个容易被坑的地方是匀速段速度正负的问题。如果你做的是往复运动，目标位置比当前位置小，那么整个运动方向是负的。很多人初始化时只写targetPos，忘了比较当前位置和目标位置的大小，结果速度方向反了，电机反向猛冲。我的做法是在初始化函数里先算方向dir，然后所有参数都按正值算，实际输出速度乘以dir，这样逻辑统一、不容易错。

另外，关于jerk的单位和量纲也要留意。有些供应商给的是“加加速度时间常数”，不是真正的jerk值，需要换算。还有电机驱动器的单位可能是pulse/s²，不是mm/s²，移植代码时先把物理量纲统一成脉冲数，不然会出现曲线形态很怪但程序逻辑每处都正确的情况。我把所有输入先换算成脉冲域，再用S型曲线算法，就是因为在一次跨度较大的项目中吃过量纲混乱的亏。

如果你在运行中发现电机已经到位，但状态机还没跳出DONE，多半是最后一段的stageTime只差一点点没有触发切换，加一个“总位移判定”兜底逻辑就好：当前位置与目标位置的差小于一个脉冲，直接强制DONE。这样既省心又可靠。

6. 后续可以怎么扩展

七段式S型曲线只是运动控制的基础模块，实际工程里还会有更多玩法。比如把S型曲线和速度前瞻结合起来，在连续多段路径中自动规划每段的进入速度，不会因为频繁启停影响效率；又比如把算法改造成支持在线改变目标位置，通过重新规划当前速度和剩余距离来无缝衔接新位置。这些方向本质上都是对状态机和初始化逻辑的扩展，核心公式不变，只是在原有框架上增加一些判断和重算。

我个人在实际操作中最深的体会是：先把七段对称情况跑顺，再去碰非对称和变速情况。很多人一上来就追求通用，结果被各种边界条件绕晕。等你把基础版本吃透了，再看那些复杂需求，其实都是原模型的组合与变体，思路会清晰很多。S型速度曲线这个项目看着小，但它几乎覆盖了运动控制里所有核心的数学建模和工程调试痛点，做完一次，后面做别的规划算法会顺手很多。