【嵌入式必读】彻底理解步进电机梯形加减速运动算法原理

步进电机梯形加速度

关于代码实现，请大家看我这篇文章，所有代码均经过验证，可以直接拿来用。https://blog.csdn.net/yingzhefengyuzou/article/details/132199922?spm=1001.2014.3001.5502

为什么要使用加减速呢？步进电机有一个很重要的技术参数：空载启动频率，也就是在没有负载的情况下能够正常启动的最大脉冲频率，如果脉冲频率大于该值，步进电机则不能够正常启动，发生丢步或者堵转的情况；或者也可以理解为由于步进脉冲变化过快，转子由于惯性的作用跟不上电信号的变化。所以要使用加减速来解决启动频率低的问题，在启动时使用较低的脉冲频率，然后逐渐的加快频率。

梯形加减速一共分为三个阶段，OA 加速部分、AB 匀速部分 和 BC 减速部分。 在 OA 加速过程中，由低于步进电机的启动频率开始启动（模型中由 0 启动），以固定的加速度增加速度到目标值；在 AB 匀速过程中，以最大速度匀速运动； 在 BC 减速部分中，以加速度不变的速度递减到 0； 这种算法是一种在加速过程和减速过程中加速度不变的匀变速控制算法，由于速度变化的曲线有折点，所以在启动、停止、匀速段中很容易产生冲击和振动。

步进电机的转动需要控制器发送脉冲，如果控制器以恒定速度发送脉冲，那么步进电机就以恒定速度转动；如果控制器以加速度发送脉冲，那么步进电机就以加速度运动；所以只要改变脉冲的频率 就可以改变速度的变化，也就是说调整脉冲之间的时间间隔就可以改变速度。

上图为步进电机与时间的示意图，其中

以 stm32 的高级定时器 8 为例，使用定时器PWM输出模式给控制器发送脉冲。高级定时器 8 的时钟频率为 168MHZ, 如果将分频值设置为 5，那么定时器的时钟频率则为： f t f_t ft​=168/(5+1)=28MHZ, 相当于计数 28M 次正好为一秒，周期与频率为倒 数关系，所以分频值为 5 的定时器 8 的计数周期 t t = 1 / f t t_t = 1/f_t tt​=1/ft​; C 0 C_0 C0​和 C 1 C_1 C1​的值即为ARR寄存器的值，所以我们在固定的定时器频率下，只需要改变ARR的值，即可改变电机的速度。

我们通过控制定时器输出PWM波形来描绘上述梯形，至少需要知道以下变量：

电机转过的角度θ等于脉冲数乘以步距角 θ = n ∗ α (1) \boxed{θ = n * α} \tag{1} θ=n∗α​(1) 其中：n表示驱动电机的脉冲数，α表示电机的步距角

由匀加速直线运动公式： S = ν 0 t + 1 2 a t 2 (2) \boxed{S =ν_0 t+{1\over2} at^2} \tag{2} S=ν0​t+21​at2​(2) 可推导出,电机转过的角度θ： θ = ω 0 t + 1 2 ω ˊ t n 2 (3) \boxed{θ = ω_0 t+{1\over2}ώt_n^2} \tag{3} θ=ω0​t+21​ωˊtn2​​(3) 其中：θ为转过的角度 ， ω 0 ω_0 ω0​为初角速度，ώ为加角速度， t n t_n tn​为所用的时间

当初角速度 ω 0 = 0 ω_0 = 0 ω0​=0时，电机转过θ所用的时间 t n t_n tn​： t n = 2 θ ω ˊ (4) \boxed{t_n = \sqrt{2θ\over{ώ}}} \tag{4} tn​=ωˊ2θ​ ​​(4)

时间间隔Δt： Δ t = C n ∗ t t (5) \boxed{Δt = C_n*t_t} \tag{5} Δt=Cn​∗tt​​(5) 其中： C n C_n Cn​表示定时器计数器相邻两脉冲间的计数值， t t t_t tt​表示定时器的时钟周期，定时器频率的倒数 假设相邻两脉冲间转过的角度分别用 θ t θ_t θt​和 θ t − 1 θ_{t-1} θt−1​表示，所用时间用 t n t_n tn​和 t n − 1 t_{n-1} tn−1​表示，则Δt根据公式（4）也可以表示为 Δ t = t n – t n − 1 = 2 θ t ω ˊ − 2 θ t − 1 ω ˊ \boxed{Δt = t_n – t_{n-1}\\ = \sqrt{2θ_t\over{ώ}} - \sqrt{2θ_{t-1}\over{ώ}} } Δt=tn​–tn−1​=ωˊ2θt​​ ​−ωˊ2θt−1​​ ​​ 上式中n表示转过 θ t − 1 θ_{t-1} θt−1​角度时，所用的脉冲数 Δ t = t n – t n − 1 = 2 θ t ω ˊ − 2 θ t − 1 ω ˊ \begin{aligned} Δt &= t_n – t_{n-1} \\ &= \sqrt{2θ_t\over{ώ}} - \sqrt{2θ_{t-1}\over{ώ}} \end{aligned} Δt​=tn​–tn−1​=ωˊ2θt​​ ​−ωˊ2θt−1​​ ​​ 上式中 θ t θ_t θt​和 θ t − 1 θ_{t-1} θt−1​根据公式(1)可得出： θ t = ( n + 1 ) α , θ t − 1 = n α θ_t = (n+1) α, θ_t-1 = nα θt​=(n+1)α,θt​−1=nα,则带入最终可得到公式（6） Δ t = 2 α ω ˊ ( n + 1 − n ) (6) \boxed{Δt = \sqrt{2α\overώ} (\sqrt{n+1}- \sqrt{n})}\tag{6} Δt=ωˊ2α​ ​(n+1 ​−n ​)​(6) 上式中n表示转过 θ t − 1 θ_{t-1} θt−1​角度时，所用的脉冲数 此处计算时，让 θ t = ( n + 1 ) α , θ t − 1 = n α ， θ_t = (n+1) α, θ_{t-1} = nα， θt​=(n+1)α,θt−1​=nα，而不是 θ t = n α θ_t = nα θt​=nα, θ t − 1 = （ n − 1 ） α θ_{t-1} = （n-1）α θt−1​=（n−1）α，是因为Δt是两脉冲之间的时间间隔， θ t − 1 θ_{t-1} θt−1​的脉冲数n最小也应该等于1,所以在n为正整数的情况下，采用 θ t = ( n + 1 ) α θ_t = (n+1) α θt​=(n+1)α, θ t − 1 = n α θ_{t-1} = nα θt−1​=nα更符合实际情况。

第1个周期内计数器的值，即电机匀加速运动时的初始值，加速初始PWM频率。 根据公式（5）和（6）可求得： C 0 = Δ t 0 t t = 2 α ω ˊ ( n + 1 − n ) / t t \begin{aligned} C_0 &= {Δt_0\over{t_t}} \\ &= \sqrt{2α\over{ώ}}(\sqrt{n+1} - \sqrt{n})/t_t \end{aligned} C0​​=tt​Δt0​​=ωˊ2α​ ​(n+1 ​−n ​)/tt​​ 由上得出： C 0 = 2 α ω ˊ t t (7) \begin{aligned} \boxed{C_0 = {\sqrt{2α\over{ώ}}\over{t_t}}}\tag{7} \end{aligned} C0​=tt​ωˊ2α​ ​​​​(7) 用公式（7）求第1个周期内计数器的值

求第n个周期内计数器的值： C n = Δ t n t t = 2 α ω ˊ ( n + 1 − n ) / t t \begin{aligned} C_n &= {Δt_n\over{t_t}} \\ &= \sqrt{2α\over{ώ}}(\sqrt{n+1} - \sqrt{n})/t_t \end{aligned} Cn​​=tt​Δtn​​=ωˊ2α​ ​(n+1 ​−n ​)/tt​​ 由上得出： C n = Δ t n t t = 2 α ω ˊ ( n + 1 − n ) / t t = 2 α ω ˊ t t ( n + 1 − n ) \begin{aligned} Cn &= {Δt_n\over{t_t}}\\ &= \sqrt{2α\over{ώ}}(\sqrt{n+1} - \sqrt{n})/t_t\\ &={\sqrt{2α\over{ώ}}\over{t_t}}(\sqrt{n+1} - \sqrt{n}) \end{aligned} Cn​=tt​Δtn​​=ωˊ2α​ ​(n+1 ​−n ​)/tt​=tt​ωˊ2α​ ​​(n+1 ​−n ​)​ 其中根据公式（7） 2 α ω ˊ / t t = C 0 {\sqrt{2α\over{ώ}}/t_t} = C_0 ωˊ2α​ ​/tt​=C0​ 可得出： C n = C 0 ( n + 1 − n ) (8) \begin{aligned} \boxed{C_n =C_0({\sqrt{n+1}-\sqrt{n}})}\tag{8} \end{aligned} Cn​=C0​(n+1 ​−n ​)​​(8) 由于计算的过程中需要进行开方运算，微控制器的计算能力有限，因此在此使用泰勒公式进行泰勒级数逐级逼近的方法。在这里主要是用的是泰勒公式的特例——麦克劳林公式；具体如下： 1 ± 1 n = 1 ± 1 2 n − 1 8 n 2 + o 1 n 3 \sqrt{1±{1\over{n}}} = 1 ± {1\over{2n}} - {1\over{8n^2}}+o{1\over{n^3}} 1±n1​ ​=1±2n1​−8n21​+on31​ 为构造与麦克劳林相同的公式将 n-1 ，并且与公式 (9) 做比值处理，并进行化简计算，具体如下图 所示：

公式推导一共分为以下 5 个步骤推导： A. 将 C n C_n Cn​ 与 C n − 1 C_{n-1} Cn−1​ 利用公式8求比值； B. 分子分母提出 C 0 C_0 C0​ 和$ \sqrt{n}$, 并将其约掉； C. 整理化简根号下的内容； D. 将麦克劳林公式带入； E. 忽略无穷下余项，化简求得； 将其化简为关于 C n C_n Cn​ 的式子如下： c n c n − 1 = 4 n − 1 4 n + 1 C n = c n − 1 ( 4 n + 1 − 2 ) 4 n + 1 \begin{aligned} {c_n\over{c_{n-1}}} &= {{4n-1}\over{4n+1}}\\ C_n &= {c_{n-1} (4n+1-2)\over{4n+1}}\\ \end{aligned} cn−1​cn​​Cn​​=4n+14n−1​=4n+1cn−1​(4n+1−2)​​ 化简后得出： C n = C n − 1 − 2 ∗ C n − 1 4 n + 1 (9) \begin{aligned} \boxed{C_n = C_{n-1} - 2*{C_{n-1}\over{4n+1}} } \tag{9} \end{aligned} Cn​=Cn−1​−2∗4n+1Cn−1​​​​(9) 通过公式（7）和公式（9），即可推到出每个周期内的计时器的值，及每步的频率。经过化简的公式（9）有一定的误差，其得出的结果比公式（8）快。所以我们对公式（7）乘以0.676加以校正。 C 0 = 0.676 ∗ 2 α ω ˊ t t (10) \begin{aligned} \boxed{C_0= 0.676 *{\sqrt{2α\over{ώ}}\over{t_t}}} \tag{10} \end{aligned} C0​=0.676∗tt​ωˊ2α​ ​​​​(10) 通过公式（9）和公式（10）我们可以求出：

知道寄存器ARR的值后，我们就可以控制电机加速了，但是我们不知道电机应该加速到什么时候，即电机加速多少步以后开始匀速运行，多少步以后开始减速运行，接下来我们继续推导以上变量。

在电机由速度零匀加速或匀减速到速度零的运动情况下，由 θ = 1 / 2 ω ˊ t n 2 θ=1/2 ώt_n^2 θ=1/2ωˊtn2​ 和 θ = n α θ=nα θ=nα可推出： n α = 1 2 ω ˊ t n 2 nα = {1\over2} ώt_n^2 nα=21​ωˊtn2​ 由速度 ω n = ω ˊ t n ω_n = ώt_n ωn​=ωˊtn​带入上式可得： n α = 1 2 ω ˊ ( ω n ω ˊ ) 2 nα = {1\over2} ώ({ω_n \over{ώ}})^2 nα=21​ωˊ(ωˊωn​​)2 转化一下后： n ω ˊ = ω n 2 2 α (11) \begin{aligned} \boxed{n ώ = {ω_n^2\over{2α}} } \tag{11} \end{aligned} nωˊ=2αωn2​​​​(11) 公式（11）中，ω_n为最大速度，α为步距角，由此看出在最大速度和步距角不变的情况下，脉冲与加速度成反比。

由公式（11）可得: n 1 ω ˊ 1 = n 2 ω ˊ 2 (12) \begin{aligned} \boxed{n_1 ώ_1 = n_2 ώ_2} \tag{12} \end{aligned} n1​ωˊ1​=n2​ωˊ2​​​(12) 两边同时加 n 1 ω ˊ 2 n_1ώ_2 n1​ωˊ2​得： n 1 ω ˊ 1 + n 1 ω ˊ 2 = n 2 ω ˊ 2 + n 1 ω ˊ 2 n 1 （ ω ˊ 1 + ω ˊ 2 ） = ω ˊ 2 （ n 2 + n 1 ） \begin{aligned} n_1 ώ_1 + n_1 ώ_2 &= n_2 ώ_2 + n_1 ώ_2\\ n_1（ώ_1 + ώ_2）&= ώ_2 （n_2 + n_1 ） \end{aligned} n1​ωˊ1​+n1​ωˊ2​n1​（ωˊ1​+ωˊ2​）​=n2​ωˊ2​+n1​ωˊ2​=ωˊ2​（n2​+n1​）​ 化简得出： n 1 = ω ˊ 2 n 1 + n 2 ω ˊ 1 + ω ˊ 2 (13) \begin{aligned} \boxed{{n_1 = ώ_2 {n_1 + n_2\over{ώ_1 + ώ_2}} } }\tag{13} \end{aligned} n1​=ωˊ2​ωˊ1​+ωˊ2​n1​+n2​​​​(13)

对于步进电机加减速阶段来说，由于已经设定好了步进电机加速度、减速度、最大速度和总运行步数，所以说一共分为2种情况：

由 S = ν t 2 – ν 0 2 2 a S = {ν_t^2 –ν_0^2\over{2a}} S=2aνt2​–ν02​​得到加速阶段的步数: a c c e l _ s t e p = s p e e d 2 2 ω ˊ 1 ∗ α (14) \begin{aligned} \boxed{accel\_step={speed^2\over{2ώ_1 * α}}}\tag{14} \end{aligned} accel_step=2ωˊ1​∗αspeed2​​​(14)

A. 由公式（13）得到: a c c e l _ l i m = n 1 = ω ˊ 2 ∗ t o t a l _ s t e p ω ˊ 1 + ω ˊ 2 (15) \begin{aligned} \boxed{accel\_lim = n_1 = ώ_2*{ total\_step\over{ώ_1+ ώ_2}}}\tag{15} \end{aligned} accel_lim=n1​=ωˊ2​∗ωˊ1​+ωˊ2​total_step​​​(15) B. 由公式（12）得出 d e c e l s t e p = − a c c e l _ s t e p ∗ ω ˊ 1 ω ˊ 2 (16) \boxed{decel_step = - accel\_step*{ώ_1\over{ώ_2}}}\tag{16} decels​tep=−accel_step∗ωˊ2​ωˊ1​​​(16) 负号是因为减速过程，带入公式（9）时用。带入公式（9）时， n 0 n_0 n0​应为decel_step，其后每一步加1，直到停止。

加速阶段的步数由公式（15）计算，减速步数如下: d e c e l s t e p = − ( t o t a l s t e p − a c c e l l i m ) (17) \boxed{decel_step = - ( total_step - accel_lim)}\tag{17} decels​tep=−(totals​tep−accell​im)​(17) 负号是因为减速过程，带入公式（9）时用

通过以上推导，我们求出了梯形算法要求的所有变量。

由于算法在计算过程中涉及到一些浮点型运算，大量的浮点型运算会使得效率大大降低，为了使得计算速度得到更好的优化，我们做如下优化：

在给定步距角α、定时器频率 f t f_t ft​、加速度 ω ˊ 1 ώ_1 ωˊ1​、减速度 ω ˊ 2 ώ_2 ωˊ2​和最大速度speed以及总步数total_step的情况下，电机的加减速计算步骤如下：

加、减速度扩大100倍， T 1 _ F Q E Q = 0.676 ∗ f t ∗ 1 0 − 2 A _ S Q = 2 α ∗ 1 0 10 \begin{aligned} T1\_FQEQ &=0.676 *f_t*10^{-2}\\ A\_SQ &= 2α*10^{10} \end{aligned} T1_FQEQA_SQ​=0.676∗ft​∗10−2=2α∗1010​

下列公式分别计算： a c c e l _ s t e p = s p e e d 2 200 ω ˊ 1 α a c c e l _ l i m = ω ˊ 2 ∗ t o t a l _ s t e p ω ˊ 1 + ω ˊ 2 \begin{aligned} accel\_step &= {speed^2\over{200ώ_1 α}}\\ accel\_lim &= ώ_2*{ total\_step\over{ώ_1+ ώ_2}}\\ \end{aligned} accel_stepaccel_lim​=200ωˊ1​αspeed2​=ωˊ2​∗ωˊ1​+ωˊ2​total_step​​ 如果计算出的accel_step < accel_lim，则 d e c e l _ s t e p = − a c c e l _ s t e p ∗ ω ˊ 1 ω ˊ 2 \begin{aligned} decel\_step &= - accel\_step*{ώ_1\over{ώ_2}} \end{aligned} decel_step​=−accel_step∗ωˊ2​ωˊ1​​​

否则 d e c e l _ s t e p = − ( t o t a l _ s t e p − a c c e l _ l i m ) \begin{aligned} decel\_step &= - ( total\_step - accel\_lim) \end{aligned} decel_step​=−(total_step−accel_lim)​

由公式（20） C 0 = T 1 _ F Q E Q A _ S Q ω ˊ / 100 \begin{aligned} C_0 = T1\_FQEQ\sqrt{A\_SQ\over{ώ}}/100 \end{aligned} C0​=T1_FQEQωˊA_SQ​ ​/100​ 求出电机加速阶段的第一步的计时器的值。

C n = C n − 1 − 2 C n − 1 + R e s t 4 n + 1 N e w _ R e s t = ( 2 C n − 1 + R e s t ) ( m o d ( 4 ∗ n + 1 ) ) \begin{aligned} C_n &= C_{n-1}-{2C_{n-1} + Rest\over{4n+1}}\\ New\_Rest &=(2C_{n-1}+ Rest)(mod(4* n +1)) \end{aligned} Cn​New_Rest​=Cn−1​−4n+12Cn−1​+Rest​=(2Cn−1​+Rest)(mod(4∗n+1))​ 求出每步的计时器的计数值

已加速阶段的最大速度匀速运行

C n = C n − 1 − 2 C n − 1 + R e s t 4 n + 1 N e w R e s t = ( 2 C n − 1 + R e s t ) ( m o d ( 4 ∗ n + 1 ) ) \begin{aligned} C_n &= C_{n-1}-{2C_{n-1} + Rest\over{4n+1}}\\ New_Rest &=(2C_{n-1}+ Rest)(mod(4* n +1)) \end{aligned} Cn​NewR​est​=Cn−1​−4n+12Cn−1​+Rest​=(2Cn−1​+Rest)(mod(4∗n+1))​ 求出每步的计时器的计数值，其中n的值decal_step，其值为负值。其后每一步计算Cn, decal_step加1，直到为0。