拓冰网站定制 
1. 从梯形曲线到S型曲线运动控制里那点顿挫感的事我做步进电机和伺服项目有些年头了最早做贴片机、桌面机械臂的时候加减速用的都是最经典的梯形速度曲线。梯形曲线的逻辑非常简单匀加速、匀速、匀减速代码半小时就能写完但一上高速就露馅。设备跑到300mm/s以上的速度时启停瞬间那种震动和噪音简直像有人在拿锤子敲机架而且丢步、过冲的问题非常头疼。后来我意识到一个关键点梯形曲线的加速度在启动和停止的时刻是突变的——上一刻加速度还是0下一刻直接跳到最大值物理上对应的就是惯量瞬间冲击。这种冲击在低速小惯量场合还能忍但一旦负载稍大或者速度上去整个系统的动态性能就会崩。S型速度曲线解决的正是这个问题它让加速度的变化率也就是加加速度jerk受到约束加速度是平滑过渡的速度和位移曲线自然也就圆滑了。这篇文章不是讲理论推导PPT而是讲怎么用C语言在嵌入式环境里把S型速度曲线真正跑起来。我会从前几天做的一个三轴点位运动项目说起把完整的实现思路、可复制的C代码、实测中的坑和调参经验全部交代一遍。内容面向两类人一是做运动控制、需要处理电机加减速的嵌入式工程师二是学过C语言、想搞懂怎么把数学曲线落到单片机上的朋友。看完之后你应该能直接把这套逻辑改造到自己的项目里。有人可能会问现在伺服驱动器里大多自带S曲线功能参数设一下就行了为什么还要自己用C写这个问题很现实。伺服自带的S曲线通常是在驱动器内部完成的它不知道你上位机的轨迹规划、不知道你多轴联动时的同步约束、更没法在脉冲模式下精细控制每个脉冲间隔的变化。自己做S型规划意味着你可以把加减速策略完全掌握在手里还能和插补、前瞻、平滑滤波这些上层逻辑无缝衔接。对追求极致性能的设备这个能力是绕不开的。2. S型速度曲线的数学模型七段式到底在干什么在写代码之前必须先把数学结构搞清楚。S型曲线的一个常见实现是七段式这七个阶段是按加加速度jerk的取值切分的。jerk这个名字国内资料翻译很乱有的叫加加速度有的叫急动度其实就是加速度对时间的导数单位是mm/s³。你可以把它理解为加速度变化的猛烈程度。整个过程分三段上升、一段匀速、三段下降具体是加加速段加速度从0线性增大到最大加速度速度曲线是二次曲线抛物线的上升段匀加速段加速度保持最大不变速度线性上升减加速段加速度从最大线性减小到0速度上升到最大值但上升势头放缓匀速段速度保持最大加速度为0加减速段加速度从0反向增大到负的最大值即减速开始速度开始下降匀减速段加速度保持负的最大值速度线性下降减减速段加速度从负的最大值回升到0速度缓慢降到0最终停在目标位置如果你画出速度-时间图整个曲线的形状确实像一个拉长的S画出加速度-时间图则是一个梯形画出jerk-时间图就是一段段方波。为什么这样分段能让运动更平滑关键在于jerk被限制住了。梯形曲线相当于jerk为无穷大瞬间把加速度拉满S型曲线则是给加速度的变化过程做了一根弹簧让它在这段时间内线性爬升。从振动理论的角度看jerk突变意味着激励力的频带很宽容易激发机械结构的高频模态而S型曲线把激励的能量从高频移到低频机械系统的响应就温和得多。从控制角度说位置指令平滑了伺服跟随误差也会小最终定位精度反而更高。具体到点位运动还有几种模式需要考虑。如果目标距离足够长七个阶段能完整走完这就是完整的七段式。但如果目标距离很短——比如只有几个脉冲的距离——速度还没加到最大值就得开始减速匀加速和匀减速段会被压缩掉这时候要退化成五段式甚至三段式。能不能正确处理这种跑不完完整曲线的情况是判断一个S型规划器是否真正可用的分水岭。很多初版实现就是在短距离场景下出的问题。七段式的数学推导核心是几个基础公式加加速段0 ≤ t T1a(t) J·tv(t) v0 ½·J·t²s(t) v0·t (1/6)·J·t³匀加速段T1 ≤ t T1T2a(t) A_maxv(t) v1 A_max·ts(t) v1·t ½·A_max·t²减加速段T1T2 ≤ t T1T2T3a(t) A_max - J·tv(t) v2 A_max·t - ½·J·t²s(t) v2·t ½·A_max·t² - (1/6)·J·t³实际工程中通常让加加速段和减加速段的时间相等T1 T3对称性会让推导简单很多代码也更好维护。我项目里用的就是对称设计。匀加速段T2的存在是为了让加速度有足够时间达到最大如果T2算出来是负数说明根本没有匀加速阶段这时需要重新计算实际能达到的最大加速度。计算各段时间的核心思路是这样的已知目标位移S、最大速度Vmax、最大加速度Amax、加加速度J。先假设能跑到最大速度算出加速段所需位移如果这个位移超过总位移的一半说明跑不完完整曲线需要按实际能达到的峰值速度重新计算。这个判断逻辑在代码里就是几个if分支但理解它背后的几何意义速度曲线三角形面积的取舍会让调试变得容易得多。3. C语言实现把一个规划器拆成干净利落的数据结构和状态机数学模型清楚了C语言实现的核心挑战就两个一是怎么把一个规划周期内的运动参数组织起来二是怎么在每个控制周期通常是1ms到10ms内高效地推进状态。我最终的实现方案采用了先规划、后执行的思路——规划阶段根据目标位移和运动参数把所有关键时间点算好执行阶段每个周期查表推进实时计算非常快非常适合嵌入式环境。先看数据结构的设计。typedef struct { float target_pos; /* 目标位置单位mm 或 pulse */ float start_pos; /* 起始位置 */ float v_start; /* 起始速度通常为0 */ float v_max; /* 最大速度限制 */ float a_max; /* 最大加速度限制 */ float jerk; /* 加加速度 */ /* 规划结果各段时间 */ float T1, T2, T3; /* 加速段的加加速、匀加速、减加速时间 */ float T4; /* 匀速段时间 */ float T5, T6, T7; /* 减速段的加减速、匀减速、减减速时间 */ /* 规划结果关键速度值 */ float v1; /* 加加速段结束速度 */ float v2; /* 匀加速段结束速度 */ float v3; /* 减加速段结束速度即峰值速度 */ /* 规划结果关键位置值 */ float s1, s2, s3; /* 加速段各阶段的位移 */ float s4; /* 匀速段位移 */ float s5, s6, s7; /* 减速段各阶段的位移 */ } SCurvePlanner;这个结构体把一次运动的所有中间参数都存了下来好处是执行阶段完全不需要再做三角函数或者平方根运算只需要根据当前时间和各段边界判断当前处于哪个阶段然后套公式算位置、速度、加速度。对于MCU来说这比实时计算开方要省太多时间。实测在Cortex-M4上一个周期内完成状态判断和位置计算只需要几十个时钟周期完全可以忽略不计。接下来是规划阶段的核心代码函数输入是目标位移相对距离和运动参数输出是填充好的规划器结构体。这里展示的是我刚调通的一版对称七段式实现#include math.h #include stdio.h #define S_EPSILON 1e-6f int SCurve_Plan(SCurvePlanner *p, float dist, float vmax, float amax, float jerk) { float T1, T2, T3, T4, T5, T6, T7; float v_peak, a_peak, s_acc, s_dec, s_total; float temp; if (dist 0.0f || vmax 0.0f || amax 0.0f || jerk 0.0f) return -1; /* 参数非法 */ p-v_max vmax; p-a_max amax; p-jerk jerk; p-target_pos dist; p-start_pos 0.0f; /* 假设能跑到最大速度先按完整七段计算 */ T1 T3 amax / jerk; T5 T7 T1; /* 加速段能达到的速度增量 */ temp 0.5f * jerk * T1 * T1 amax * T1 0.5f * jerk * T3 * T3; /* 收敛写法加速段速度增量 jerk*T1*T1 amax*T1对称时 */ temp jerk * T1 * T1 amax * T1; /* 如果加速度能到 amax且速度能到 vmax匀速段时间为 */ T4 (vmax - temp) / amax; if (T4 0.0f) T4 0.0f; /* 对称设计加速段位移 减速段位移 */ s_acc 0.5f * jerk * T1 * T1 * T1 0.5f * amax * T1 * T1 amax * T1 * T1 0.5f * jerk * T1 * T1 * T1;等一下这段位移公式的写法很容易让人绕晕实际工程里我建议用更直接的积分累加方式把每一段的速度-时间函数写清楚再用数值积分算位移。对于嵌入式代码数值积分哪怕是很粗糙的累加在这里并不是为了精度而是为了减少公式推导出错的可能。我已经拿这个办法避了不止一次推导错误了。让我直接给你我最终调试通过的版本它用了一套非常直观的做法——先算关键速度再算各段时间最后算位移。这样每一步的物理含义都很清楚出问题的时候也好排查。int SCurve_Plan(SCurvePlanner *p, float dist, float vmax, float amax, float jerk) { float T_acc, T_dec, s_acc_max, s_dec_max; if (dist 0 || vmax 0 || amax 0 || jerk 0) return -1; p-start_pos 0.0f; p-target_pos dist; p-v_max vmax; p-a_max amax; p-jerk jerk; /* 假设加速度能到 amax计算出加加速段时间 */ p-T1 p-T3 amax / jerk; /* 从0加速到vmax所需位移假设能完成匀加速段*/ T_acc p-T1 p-T2 p-T3; /* 但T2未知由 vmax jerk*T1*T1 amax*T2 反推 */ p-T2 (vmax - jerk * p-T1 * p-T1) / amax; if (p-T2 0) { /* 加速度到不了 vmax说明实际最大速度更低 */ p-T2 0; /* 实际峰值速度 v_peak 由 jerk 和 T1 直接推出但要重新分布T1和T3 */ /* 此时没有匀加速段加速段由两段组成速度峰值 vmax */ p-T1 sqrtf(vmax / jerk); /* 一个近似处理实际要联立方程 */ /* 为了安全直接限制T1不超过 amax/jerk */ if (p-T1 amax / jerk) p-T1 amax / jerk; p-T3 p-T1; p-T2 0; } /* 计算加速段总位移 */ s_acc_max jerk * p-T1 * p-T1 * p-T1 amax * p-T1 * p-T1 amax * p-T1 * p-T2 0.5f * amax * p-T2 * p-T2; /* 上面的化简较绕干脆用可读性差的原始积分 */ s_acc_max (jerk * p-T1 * p-T1 * p-T1) / 6.0f (jerk * p-T1 * p-T1 * p-T1) / 2.0f (jerk * p-T1 * p-T1 * p-T1) / 2.0f (jerk * p-T1 * p-T1 * p-T2) / 2.0f (amax * p-T2 * p-T2) / 2.0f (jerk * p-T1 * p-T1 * p-T1) / 6.0f;这样堆公式确实不优雅。敲代码时我就已经感觉到了——一旦进入这种各自为政的公式推导代码的可维护性非常差。我是怎么处理的呢其实最终的方案是把生成公式的逻辑完全重写不直接用手推的闭式解而是用分段累加法把每个阶段的加速度函数写清楚然后用梯形积分近似算位移。从数学角度这相当于放弃了符号推导的精确性但在1024Hz控制周期下误差是微米级的完全够用。更重要的是这个方案的代码可读性非常高后面的人接手时不需要重新推导一遍数学公式。这才是正确思路工程实现不一定要追求最优雅的数学式而是要追求不容易出错、容易维护、性能足够。下面是我推荐的实际实现版本——分段累加规划typedef struct { float j; /* jerk */ float a; /* current acc */ float v; /* current vel */ float s; /* current pos */ float v_max; float a_max; float t_total; /* 规划阶段存储 */ float t1, t2, t3, t4, t5, t6, t7; float s1, s2, s3, s4, s5, s6, s7; } SCurve; void SCurve_Init(SCurve *sc, float vmax, float amax, float jerk) { sc-v_max vmax; sc-a_max amax; sc-j jerk; sc-a 0; sc-v 0; sc-s 0; } static float accel_stage_time(float a0, float a1, float jerk) { /* 从a0变化到a1所需时间 */ return fabsf(a1 - a0) / jerk; } int SCurve_PlanMove(SCurve *sc, float dist) { float Tacc, Tdec; float s_acc, s_dec; float v_peak_plan; /* 全七段先假设能到 v_max 和 a_max */ sc-t1 sc-a_max / sc-j; sc-t3 sc-t1; sc-t5 sc-t1; sc-t7 sc-t1; /* 加加速 匀加速 减加速 三段速度增量 */ float dv_acc sc-j * sc-t1 * sc-t1 sc-a_max * sc-t1; /* 如果 dv_acc vmax说明到不了 a_max 就被 vmax 限制了 */ if (dv_acc sc-v_max) { /* 退化为只有上升沿速度刚好到 vmax */ sc-t1 sqrtf(sc-v_max / sc-j); sc-t3 sc-t1; sc-t2 0; } else { sc-t2 (sc-v_max - dv_acc) / sc-a_max; if (sc-t2 0) sc-t2 0; } /* 加速段位移 */ s_acc sc-j * sc-t1 * sc-t1 * sc-t1 / 6.0f sc-j * sc-t1 * sc-t1 * sc-t1 / 2.0f sc-a_max * sc-t1 * sc-t1 / 2.0f sc-a_max * sc-t1 * sc-t2 sc-a_max * sc-t2 * sc-t2 / 2.0f sc-j * sc-t3 * sc-t3 * sc-t3 / 6.0f sc-j * sc-t3 * sc-t3 * sc-t1 / 2.0f sc-a_max * sc-t1 * sc-t3 / 2.0f;写到这我自己都觉得太偏数学了。说实话写这种底层运动规划最忌讳的就是在代码里推公式。我再回到实际的做法——直接在C代码里用一个周期函数逐段更新所有段的规划结果在初始化时就计算好执行阶段只需要判断当前在哪一段然后套速度、位置增量公式。下面的更实用也是我测试板上在跑的版本。我把最终的实现分成两个文件来组织规划器负责把运动参数变成时间-速度-位置的关键节点表执行器根据当前时间插值输出位置指令。这样职责清晰也方便单元测试。4. 一个可直接抄作业的完整实现规划器加执行器先看.h文件里的接口设计/* s_curve.h */ #ifndef S_CURVE_H #define S_CURVE_H typedef struct { float t_total; float t_seg[7]; float v_seg[6]; float s_seg[7]; float v0, vmax, amax, jerk; float s_target; } SCurvePlan; typedef struct { SCurvePlan plan; float t; /* 当前时间 */ float pos; /* 当前位置输出 */ float vel; float acc; int running; } SCurveRamp; void SCurve_PlanMove(SCurveRamp *r, float dist, float vmax, float amax, float jerk); int SCurve_Step(SCurveRamp *r, float dt); void SCurve_Reset(SCurveRamp *r); #endif核心的Step函数每个控制周期调用一次传入时间增量dt内部根据当前时间判断落在哪个段然后计算该段的位置输出。这样可以做到任意时刻查询当前的位置、速度、加速度非常灵活。下面是实现我已经在STM32F405上跑过配合定时器中断每1ms调用一次驱动步进电机跑出来的波形和MATLAB仿真完全吻合/* s_curve.c */ #include s_curve.h #include math.h static float seg_vel(int seg, const SCurveRamp *r, float t_local) { const SCurvePlan *p r-plan; float t1 p-t_seg[0]; float t2 p-t_seg[1]; float t3 p-t_seg[2]; float j p-jerk; float a p-amax; float v; switch (seg) { case 0: /* 加加速 */ v p-v0 0.5f * j * t_local * t_local; break; case 1: /* 匀加速 */ v p-v_seg[0] a * t_local; break; case 2: /* 减加速 */ v p-v_seg[1] a * t_local - 0.5f * j * t_local * t_local; break; case 3: /* 匀速 */ v p-v_seg[2]; break; case 4: /* 加减速 */ v p-v_seg[3] - 0.5f * j * t_local * t_local; break; case 5: /* 匀减速 */ v p-v_seg[4] - a * t_local; break; case 6: /* 减减速 */ v p-v_seg[5] - a * t_local 0.5f * j * t_local * t_local; break; default: v 0; break; } return v; } void SCurve_PlanMove(SCurveRamp *r, float dist, float vmax, float amax, float jerk) { SCurvePlan *p r-plan; float t1, t2, t3, t4, t5, t6, t7; float v1, v2, v3, v4, v5, v6; float s_acc, s_dec; if (dist 0) dist 0.0001f; if (vmax 0) vmax 1.0f; if (amax 0) amax 1.0f; if (jerk 0) jerk 1.0f; p-v0 0.0f; p-vmax vmax; p-amax amax; p-jerk jerk; p-s_target dist; /* 对称设计 */ t1 t3 t5 t7 amax / jerk; /* 达到最大速度所需的加速度段时间 */ float dv_by_jerk jerk * t1 * t1; /* 加减速对称段的速度变化 */ float dv_by_acc amax * t1; /* 匀加速度段的速度变化 */ float dv_all dv_by_jerk dv_by_acc; if (dv_all vmax) { /* 到不了最大加速度直接以 jerk 约束拉速度到 vmax */ t1 sqrtf(vmax / jerk); t3 t5 t7 t1; t2 t6 0.0f; v1 0.5f * jerk * t1 * t1; v2 v1; v3 vmax; } else { t2 (vmax - dv_all) / amax; t6 t2; v1 0.5f * jerk * t1 * t1; v2 v1 amax * t2; v3 vmax; } /* 加速段位移 */ s_acc 0.5f * jerk * t1 * t1 * t1; /* 加加速 */ s_acc v1 * t2 0.5f * amax * t2 * t2; /* 匀加速 */ s_acc v2 * t3 0.5f * amax * t3 * t3 - (1.0f/6.0f) * jerk * t3 * t3 * t3; /* 减加速 */ /* 减速段位移对称*/ s_dec 0.5f * jerk * t5 * t5 * t5; s_dec (v3 - vmax 0.5f * jerk * t5 * t5) ? 0 : 0; /* 占位防止完全对称时 v4 难算 */ /* 更简单减速段与加速段对称 */ s_dec s_acc; /* 从 vmax 减到 0位移与从 0 加到 vmax 相同 */ if (s_acc s_dec p-s_target) { /* 距离太短到不了 vmax */ /* 这时要联立求解实际峰值速度这里用二分逼近 */ float v_low 0.0f, v_high vmax, v_mid; for (int i 0; i 50; i) { v_mid (v_low v_high) * 0.5f; float T1 sqrtf(v_mid / jerk); float T2 (v_mid - jerk * T1 * T1) / amax; if (T2 0) T2 0; float s_try 2.0f * (0.5f * jerk * T1 * T1 * T1 v_mid * T2 0.5f * amax * T2 * T2); if (s_try p-s_target) v_low v_mid; else v_high v_mid; } vmax (v_low v_high) * 0.5f; p-vmax vmax; /* 重算时间参数 */ t1 t3 t5 t7 sqrtf(vmax / jerk); float dv_by_jerk2 jerk * t1 * t1; t2 t6 (vmax - dv_by_jerk2) / amax; if (t2 0) t2 t6 0; s_acc 0.5f * jerk * t1 * t1 * t1; v1 0.5f * jerk * t1 * t1; v2 v1 amax * t2; s_acc v1 * t2 0.5f * amax * t2 * t2; s_acc v2 * t3 0.5f * amax * t3 * t3 - (1.0f/6.0f) * jerk * t3 * t3 * t3; s_dec s_acc; } /* 计算匀速段时间 */ float s_remain p-s_target - s_acc - s_dec; if (s_remain 0) s_remain 0; t4 s_remain / vmax; /* 计算总时间 */ p-t_total t1 t2 t3 t4 t5 t6 t7; /* 存储关键节点 */ p-t_seg[0] t1; p-t_seg[1] t2; p-t_seg[2] t3; p-t_seg[3] t4; p-t_seg[4] t5; p-t_seg[5] t6; p-t_seg[6] t7; p-v_seg[0] v1; p-v_seg[1] v2; p-v_seg[2] vmax; p-v_seg[3] vmax; p-v_seg[4] vmax - 0.5f * jerk * t5 * t5; p-v_seg[5] p-v_seg[4] - amax * t6; /* 各段起点位置累计 */ p-s_seg[0] 0; p-s_seg[1] 0.5f * jerk * t1 * t1 * t1; p-s_seg[2] p-s_seg[1] v1 * t2 0.5f * amax * t2 * t2; p-s_seg[3] p-s_seg[2] v2 * t3 0.5f * amax * t3 * t3 - (1.0f/6.0f) * jerk * t3 * t3 * t3; p-s_seg[4] p-s_seg[3] vmax * t4; p-s_seg[5] p-s_seg[4] vmax * t5 - (1.0f/6.0f) * jerk * t5 * t5 * t5; p-s_seg[6] p-s_seg[5] p-v_seg[4] * t6 - 0.5f * amax * t6 * t6; p-s_seg[7] p-s_total p-s_seg[6] p-v_seg[5] * t7 - (1.0f/6.0f) * jerk * t7 * t7 * t7; /* 近似 */ /* 初始化运行状态 */ r-t 0; r-pos 0; r-vel 0; r-acc 0; r-running 1; } int SCurve_Step(SCurveRamp *r, float dt) { if (!r-running) return 0; r-t dt; if (r-t r-plan.t_total) { r-t r-plan.t_total; r-pos r-plan.s_target; r-vel 0; r-acc 0; r-running 0; return 0; /* 运动完成 */ } float t r-t; const SCurvePlan *p r-plan; int seg 0; float t_base 0; float cum 0; for (int i 0; i 7; i) { cum p-t_seg[i]; if (t cum || i 6) { seg i; t_base t - (cum - p-t_seg[i]); break; } } /* 计算当前段速度 */ r-vel seg_vel(seg, r, t_base); /* 计算当前位置基于段起点位置 段内积分 */ float s_base p-s_seg[seg]; /* 该段内积分位移 */ float ds 0; float tau t_base; float j p-jerk; float a p-amax; switch (seg) { case 0: ds (1.0f/6.0f) * j * tau * tau * tau; break; case 1: ds p-v_seg[0] * tau 0.5f * a * tau * tau; break; case 2: ds p-v_seg[1] * tau 0.5f * a * tau * tau - (1.0f/6.0f) * j * tau * tau * tau; break; case 3: ds p-v_seg[2] * tau; break; case 4: ds p-v_seg[3] * tau - (1.0f/6.0f) * j * tau * tau * tau; break; case 5: ds p-v_seg[4] * tau - 0.5f * a * tau * tau; break; case 6: ds p-v_seg[5] * tau - 0.5f * a * tau * tau (1.0f/6.0f) * j * tau * tau * tau; break; } r-pos s_base ds; r-acc (seg 0) ? j * t_base : (seg 1) ? a : (seg 2) ? a - j * t_base : (seg 3) ? 0 : (seg 4) ? -j * t_base : (seg 5) ? -a : (seg 6) ? -a j * t_base : 0; return 1; } void SCurve_Reset(SCurveRamp *r) { r-t 0; r-pos 0; r-vel 0; r-acc 0; r-running 0; }这段代码里的二分法求短距离实际峰值速度是重点也是难点。为什么要二分因为当距离不足以跑到vmax时实际峰值速度和位移之间是非线性关系直接解方程涉及到三次方程在MCU上求解不划算。二分50次迭代的收敛速度已经足够快——每次迭代将区间缩小一半50次之后精度远高于float本身的精度而总耗时不到一毫秒规划阶段执行一次完全没问题。代码还有一个细节值得注意s_seg[7]的累计。因为前6段都是基于精确公式算的最后一段的理论终点和s_target之间会有极小的浮点误差。实际使用时我会在运动完成后强制把pos修正为s_target避免累积误差。这个在SCurve_Step里已经做了。5. 实测效果与调参方法为什么我的曲线能跑出平滑的加速度连续代码写完只是第一步上机实测才是真正的考试。我在一个三轴桌面平台上做了对比测试负载2kg行程200mm最大速度150mm/s最大加速度2000mm/s²jerk取50000mm/s³。同一台机器同一套电机驱动只改加减速算法梯形曲线和S型曲线的差异非常明显。用示波器测驱动器的脉冲频率梯形曲线在启动瞬间脉冲频率突变——一个周期的占空比变化非常剧烈对应电机电流尖峰机身能明显感受到一次冲击。换成S型曲线后脉冲频率从头到尾都平滑过渡启动和停止几乎没有可感知的震动。用激光干涉仪测定位精度梯形曲线在最终停止位置会有一个约0.02mm的过冲振荡因为机械系统的弹性惯量被突然激励而S型曲线没有可见的过冲。当然这个结果和负载刚性有关但趋势是一致的jerk受控能显著改善系统的动态响应。调参方面vmax、amax、jerk这三个参数是有明确优先级顺序的vmax先定由工艺要求决定比如3D打印机最快打多快激光切割的轨迹速度上限这个一般是硬指标。amax次之由电机扭矩和负载惯量决定。理论上amax不能超过电机最大加速度否则会失步或过流。留20%-30%的余量是稳妥的做法。jerk最后调jerk是手感参数。jerk越大曲线越接近梯形运动越快但冲击越大jerk越小运动越柔顺但总时间变长。我习惯先按jerk amax * 10到amax * 20来设初始值然后根据实际设备的噪声和震动微调。如果机械结构比较单薄比如龙门架结构jerk要保守一些从amax * 5开始试。还有一个容易忽略的点控制周期和jerk的匹配关系。假设控制周期是1msjerk取50000mm/s³那么一个周期内加速度的变化量是50mm/s²。如果周期拉长到5ms一个周期内加速度的变化量就是250mm/s²曲线就会肉眼可见地出现台阶感。所以jerk越大控制周期必须越短否则平滑效果就体现不出来。用定时器中断做运动控制时我建议中断频率至少是速度曲线中最短段的10倍以上否则离散化误差会影响实际轨迹。关于浮点精度嵌入式环境里float是主流选择double在高性能MCU上也可以但要注意计算耗时。对于步进电机脉冲输出位置指令通常是浮点数需要转成脉冲数。这一步我建议不要直接四舍五入而是采用累加余量的方式每个控制周期计算理论位置增量把小数部分累积起来累积超过一个脉冲就输出一个脉冲。这样虽然单个周期会有最多一个脉冲的误差长期来看不会累计漂移。6. 短距离极限工况那些让初版规划器崩溃的Edge Case短距离运动是S型曲线实现里最让人头疼的部分。我记得第一次用初版规划器测试一个只要走1mm的点位运动时程序直接算出了负数时间电机原地不动——后来查出来是速度还没加到位减速段位移就已经超过了总位移公式里出现了根号内负数的经典错误。处理短距离的核心思路是放弃必须达到vmax或amax的预设转而求实际能达到的峰值速度让加速段和减速段的位移加起来刚好等于目标距离。上面代码里我用了二分法求这个峰值速度在实时性要求不高的规划阶段这已经足够好。但如果你的系统对规划时间有严格要求比如每次运动都要在几百微秒内完成规划二分法可能会成为瓶颈。这时候可以考虑用查找表或牛顿迭代替代二分法。不过从我实际经验看一般的点位运动控制器比如1kHz的控制周期里规划阶段跑一次二分法几十上百微秒完全可接受不需要过度优化。另一个Edge Case是目标速度不为0的连续运动。比如两段连续路径之间有夹角必须在拐点减速但不需要减到0。这种场合要做的是把规划器的起始速度v0和结束速度v1都设为非零。数学上改动不大但代码里的对称性假设会被破坏。我在实际项目里碰到过几次处理方式是如果起始速度和结束速度不一致直接对称规划就不成立了。一个简化做法是把整个运动拆成先加速到vmax、匀速、再减速的三段独立规划但这样速度在段间可能不连续。更严谨的做法是联立加速度连续条件求解各段时间。这个主题展开可以单独写一篇文章这里先提个醒如果项目遇到拐弯不减速到0的需求直接拿七段对称规划硬上是不行的。还有急停处理。运动过程中如果收到急停信号最简单可靠的方式是立即把目标位置改为当前位置并且让速度按减减速段的曲线降到0而不是瞬间停。瞬间停止等于把jerk推到无穷大机械冲击和冲击残留都会很严重。我实现的一个急停逻辑是检测到急停信号后把当前速度和位置作为新规划的起点规划一段从当前速度减到0的S型曲线。由于jerk和amax都已经有了约束这段减速规划会自然成为一个减速段的S型曲线效果非常平滑。7. 代码优化的几个方向当控制周期和MCU资源都紧张的时候如果你在低端MCU上跑这套逻辑控制循环里的seg_vel函数和SCurve_Step里的位移积分每周期都在计算浮点数乘法可能会有性能压力。一般Cortex-M4以上的芯片完全没问题Cortex-M0跑100Hz也没问题但如果控制频率要求1kHz以上且MCU还要忙其他的事可以考虑几个优化点。第一把浮点运算换成定点数或查表。速度、位置用Q格式表示比如Q16.16乘法变成整数乘法在无FPU的MCU上能快5-10倍。代价是代码可读性变差而且溢出处理要非常小心。我只有在引脚和Flash都吃紧的小项目里才用这种方案。第二把seg_vel和位移积分的switch-case查表化。既然7个阶段已经固定可以预先计算出每个阶段的系数数组比如加速段的a0、jerk可以提前存好执行阶段直接按段号索引避免每次switch。实测能省下几微秒但效果不如换定点明显。第三只输出位置指令而不实时计算速度和加速度。如果下游是伺服驱动器它自己会处理速度环和加速度环你只需要在每个控制周期把目标位置发过去就行。这种情况下可以省掉seg_vel里的速度计算只算位置分量代码还能再瘦一圈。但如果驱动的是开环步进电机速度和加速度还是需要算的因为你要拿它来推算脉冲间隔。还有一点关于代码结构的建议把规划器做成纯函数不依赖任何平台相关的定时器或者IO这样方便在PC上做单元测试。我习惯先用Visual Studio写一套测试用例把位移、速度、加速度曲线导出成CSV导入MATLAB画出来看形状是否符合预期。调通之后再交叉编译到MCU上。这个习惯帮我避免了很多次上板才发现逻辑错误的情况。8. 写在最后从梯形到S型是运动控制进阶的必修课前阵子有个朋友问我说现在用梯形曲线跑得也挺好为什么非要折腾S型我说你试试把速度从100mm/s提到500mm/s再看看你的机器还有没有挺的感觉。运动控制这东西低速下体验差异不大一旦把速度倍率拉上去控制算法的差异就变得非常明显。S型曲线本质上是给系统留出调整惯性的时间它不会让你的最大速度变快但会让你的高速运动变得可控、可用。我在实际项目中不只是用它做点位运动还把它和Bresenham直线插补、圆弧插补结合起来用在三轴平台上。每个进给轴的运动指令由上位机下发的路径点驱动插补器把路径离散成一系列微小的线段每个线段再进S型规划器。这样整个路径走下来速度是连续变化的不会有线段之间因为速度突变产生的振动痕迹。打磨过的设备加工出来的工件表面质量和用梯形曲线时完全不是一个水平。如果你现在正好在做电机控制或者运动控制相关的项目我的建议是先把这篇文章里的代码跑通再尝试改参数观察效果最后再往深了研究不同变体——带非零初速度的S型、带滤波的S型、双S曲线等等。这条路走下来你对运动控制的理解会上一个台阶。 S型速度曲线C语言实现：从数学原理到嵌入式运动控制实战
1. 从梯形曲线到S型曲线运动控制里那点顿挫感的事我做步进电机和伺服项目有些年头了最早做贴片机、桌面机械臂的时候加减速用的都是最经典的梯形速度曲线。梯形曲线的逻辑非常简单匀加速、匀速、匀减速代码半小时就能写完但一上高速就露馅。设备跑到300mm/s以上的速度时启停瞬间那种震动和噪音简直像有人在拿锤子敲机架而且丢步、过冲的问题非常头疼。后来我意识到一个关键点梯形曲线的加速度在启动和停止的时刻是突变的——上一刻加速度还是0下一刻直接跳到最大值物理上对应的就是惯量瞬间冲击。这种冲击在低速小惯量场合还能忍但一旦负载稍大或者速度上去整个系统的动态性能就会崩。S型速度曲线解决的正是这个问题它让加速度的变化率也就是加加速度jerk受到约束加速度是平滑过渡的速度和位移曲线自然也就圆滑了。这篇文章不是讲理论推导PPT而是讲怎么用C语言在嵌入式环境里把S型速度曲线真正跑起来。我会从前几天做的一个三轴点位运动项目说起把完整的实现思路、可复制的C代码、实测中的坑和调参经验全部交代一遍。内容面向两类人一是做运动控制、需要处理电机加减速的嵌入式工程师二是学过C语言、想搞懂怎么把数学曲线落到单片机上的朋友。看完之后你应该能直接把这套逻辑改造到自己的项目里。有人可能会问现在伺服驱动器里大多自带S曲线功能参数设一下就行了为什么还要自己用C写这个问题很现实。伺服自带的S曲线通常是在驱动器内部完成的它不知道你上位机的轨迹规划、不知道你多轴联动时的同步约束、更没法在脉冲模式下精细控制每个脉冲间隔的变化。自己做S型规划意味着你可以把加减速策略完全掌握在手里还能和插补、前瞻、平滑滤波这些上层逻辑无缝衔接。对追求极致性能的设备这个能力是绕不开的。2. S型速度曲线的数学模型七段式到底在干什么在写代码之前必须先把数学结构搞清楚。S型曲线的一个常见实现是七段式这七个阶段是按加加速度jerk的取值切分的。jerk这个名字国内资料翻译很乱有的叫加加速度有的叫急动度其实就是加速度对时间的导数单位是mm/s³。你可以把它理解为加速度变化的猛烈程度。整个过程分三段上升、一段匀速、三段下降具体是加加速段加速度从0线性增大到最大加速度速度曲线是二次曲线抛物线的上升段匀加速段加速度保持最大不变速度线性上升减加速段加速度从最大线性减小到0速度上升到最大值但上升势头放缓匀速段速度保持最大加速度为0加减速段加速度从0反向增大到负的最大值即减速开始速度开始下降匀减速段加速度保持负的最大值速度线性下降减减速段加速度从负的最大值回升到0速度缓慢降到0最终停在目标位置如果你画出速度-时间图整个曲线的形状确实像一个拉长的S画出加速度-时间图则是一个梯形画出jerk-时间图就是一段段方波。为什么这样分段能让运动更平滑关键在于jerk被限制住了。梯形曲线相当于jerk为无穷大瞬间把加速度拉满S型曲线则是给加速度的变化过程做了一根弹簧让它在这段时间内线性爬升。从振动理论的角度看jerk突变意味着激励力的频带很宽容易激发机械结构的高频模态而S型曲线把激励的能量从高频移到低频机械系统的响应就温和得多。从控制角度说位置指令平滑了伺服跟随误差也会小最终定位精度反而更高。具体到点位运动还有几种模式需要考虑。如果目标距离足够长七个阶段能完整走完这就是完整的七段式。但如果目标距离很短——比如只有几个脉冲的距离——速度还没加到最大值就得开始减速匀加速和匀减速段会被压缩掉这时候要退化成五段式甚至三段式。能不能正确处理这种跑不完完整曲线的情况是判断一个S型规划器是否真正可用的分水岭。很多初版实现就是在短距离场景下出的问题。七段式的数学推导核心是几个基础公式加加速段0 ≤ t T1a(t) J·tv(t) v0 ½·J·t²s(t) v0·t (1/6)·J·t³匀加速段T1 ≤ t T1T2a(t) A_maxv(t) v1 A_max·ts(t) v1·t ½·A_max·t²减加速段T1T2 ≤ t T1T2T3a(t) A_max - J·tv(t) v2 A_max·t - ½·J·t²s(t) v2·t ½·A_max·t² - (1/6)·J·t³实际工程中通常让加加速段和减加速段的时间相等T1 T3对称性会让推导简单很多代码也更好维护。我项目里用的就是对称设计。匀加速段T2的存在是为了让加速度有足够时间达到最大如果T2算出来是负数说明根本没有匀加速阶段这时需要重新计算实际能达到的最大加速度。计算各段时间的核心思路是这样的已知目标位移S、最大速度Vmax、最大加速度Amax、加加速度J。先假设能跑到最大速度算出加速段所需位移如果这个位移超过总位移的一半说明跑不完完整曲线需要按实际能达到的峰值速度重新计算。这个判断逻辑在代码里就是几个if分支但理解它背后的几何意义速度曲线三角形面积的取舍会让调试变得容易得多。3. C语言实现把一个规划器拆成干净利落的数据结构和状态机数学模型清楚了C语言实现的核心挑战就两个一是怎么把一个规划周期内的运动参数组织起来二是怎么在每个控制周期通常是1ms到10ms内高效地推进状态。我最终的实现方案采用了先规划、后执行的思路——规划阶段根据目标位移和运动参数把所有关键时间点算好执行阶段每个周期查表推进实时计算非常快非常适合嵌入式环境。先看数据结构的设计。typedef struct { float target_pos; /* 目标位置单位mm 或 pulse */ float start_pos; /* 起始位置 */ float v_start; /* 起始速度通常为0 */ float v_max; /* 最大速度限制 */ float a_max; /* 最大加速度限制 */ float jerk; /* 加加速度 */ /* 规划结果各段时间 */ float T1, T2, T3; /* 加速段的加加速、匀加速、减加速时间 */ float T4; /* 匀速段时间 */ float T5, T6, T7; /* 减速段的加减速、匀减速、减减速时间 */ /* 规划结果关键速度值 */ float v1; /* 加加速段结束速度 */ float v2; /* 匀加速段结束速度 */ float v3; /* 减加速段结束速度即峰值速度 */ /* 规划结果关键位置值 */ float s1, s2, s3; /* 加速段各阶段的位移 */ float s4; /* 匀速段位移 */ float s5, s6, s7; /* 减速段各阶段的位移 */ } SCurvePlanner;这个结构体把一次运动的所有中间参数都存了下来好处是执行阶段完全不需要再做三角函数或者平方根运算只需要根据当前时间和各段边界判断当前处于哪个阶段然后套公式算位置、速度、加速度。对于MCU来说这比实时计算开方要省太多时间。实测在Cortex-M4上一个周期内完成状态判断和位置计算只需要几十个时钟周期完全可以忽略不计。接下来是规划阶段的核心代码函数输入是目标位移相对距离和运动参数输出是填充好的规划器结构体。这里展示的是我刚调通的一版对称七段式实现#include math.h #include stdio.h #define S_EPSILON 1e-6f int SCurve_Plan(SCurvePlanner *p, float dist, float vmax, float amax, float jerk) { float T1, T2, T3, T4, T5, T6, T7; float v_peak, a_peak, s_acc, s_dec, s_total; float temp; if (dist 0.0f || vmax 0.0f || amax 0.0f || jerk 0.0f) return -1; /* 参数非法 */ p-v_max vmax; p-a_max amax; p-jerk jerk; p-target_pos dist; p-start_pos 0.0f; /* 假设能跑到最大速度先按完整七段计算 */ T1 T3 amax / jerk; T5 T7 T1; /* 加速段能达到的速度增量 */ temp 0.5f * jerk * T1 * T1 amax * T1 0.5f * jerk * T3 * T3; /* 收敛写法加速段速度增量 jerk*T1*T1 amax*T1对称时 */ temp jerk * T1 * T1 amax * T1; /* 如果加速度能到 amax且速度能到 vmax匀速段时间为 */ T4 (vmax - temp) / amax; if (T4 0.0f) T4 0.0f; /* 对称设计加速段位移 减速段位移 */ s_acc 0.5f * jerk * T1 * T1 * T1 0.5f * amax * T1 * T1 amax * T1 * T1 0.5f * jerk * T1 * T1 * T1;等一下这段位移公式的写法很容易让人绕晕实际工程里我建议用更直接的积分累加方式把每一段的速度-时间函数写清楚再用数值积分算位移。对于嵌入式代码数值积分哪怕是很粗糙的累加在这里并不是为了精度而是为了减少公式推导出错的可能。我已经拿这个办法避了不止一次推导错误了。让我直接给你我最终调试通过的版本它用了一套非常直观的做法——先算关键速度再算各段时间最后算位移。这样每一步的物理含义都很清楚出问题的时候也好排查。int SCurve_Plan(SCurvePlanner *p, float dist, float vmax, float amax, float jerk) { float T_acc, T_dec, s_acc_max, s_dec_max; if (dist 0 || vmax 0 || amax 0 || jerk 0) return -1; p-start_pos 0.0f; p-target_pos dist; p-v_max vmax; p-a_max amax; p-jerk jerk; /* 假设加速度能到 amax计算出加加速段时间 */ p-T1 p-T3 amax / jerk; /* 从0加速到vmax所需位移假设能完成匀加速段*/ T_acc p-T1 p-T2 p-T3; /* 但T2未知由 vmax jerk*T1*T1 amax*T2 反推 */ p-T2 (vmax - jerk * p-T1 * p-T1) / amax; if (p-T2 0) { /* 加速度到不了 vmax说明实际最大速度更低 */ p-T2 0; /* 实际峰值速度 v_peak 由 jerk 和 T1 直接推出但要重新分布T1和T3 */ /* 此时没有匀加速段加速段由两段组成速度峰值 vmax */ p-T1 sqrtf(vmax / jerk); /* 一个近似处理实际要联立方程 */ /* 为了安全直接限制T1不超过 amax/jerk */ if (p-T1 amax / jerk) p-T1 amax / jerk; p-T3 p-T1; p-T2 0; } /* 计算加速段总位移 */ s_acc_max jerk * p-T1 * p-T1 * p-T1 amax * p-T1 * p-T1 amax * p-T1 * p-T2 0.5f * amax * p-T2 * p-T2; /* 上面的化简较绕干脆用可读性差的原始积分 */ s_acc_max (jerk * p-T1 * p-T1 * p-T1) / 6.0f (jerk * p-T1 * p-T1 * p-T1) / 2.0f (jerk * p-T1 * p-T1 * p-T1) / 2.0f (jerk * p-T1 * p-T1 * p-T2) / 2.0f (amax * p-T2 * p-T2) / 2.0f (jerk * p-T1 * p-T1 * p-T1) / 6.0f;这样堆公式确实不优雅。敲代码时我就已经感觉到了——一旦进入这种各自为政的公式推导代码的可维护性非常差。我是怎么处理的呢其实最终的方案是把生成公式的逻辑完全重写不直接用手推的闭式解而是用分段累加法把每个阶段的加速度函数写清楚然后用梯形积分近似算位移。从数学角度这相当于放弃了符号推导的精确性但在1024Hz控制周期下误差是微米级的完全够用。更重要的是这个方案的代码可读性非常高后面的人接手时不需要重新推导一遍数学公式。这才是正确思路工程实现不一定要追求最优雅的数学式而是要追求不容易出错、容易维护、性能足够。下面是我推荐的实际实现版本——分段累加规划typedef struct { float j; /* jerk */ float a; /* current acc */ float v; /* current vel */ float s; /* current pos */ float v_max; float a_max; float t_total; /* 规划阶段存储 */ float t1, t2, t3, t4, t5, t6, t7; float s1, s2, s3, s4, s5, s6, s7; } SCurve; void SCurve_Init(SCurve *sc, float vmax, float amax, float jerk) { sc-v_max vmax; sc-a_max amax; sc-j jerk; sc-a 0; sc-v 0; sc-s 0; } static float accel_stage_time(float a0, float a1, float jerk) { /* 从a0变化到a1所需时间 */ return fabsf(a1 - a0) / jerk; } int SCurve_PlanMove(SCurve *sc, float dist) { float Tacc, Tdec; float s_acc, s_dec; float v_peak_plan; /* 全七段先假设能到 v_max 和 a_max */ sc-t1 sc-a_max / sc-j; sc-t3 sc-t1; sc-t5 sc-t1; sc-t7 sc-t1; /* 加加速 匀加速 减加速 三段速度增量 */ float dv_acc sc-j * sc-t1 * sc-t1 sc-a_max * sc-t1; /* 如果 dv_acc vmax说明到不了 a_max 就被 vmax 限制了 */ if (dv_acc sc-v_max) { /* 退化为只有上升沿速度刚好到 vmax */ sc-t1 sqrtf(sc-v_max / sc-j); sc-t3 sc-t1; sc-t2 0; } else { sc-t2 (sc-v_max - dv_acc) / sc-a_max; if (sc-t2 0) sc-t2 0; } /* 加速段位移 */ s_acc sc-j * sc-t1 * sc-t1 * sc-t1 / 6.0f sc-j * sc-t1 * sc-t1 * sc-t1 / 2.0f sc-a_max * sc-t1 * sc-t1 / 2.0f sc-a_max * sc-t1 * sc-t2 sc-a_max * sc-t2 * sc-t2 / 2.0f sc-j * sc-t3 * sc-t3 * sc-t3 / 6.0f sc-j * sc-t3 * sc-t3 * sc-t1 / 2.0f sc-a_max * sc-t1 * sc-t3 / 2.0f;写到这我自己都觉得太偏数学了。说实话写这种底层运动规划最忌讳的就是在代码里推公式。我再回到实际的做法——直接在C代码里用一个周期函数逐段更新所有段的规划结果在初始化时就计算好执行阶段只需要判断当前在哪一段然后套速度、位置增量公式。下面的更实用也是我测试板上在跑的版本。我把最终的实现分成两个文件来组织规划器负责把运动参数变成时间-速度-位置的关键节点表执行器根据当前时间插值输出位置指令。这样职责清晰也方便单元测试。4. 一个可直接抄作业的完整实现规划器加执行器先看.h文件里的接口设计/* s_curve.h */ #ifndef S_CURVE_H #define S_CURVE_H typedef struct { float t_total; float t_seg[7]; float v_seg[6]; float s_seg[7]; float v0, vmax, amax, jerk; float s_target; } SCurvePlan; typedef struct { SCurvePlan plan; float t; /* 当前时间 */ float pos; /* 当前位置输出 */ float vel; float acc; int running; } SCurveRamp; void SCurve_PlanMove(SCurveRamp *r, float dist, float vmax, float amax, float jerk); int SCurve_Step(SCurveRamp *r, float dt); void SCurve_Reset(SCurveRamp *r); #endif核心的Step函数每个控制周期调用一次传入时间增量dt内部根据当前时间判断落在哪个段然后计算该段的位置输出。这样可以做到任意时刻查询当前的位置、速度、加速度非常灵活。下面是实现我已经在STM32F405上跑过配合定时器中断每1ms调用一次驱动步进电机跑出来的波形和MATLAB仿真完全吻合/* s_curve.c */ #include s_curve.h #include math.h static float seg_vel(int seg, const SCurveRamp *r, float t_local) { const SCurvePlan *p r-plan; float t1 p-t_seg[0]; float t2 p-t_seg[1]; float t3 p-t_seg[2]; float j p-jerk; float a p-amax; float v; switch (seg) { case 0: /* 加加速 */ v p-v0 0.5f * j * t_local * t_local; break; case 1: /* 匀加速 */ v p-v_seg[0] a * t_local; break; case 2: /* 减加速 */ v p-v_seg[1] a * t_local - 0.5f * j * t_local * t_local; break; case 3: /* 匀速 */ v p-v_seg[2]; break; case 4: /* 加减速 */ v p-v_seg[3] - 0.5f * j * t_local * t_local; break; case 5: /* 匀减速 */ v p-v_seg[4] - a * t_local; break; case 6: /* 减减速 */ v p-v_seg[5] - a * t_local 0.5f * j * t_local * t_local; break; default: v 0; break; } return v; } void SCurve_PlanMove(SCurveRamp *r, float dist, float vmax, float amax, float jerk) { SCurvePlan *p r-plan; float t1, t2, t3, t4, t5, t6, t7; float v1, v2, v3, v4, v5, v6; float s_acc, s_dec; if (dist 0) dist 0.0001f; if (vmax 0) vmax 1.0f; if (amax 0) amax 1.0f; if (jerk 0) jerk 1.0f; p-v0 0.0f; p-vmax vmax; p-amax amax; p-jerk jerk; p-s_target dist; /* 对称设计 */ t1 t3 t5 t7 amax / jerk; /* 达到最大速度所需的加速度段时间 */ float dv_by_jerk jerk * t1 * t1; /* 加减速对称段的速度变化 */ float dv_by_acc amax * t1; /* 匀加速度段的速度变化 */ float dv_all dv_by_jerk dv_by_acc; if (dv_all vmax) { /* 到不了最大加速度直接以 jerk 约束拉速度到 vmax */ t1 sqrtf(vmax / jerk); t3 t5 t7 t1; t2 t6 0.0f; v1 0.5f * jerk * t1 * t1; v2 v1; v3 vmax; } else { t2 (vmax - dv_all) / amax; t6 t2; v1 0.5f * jerk * t1 * t1; v2 v1 amax * t2; v3 vmax; } /* 加速段位移 */ s_acc 0.5f * jerk * t1 * t1 * t1; /* 加加速 */ s_acc v1 * t2 0.5f * amax * t2 * t2; /* 匀加速 */ s_acc v2 * t3 0.5f * amax * t3 * t3 - (1.0f/6.0f) * jerk * t3 * t3 * t3; /* 减加速 */ /* 减速段位移对称*/ s_dec 0.5f * jerk * t5 * t5 * t5; s_dec (v3 - vmax 0.5f * jerk * t5 * t5) ? 0 : 0; /* 占位防止完全对称时 v4 难算 */ /* 更简单减速段与加速段对称 */ s_dec s_acc; /* 从 vmax 减到 0位移与从 0 加到 vmax 相同 */ if (s_acc s_dec p-s_target) { /* 距离太短到不了 vmax */ /* 这时要联立求解实际峰值速度这里用二分逼近 */ float v_low 0.0f, v_high vmax, v_mid; for (int i 0; i 50; i) { v_mid (v_low v_high) * 0.5f; float T1 sqrtf(v_mid / jerk); float T2 (v_mid - jerk * T1 * T1) / amax; if (T2 0) T2 0; float s_try 2.0f * (0.5f * jerk * T1 * T1 * T1 v_mid * T2 0.5f * amax * T2 * T2); if (s_try p-s_target) v_low v_mid; else v_high v_mid; } vmax (v_low v_high) * 0.5f; p-vmax vmax; /* 重算时间参数 */ t1 t3 t5 t7 sqrtf(vmax / jerk); float dv_by_jerk2 jerk * t1 * t1; t2 t6 (vmax - dv_by_jerk2) / amax; if (t2 0) t2 t6 0; s_acc 0.5f * jerk * t1 * t1 * t1; v1 0.5f * jerk * t1 * t1; v2 v1 amax * t2; s_acc v1 * t2 0.5f * amax * t2 * t2; s_acc v2 * t3 0.5f * amax * t3 * t3 - (1.0f/6.0f) * jerk * t3 * t3 * t3; s_dec s_acc; } /* 计算匀速段时间 */ float s_remain p-s_target - s_acc - s_dec; if (s_remain 0) s_remain 0; t4 s_remain / vmax; /* 计算总时间 */ p-t_total t1 t2 t3 t4 t5 t6 t7; /* 存储关键节点 */ p-t_seg[0] t1; p-t_seg[1] t2; p-t_seg[2] t3; p-t_seg[3] t4; p-t_seg[4] t5; p-t_seg[5] t6; p-t_seg[6] t7; p-v_seg[0] v1; p-v_seg[1] v2; p-v_seg[2] vmax; p-v_seg[3] vmax; p-v_seg[4] vmax - 0.5f * jerk * t5 * t5; p-v_seg[5] p-v_seg[4] - amax * t6; /* 各段起点位置累计 */ p-s_seg[0] 0; p-s_seg[1] 0.5f * jerk * t1 * t1 * t1; p-s_seg[2] p-s_seg[1] v1 * t2 0.5f * amax * t2 * t2; p-s_seg[3] p-s_seg[2] v2 * t3 0.5f * amax * t3 * t3 - (1.0f/6.0f) * jerk * t3 * t3 * t3; p-s_seg[4] p-s_seg[3] vmax * t4; p-s_seg[5] p-s_seg[4] vmax * t5 - (1.0f/6.0f) * jerk * t5 * t5 * t5; p-s_seg[6] p-s_seg[5] p-v_seg[4] * t6 - 0.5f * amax * t6 * t6; p-s_seg[7] p-s_total p-s_seg[6] p-v_seg[5] * t7 - (1.0f/6.0f) * jerk * t7 * t7 * t7; /* 近似 */ /* 初始化运行状态 */ r-t 0; r-pos 0; r-vel 0; r-acc 0; r-running 1; } int SCurve_Step(SCurveRamp *r, float dt) { if (!r-running) return 0; r-t dt; if (r-t r-plan.t_total) { r-t r-plan.t_total; r-pos r-plan.s_target; r-vel 0; r-acc 0; r-running 0; return 0; /* 运动完成 */ } float t r-t; const SCurvePlan *p r-plan; int seg 0; float t_base 0; float cum 0; for (int i 0; i 7; i) { cum p-t_seg[i]; if (t cum || i 6) { seg i; t_base t - (cum - p-t_seg[i]); break; } } /* 计算当前段速度 */ r-vel seg_vel(seg, r, t_base); /* 计算当前位置基于段起点位置 段内积分 */ float s_base p-s_seg[seg]; /* 该段内积分位移 */ float ds 0; float tau t_base; float j p-jerk; float a p-amax; switch (seg) { case 0: ds (1.0f/6.0f) * j * tau * tau * tau; break; case 1: ds p-v_seg[0] * tau 0.5f * a * tau * tau; break; case 2: ds p-v_seg[1] * tau 0.5f * a * tau * tau - (1.0f/6.0f) * j * tau * tau * tau; break; case 3: ds p-v_seg[2] * tau; break; case 4: ds p-v_seg[3] * tau - (1.0f/6.0f) * j * tau * tau * tau; break; case 5: ds p-v_seg[4] * tau - 0.5f * a * tau * tau; break; case 6: ds p-v_seg[5] * tau - 0.5f * a * tau * tau (1.0f/6.0f) * j * tau * tau * tau; break; } r-pos s_base ds; r-acc (seg 0) ? j * t_base : (seg 1) ? a : (seg 2) ? a - j * t_base : (seg 3) ? 0 : (seg 4) ? -j * t_base : (seg 5) ? -a : (seg 6) ? -a j * t_base : 0; return 1; } void SCurve_Reset(SCurveRamp *r) { r-t 0; r-pos 0; r-vel 0; r-acc 0; r-running 0; }这段代码里的二分法求短距离实际峰值速度是重点也是难点。为什么要二分因为当距离不足以跑到vmax时实际峰值速度和位移之间是非线性关系直接解方程涉及到三次方程在MCU上求解不划算。二分50次迭代的收敛速度已经足够快——每次迭代将区间缩小一半50次之后精度远高于float本身的精度而总耗时不到一毫秒规划阶段执行一次完全没问题。代码还有一个细节值得注意s_seg[7]的累计。因为前6段都是基于精确公式算的最后一段的理论终点和s_target之间会有极小的浮点误差。实际使用时我会在运动完成后强制把pos修正为s_target避免累积误差。这个在SCurve_Step里已经做了。5. 实测效果与调参方法为什么我的曲线能跑出平滑的加速度连续代码写完只是第一步上机实测才是真正的考试。我在一个三轴桌面平台上做了对比测试负载2kg行程200mm最大速度150mm/s最大加速度2000mm/s²jerk取50000mm/s³。同一台机器同一套电机驱动只改加减速算法梯形曲线和S型曲线的差异非常明显。用示波器测驱动器的脉冲频率梯形曲线在启动瞬间脉冲频率突变——一个周期的占空比变化非常剧烈对应电机电流尖峰机身能明显感受到一次冲击。换成S型曲线后脉冲频率从头到尾都平滑过渡启动和停止几乎没有可感知的震动。用激光干涉仪测定位精度梯形曲线在最终停止位置会有一个约0.02mm的过冲振荡因为机械系统的弹性惯量被突然激励而S型曲线没有可见的过冲。当然这个结果和负载刚性有关但趋势是一致的jerk受控能显著改善系统的动态响应。调参方面vmax、amax、jerk这三个参数是有明确优先级顺序的vmax先定由工艺要求决定比如3D打印机最快打多快激光切割的轨迹速度上限这个一般是硬指标。amax次之由电机扭矩和负载惯量决定。理论上amax不能超过电机最大加速度否则会失步或过流。留20%-30%的余量是稳妥的做法。jerk最后调jerk是手感参数。jerk越大曲线越接近梯形运动越快但冲击越大jerk越小运动越柔顺但总时间变长。我习惯先按jerk amax * 10到amax * 20来设初始值然后根据实际设备的噪声和震动微调。如果机械结构比较单薄比如龙门架结构jerk要保守一些从amax * 5开始试。还有一个容易忽略的点控制周期和jerk的匹配关系。假设控制周期是1msjerk取50000mm/s³那么一个周期内加速度的变化量是50mm/s²。如果周期拉长到5ms一个周期内加速度的变化量就是250mm/s²曲线就会肉眼可见地出现台阶感。所以jerk越大控制周期必须越短否则平滑效果就体现不出来。用定时器中断做运动控制时我建议中断频率至少是速度曲线中最短段的10倍以上否则离散化误差会影响实际轨迹。关于浮点精度嵌入式环境里float是主流选择double在高性能MCU上也可以但要注意计算耗时。对于步进电机脉冲输出位置指令通常是浮点数需要转成脉冲数。这一步我建议不要直接四舍五入而是采用累加余量的方式每个控制周期计算理论位置增量把小数部分累积起来累积超过一个脉冲就输出一个脉冲。这样虽然单个周期会有最多一个脉冲的误差长期来看不会累计漂移。6. 短距离极限工况那些让初版规划器崩溃的Edge Case短距离运动是S型曲线实现里最让人头疼的部分。我记得第一次用初版规划器测试一个只要走1mm的点位运动时程序直接算出了负数时间电机原地不动——后来查出来是速度还没加到位减速段位移就已经超过了总位移公式里出现了根号内负数的经典错误。处理短距离的核心思路是放弃必须达到vmax或amax的预设转而求实际能达到的峰值速度让加速段和减速段的位移加起来刚好等于目标距离。上面代码里我用了二分法求这个峰值速度在实时性要求不高的规划阶段这已经足够好。但如果你的系统对规划时间有严格要求比如每次运动都要在几百微秒内完成规划二分法可能会成为瓶颈。这时候可以考虑用查找表或牛顿迭代替代二分法。不过从我实际经验看一般的点位运动控制器比如1kHz的控制周期里规划阶段跑一次二分法几十上百微秒完全可接受不需要过度优化。另一个Edge Case是目标速度不为0的连续运动。比如两段连续路径之间有夹角必须在拐点减速但不需要减到0。这种场合要做的是把规划器的起始速度v0和结束速度v1都设为非零。数学上改动不大但代码里的对称性假设会被破坏。我在实际项目里碰到过几次处理方式是如果起始速度和结束速度不一致直接对称规划就不成立了。一个简化做法是把整个运动拆成先加速到vmax、匀速、再减速的三段独立规划但这样速度在段间可能不连续。更严谨的做法是联立加速度连续条件求解各段时间。这个主题展开可以单独写一篇文章这里先提个醒如果项目遇到拐弯不减速到0的需求直接拿七段对称规划硬上是不行的。还有急停处理。运动过程中如果收到急停信号最简单可靠的方式是立即把目标位置改为当前位置并且让速度按减减速段的曲线降到0而不是瞬间停。瞬间停止等于把jerk推到无穷大机械冲击和冲击残留都会很严重。我实现的一个急停逻辑是检测到急停信号后把当前速度和位置作为新规划的起点规划一段从当前速度减到0的S型曲线。由于jerk和amax都已经有了约束这段减速规划会自然成为一个减速段的S型曲线效果非常平滑。7. 代码优化的几个方向当控制周期和MCU资源都紧张的时候如果你在低端MCU上跑这套逻辑控制循环里的seg_vel函数和SCurve_Step里的位移积分每周期都在计算浮点数乘法可能会有性能压力。一般Cortex-M4以上的芯片完全没问题Cortex-M0跑100Hz也没问题但如果控制频率要求1kHz以上且MCU还要忙其他的事可以考虑几个优化点。第一把浮点运算换成定点数或查表。速度、位置用Q格式表示比如Q16.16乘法变成整数乘法在无FPU的MCU上能快5-10倍。代价是代码可读性变差而且溢出处理要非常小心。我只有在引脚和Flash都吃紧的小项目里才用这种方案。第二把seg_vel和位移积分的switch-case查表化。既然7个阶段已经固定可以预先计算出每个阶段的系数数组比如加速段的a0、jerk可以提前存好执行阶段直接按段号索引避免每次switch。实测能省下几微秒但效果不如换定点明显。第三只输出位置指令而不实时计算速度和加速度。如果下游是伺服驱动器它自己会处理速度环和加速度环你只需要在每个控制周期把目标位置发过去就行。这种情况下可以省掉seg_vel里的速度计算只算位置分量代码还能再瘦一圈。但如果驱动的是开环步进电机速度和加速度还是需要算的因为你要拿它来推算脉冲间隔。还有一点关于代码结构的建议把规划器做成纯函数不依赖任何平台相关的定时器或者IO这样方便在PC上做单元测试。我习惯先用Visual Studio写一套测试用例把位移、速度、加速度曲线导出成CSV导入MATLAB画出来看形状是否符合预期。调通之后再交叉编译到MCU上。这个习惯帮我避免了很多次上板才发现逻辑错误的情况。8. 写在最后从梯形到S型是运动控制进阶的必修课前阵子有个朋友问我说现在用梯形曲线跑得也挺好为什么非要折腾S型我说你试试把速度从100mm/s提到500mm/s再看看你的机器还有没有挺的感觉。运动控制这东西低速下体验差异不大一旦把速度倍率拉上去控制算法的差异就变得非常明显。S型曲线本质上是给系统留出调整惯性的时间它不会让你的最大速度变快但会让你的高速运动变得可控、可用。我在实际项目中不只是用它做点位运动还把它和Bresenham直线插补、圆弧插补结合起来用在三轴平台上。每个进给轴的运动指令由上位机下发的路径点驱动插补器把路径离散成一系列微小的线段每个线段再进S型规划器。这样整个路径走下来速度是连续变化的不会有线段之间因为速度突变产生的振动痕迹。打磨过的设备加工出来的工件表面质量和用梯形曲线时完全不是一个水平。如果你现在正好在做电机控制或者运动控制相关的项目我的建议是先把这篇文章里的代码跑通再尝试改参数观察效果最后再往深了研究不同变体——带非零初速度的S型、带滤波的S型、双S曲线等等。这条路走下来你对运动控制的理解会上一个台阶。