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

关于梯形加速度，我打算写2篇文章来描述整个实现流程。本章我们从原理的角度详细分析一下，梯形加速度是怎么实现的，下一篇文章我们一步步看看代码如何实现。
我们先来看看最终效果。

步进电机梯形加速度

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

一、 使用加减速的理由

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

二、 梯形加减速算法特点

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

三、 算法基础

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

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

• $t_0$ 表示第1个脉冲发送时刻
• $t_1$ 表示第2个脉冲发送时刻
• $t_2$ 表示第3个脉冲发送时刻
• $t_t$ 表示定时器的计数周期
• $C_0$ 表示定时器从 $t_0$ ~ $t_1$ 时刻的定时器计数值
• $C_1$ 表示定时器从 $t_1$ ~ $t_2$ 时刻的定时器计数值
• $\Delta t_1$ 和$\Delta t_2$表示两个脉冲之间的间隔时间

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

四、 梯形算法要求的变量

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

1. 在加速阶段：

• 加速初始PWM频率，即我们要求的ARR寄存器的值$C_0$
• 加速度accel
• 加速阶段总的脉冲数

2. 匀速阶段

• 匀速所用的脉冲数

3. 减速阶段

• 减速初始频率（也是匀速运动的速度）
• 减速度decel
• 减速所用的脉冲
  一般情况下，电机加速度accel、减速度decel和总步数（即总脉冲数）是给定的，我们需要通过这3个变量推导出剩余的变量。知道以上变量，再推导出每步的定时器频率，结合脉冲数即可绘制完整的梯形图。

五、 加减速算法推导过程

1. 电机转过的角度θ的计算：

电机转过的角度θ等于脉冲数乘以步距角

$$\boxed{\theta =n\ast \alpha }\text{\hspace{1em}(1)}$$

其中：n表示驱动电机的脉冲数，α表示电机的步距角

2. 电机由速度0匀加速运动，转过θ所用的时间$t_n$:

由匀加速直线运动公式：

$$\boxed{S={\nu }_{0}t+\frac{1}{2}a{t}^2}\text{\hspace{1em}(2)}$$

可推导出,电机转过的角度θ：

$$\boxed{\theta ={\omega }_{0}t+\frac{1}{2}\acute{\omega }{t}_n^2}\text{\hspace{1em}(3)}$$

其中：θ为转过的角度 ， ${\omega }_0$为初角速度，ώ为加角速度，$t_n$为所用的时间

当初角速度${\omega }_0=0$时，电机转过θ所用的时间$t_n$：

$$\boxed{t_n=\sqrt{\frac{2\theta }{\acute{\omega }}}}\text{\hspace{1em}(4)}$$

3.求相邻脉冲时间间隔Δt：

时间间隔Δt：

$$\boxed{\Delta t=C_n\ast t_t}\text{\hspace{1em}(5)}$$

其中：$C_n$表示定时器计数器相邻两脉冲间的计数值，$t_t$表示定时器的时钟周期，定时器频率的倒数
假设相邻两脉冲间转过的角度分别用${\theta }_t$和${\theta }_{t-1}$表示，所用时间用$t_n$和 $t_{n-1}$表示，则Δt根据公式（4）也可以表示为

$$\begin{gathered} \boxed{\Delta t=t_n–t_{n-1} \\ =\sqrt{\frac{2{\theta }_t}{\acute{\omega }}}-\sqrt{\frac{2{\theta }_{t-1}}{\acute{\omega }}}} \end{gathered}$$

上式中n表示转过${\theta }_{t-1}$角度时，所用的脉冲数
$$\begin{array}{rl}\Delta t& =t_n–t_{n-1}\\ & =\sqrt{\frac{2{\theta }_t}{\acute{\omega }}}-\sqrt{\frac{2{\theta }_{t-1}}{\acute{\omega }}}\end{array}$$
上式中${\theta }_t$和${\theta }_{t-1}$根据公式(1)可得出：${\theta }_t=(n+1)\alpha ,{\theta }_t-1=n\alpha$,则带入最终可得到公式（6）

$$\boxed{\Delta t=\sqrt{\frac{2\alpha }{\acute{\omega }}}(\sqrt{n+1}-\sqrt{n})}\text{\hspace{1em}(6)}$$

上式中n表示转过${\theta }_{t-1}$角度时，所用的脉冲数
此处计算时，让${\theta }_t=(n+1)\alpha ,{\theta }_{t-1}=n\alpha ，$而不是${\theta }_t=n\alpha$, ${\theta }_{t-1}=（n-1）\alpha$，是因为Δt是两脉冲之间的时间间隔，${\theta }_{t-1}$的脉冲数n最小也应该等于1,所以在n为正整数的情况下，采用${\theta }_t=(n+1)\alpha$, ${\theta }_{t-1}=n\alpha$更符合实际情况。

4、 求第1个周期内计数器的值

第1个周期内计数器的值，即电机匀加速运动时的初始值，加速初始PWM频率。
根据公式（5）和（6）可求得：
$$\begin{array}{rl}{C}_0& =\frac{\Delta t_0}{t_t}\\ & =\sqrt{\frac{2\alpha }{\acute{\omega }}}(\sqrt{n+1}-\sqrt{n})/t_t\end{array}$$
由上得出：

$$\begin{array}{r}\boxed{C_0=\frac{\sqrt{\frac{2\alpha }{\acute{\omega }}}}{t_t}}\end{array}\text{\hspace{1em}(7)}$$

用公式（7）求第1个周期内计数器的值

求第n个周期内计数器的值：
$$\begin{array}{rl}{C}_n& =\frac{\Delta t_n}{t_t}\\ & =\sqrt{\frac{2\alpha }{\acute{\omega }}}(\sqrt{n+1}-\sqrt{n})/t_t\end{array}$$
由上得出：
$$\begin{array}{rl}Cn& =\frac{\Delta t_n}{t_t}\\ & =\sqrt{\frac{2\alpha }{\acute{\omega }}}(\sqrt{n+1}-\sqrt{n})/t_t\\ & =\frac{\sqrt{\frac{2\alpha }{\acute{\omega }}}}{t_t}(\sqrt{n+1}-\sqrt{n})\end{array}$$
其中根据公式（7） $\sqrt{\frac{2\alpha }{\acute{\omega }}}/t_t=C_0$ 可得出：

$$\begin{array}{r}\boxed{C_n=C_0(\sqrt{n+1}-\sqrt{n})}\end{array}\text{\hspace{1em}(8)}$$

由于计算的过程中需要进行开方运算，微控制器的计算能力有限，因此在此使用泰勒公式进行泰勒级数逐级逼近的方法。在这里主要是用的是泰勒公式的特例——麦克劳林公式；具体如下：
$$\sqrt{1\pm \frac{1}{n}}=1\pm \frac{1}{2n}-\frac{1}{8n^2}+o\frac{1}{n^3}$$
为构造与麦克劳林相同的公式将 n-1 ，并且与公式 (9) 做比值处理，并进行化简计算，具体如下图 所示：

公式推导一共分为以下 5 个步骤推导：
A. 将 $C_n$ 与 $C_{n-1}$ 利用公式8求比值；
B. 分子分母提出 $C_0$ 和$ \sqrt{n}$, 并将其约掉；
C. 整理化简根号下的内容；
D. 将麦克劳林公式带入；
E. 忽略无穷下余项，化简求得； 将其化简为关于 $C_n$ 的式子如下：
$$\begin{array}{rl}\frac{c_n}{c_{n-1}}& =\frac{4n-1}{4n+1}\\ C_n& =\frac{c_{n-1}(4n+1-2)}{4n+1}\end{array}$$
化简后得出：

$$\begin{array}{r}\boxed{C_n=C_{n-1}-2\ast \frac{C_{n-1}}{4n+1}}\end{array}\text{\hspace{1em}(9)}$$

通过公式（7）和公式（9），即可推到出每个周期内的计时器的值，及每步的频率。经过化简的公式（9）有一定的误差，其得出的结果比公式（8）快。所以我们对公式（7）乘以0.676加以校正。

$$\begin{array}{r}\boxed{C_0=0.676\ast \frac{\sqrt{\frac{2\alpha }{\acute{\omega }}}}{t_t}}\end{array}\text{\hspace{1em}(10)}$$
通过公式（9）和公式（10）我们可以求出：

• 加速初始PWM频率，即我们要求的ARR寄存器的值$C_0$
• 电机加速阶段每一步的PWM频率，即ARR寄存器的值C

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

5、 推导脉冲数和加速度成反比。

在电机由速度零匀加速或匀减速到速度零的运动情况下，由$\theta =1/2\acute{\omega }{t}_n^2$ 和$\theta =n\alpha$可推出：
$n\alpha =\frac{1}{2}\acute{\omega }{t}_n^2$
由速度${\omega }_n=\acute{\omega }{t}_n$带入上式可得：
$n\alpha =\frac{1}{2}\acute{\omega }(\frac{{\omega }_n}{\acute{\omega }}{)}^2$
转化一下后：

$$\begin{array}{r}\boxed{n\acute{\omega }=\frac{{\omega }_n^2}{2\alpha }}\end{array}\text{\hspace{1em}(11)}$$

公式（11）中，ω_n为最大速度，α为步距角，由此看出在最大速度和步距角不变的情况下，脉冲与加速度成反比。

6、在一个电机匀加速到一定速度，再匀减速的情况下，如下图：

由公式（11）可得:

$$\begin{array}{r}\boxed{n_1{\acute{\omega }}_1=n_2{\acute{\omega }}_2}\end{array}\text{\hspace{1em}(12)}$$

两边同时加$n_1{\acute{\omega }}_2$得：
$$\begin{array}{rl}{n}_1{\acute{\omega }}_1+n_1{\acute{\omega }}_2& =n_2{\acute{\omega }}_2+n_1{\acute{\omega }}_2\\ n_1（{\acute{\omega }}_1+{\acute{\omega }}_2）& ={\acute{\omega }}_2（n_2+n_1）\end{array}$$
化简得出：

$$\begin{array}{r}\boxed{n_1={\acute{\omega }}_2\frac{n_1+n_2}{{\acute{\omega }}_1+{\acute{\omega }}_2}}\end{array}\text{\hspace{1em}(13)}$$

7、求加减速步数

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

（1）电机能够加速到给定的最大速度，匀速运行一段时间后，再减速到0

由$S=\frac{{\nu }_t^2–{\nu }_0^2}{2a}$得到加速阶段的步数:

$$\begin{array}{r}\boxed{accel\_step=\frac{spee{d}^2}{2{\acute{\omega }}_1\ast \alpha }}\end{array}\text{\hspace{1em}(14)}$$

A. 由公式（13）得到:

$$\begin{array}{r}\boxed{accel\_lim=n_1={\acute{\omega }}_2\ast \frac{total\_step}{{\acute{\omega }}_1+{\acute{\omega }}_2}}\end{array}\text{\hspace{1em}(15)}$$

B. 由公式（12）得出

$$\boxed{dece{l}_{s}tep=-accel\_step\ast \frac{{\acute{\omega }}_1}{{\acute{\omega }}_2}}\text{\hspace{1em}(16)}$$

负号是因为减速过程，带入公式（9）时用。带入公式（9）时，$n_0$应为decel_step，其后每一步加1，直到停止。

（2）由于运动距离过短，电机无法加速到最大速度即开始减速：

加速阶段的步数由公式（15）计算，减速步数如下:

$$\boxed{dece{l}_{s}tep=-(tota{l}_{s}tep-acce{l}_{l}im)}\text{\hspace{1em}(17)}$$

负号是因为减速过程，带入公式（9）时用

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

六、 算法优化

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

1. 对于加减速的每一步来说，都需要重新计算下一步的时间，计算的过程中可能会出现除不尽的项式，为了更有利的加减速，可以采用加速向上取整，减速向下取整的原则来做运算，也可以采用余数累计的方法，在这里使用的是将余数累计的方法来提高间隔时间的精度和准确性。
   
   $$\begin{array}{r}\boxed{C_n=C_{n-1}-\frac{2C_{n-1}+Rest}{4n+1}}\end{array}\text{\hspace{1em}(18)}$$
   
   余数公式：
   
   $$\begin{array}{r}\boxed{New\_Rest=(2C_{n-1}+Rest)(mod(4\ast n+1))}\end{array}\text{\hspace{1em}(19)}$$
   
   上式中，mod就是取余数的意思，即C语言中的%。
2. 令:速度扩大100倍
   加速度扩大100倍
   扩大后的速度和加速度还用原来的字母表示
   则$C_0 = 0.676f_t\sqrt{2α/ώ} $ 加速度扩大100倍以后，变为:
   $$\begin{array}{rl}{C}_0& =0.676\ast f_t\ast \sqrt{(2\ast 10^2\ast \alpha )/\acute{\omega }}\\ & =0.676\ast f_t\ast 10^{-2}\ast \sqrt{\frac{2\alpha \ast 10^{10}}{\acute{\omega }}}\ast 10^{-2}\end{array}$$
   令：
   $$\begin{array}{rl}T1\_FQEQ& =0.676\ast f_t\ast 10^{-2}\\ A\_SQ& =2\alpha \ast 10^{10}\end{array}$$
   则：
   
   $$\begin{array}{r}\boxed{C_0=T1\_FQEQ\sqrt{\frac{A\_SQ}{\acute{\omega }}}/100}\end{array}\text{\hspace{1em}(20)}$$
   
   公式（14）中，速度和加速度扩大100后，变化为：
   
   $$\begin{array}{r}\boxed{Accel\_step=\frac{spee{d}^2}{200{\acute{\omega }}_1\alpha }}\end{array}\text{\hspace{1em}(21)}$$
   
   其他公式的加速度和速度相应放大100倍后，公式不发生变化

七、总结

在给定步距角α、定时器频率$f_t$、加速度${\acute{\omega }}_1$、减速度${\acute{\omega }}_2$和最大速度speed以及总步数total_step的情况下，电机的加减速计算步骤如下：

1、 令：速度扩大100倍

加、减速度扩大100倍，
$$\begin{array}{rl}T1\_FQEQ& =0.676\ast f_t\ast 10^{-2}\\ A\_SQ& =2\alpha \ast 10^{10}\end{array}$$

2、计算Accel_step、accel_lim和decal_step：

下列公式分别计算：
$$\begin{array}{rl}accel\_step& =\frac{spee{d}^2}{200{\acute{\omega }}_1\alpha }\\ accel\_lim& ={\acute{\omega }}_2\ast \frac{total\_step}{{\acute{\omega }}_1+{\acute{\omega }}_2}\end{array}$$
如果计算出的accel_step < accel_lim，则
$$\begin{array}{rl}decel\_step& =-accel\_step\ast \frac{{\acute{\omega }}_1}{{\acute{\omega }}_2}\end{array}$$

否则
$$\begin{array}{rl}decel\_step& =-(total\_step-accel\_lim)\end{array}$$

3、 求出加速阶段的第一步的计时器的值:

由公式（20）
$$\begin{array}{r}{C}_0=T1\_FQEQ\sqrt{\frac{A\_SQ}{\acute{\omega }}}/100\end{array}$$
求出电机加速阶段的第一步的计时器的值。

4、求计数器的值

(1)加速阶段：

$$\begin{array}{rl}{C}_n& =C_{n-1}-\frac{2C_{n-1}+Rest}{4n+1}\\ New\_Rest& =(2C_{n-1}+Rest)(mod(4\ast n+1))\end{array}$$
求出每步的计时器的计数值

(2) 匀速阶段：

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

(3)减速阶段：

$$\begin{array}{rl}{C}_n& =C_{n-1}-\frac{2C_{n-1}+Rest}{4n+1}\\ Ne{w}_{R}est& =(2C_{n-1}+Rest)(mod(4\ast n+1))\end{array}$$
求出每步的计时器的计数值，其中n的值decal_step，其值为负值。其后每一步计算Cn, decal_step加1，直到为0。