阅读说明：本技术 一种伺服电机控制方法及系统 (A kind of control method of servo motor and system ) 是由 陈志丹 汪江剑 王锐 于 2019-08-21 设计创作，主要内容包括：本发明公开了一种伺服电机控制方法,至少包括以下步骤：根据预设速度计算最短定时周期；根据步距角计算定时器的初始值；计算伺服电机达到预设速度所需步数,记为加速步数；根据加速度和减速度计算减速步数；设置伺服电机的四种状态,分别为：STOP、ACCEL、RUN和DECEL；根据伺服电机的所述四种状态以及所述加速步数和减速步数,控制伺服电机的启动和停止速度。本发明提供的一种伺服电机控制方法,能够有效使伺服电机启动和停止时平缓运行,提高伺服电机工作的稳定性。(The invention discloses a kind of control method of servo motor, at least include the following steps: calculating the most short timer period according to pre-set velocity；The initial value of timer is calculated according to step angle；It calculates servo motor and reaches step number needed for pre-set velocity, be denoted as acceleration step number；According to acceleration and deceleration calculation deceleration step number；Four kinds of states of servo motor are set, STOP, ACCEL, RUN and DECEL are respectively as follows:；According to four kinds of states of servo motor and the acceleration step number and deceleration step number, control servo motor starts and stops speed.A kind of control method of servo motor provided by the invention can effectively be such that servo motor gently runs when starting and stopping, and improve the stability of servo motor work.)

一种伺服电机控制方法及系统

技术领域

本发明涉及电机技术领域，尤其涉及一种伺服电机控制方法及系统。

背景技术

芯片从硅晶圆到成品的制造过程需要极其苛刻的工艺，工艺过程需要技术一流的设备、极端的真空状态和高度自动化。在工艺过程中会用到大量的高精度自动化设备，这些设备的精度除了依赖于功能强大的处理器或者控制器外，执行机构的精度也是非常重要的一个环节。芯片制作过程的末端环节是芯片的检测，检测平台的自动化水平也在一定程度反映了芯片制造工艺的发展水平。芯片检测平台的主要作用是检测出良品芯片和不良品芯片，是芯片生产入库的最后一道工序。芯片检测平台包含搬料机构、上下料机构、不良料和良品料存放机构等机构，这些机构的执行由伺服电机控制实现。检测平台伺服电机的控制精度和速度平滑性对于芯片检测的效率尤为重要，同时电机在高速运动过程中频繁启停，方向改变和频率发生突变时不发生丢步，堵转和过冲的现象也成为关键。伺服电机有转矩、速度和位置三种控制方式，对于芯片检测平台中的各个机构来说，需要用到定位精确的位置控制方式。

伺服电机的位置控制模式一般是通过外部输入的脉冲的频率来确定转动速度的大小，通过脉冲的个数来确定电机转动的角度，也有些伺服可以通过通讯方式直接对速度和位移进行赋值通过控制器算法计算之后产生特定的脉冲信号给伺服电机，电机旋转一定的角度或者圈数，这个位置量经过传动机构变成了对应的位移。

无论是通过外部输入特定频率大小和特定个数的脉冲，还是直接对速度和位移进行赋值，两者在电机从启动到给定速度的加速过程和减速到停止的减速过程时间都非常短暂，脉冲信号的频率一个是从零到最大，一个是从最大到零，频率突变非常厉害，伺服电机在这两个过程中可能发生丢步，堵转和过冲的现象，最终影响整个系统的平稳性。

发明内容

本发明实施例的目的是提供一种伺服电机控制方法，能够有效使伺服电机启动和停止时平缓运行，提高伺服电机工作的稳定性。

为实现上述目的，一方面，本发明的一个实施例提供了一种伺服电机控制方法，至少包括以下步骤：

根据预设速度计算最短定时周期；

根据步距角计算定时器的初始值；

计算伺服电机达到预设速度所需步数，记为加速步数；

根据加速度和减速度计算减速步数；

设置伺服电机的四种状态，分别为：STOP、ACCEL、RUN和DECEL；

根据伺服电机的所述四种状态以及所述加速步数和减速步数，控制伺服电机的启动和停止速度。

进一步地，所述根据预设速度计算最短定时周期，具体为：

设置伺服电机的步长step，加速度accel，减速度decel和预设速度speed，根据步距角计算得到最短定时周期min_delay＝(ALPHA/tt)/w，其中，ALPHA为步距角，tt为定时器周期，w为预设速度。

进一步地，所述根据步距角计算定时器的初始值，具体为：

根据S＝at²/2，得到伺服电机的修正量为其中S是电机走1步的距离，S＝ALPHA，a是加速度accel，t是时间；

定时器的初始值为：

srd.step_delay＝(T1_FREQ_148*sqrt(A_SQ/accel))/100)；其中，所述T1_FREQ_148＝(0.676*T1_FREQ)/100，A_SQ＝ALPHA*2*10000000000，T1_FREQ为定时器频率。

进一步地，所述计算伺服电机达到预设速度所需步数，记为加速步数，具体为：

V_t ²-V₀ ²＝2as，若V₀＝0，则

设置加速时间为减速时间为

若V_on＝V_off＝0，V_t＝ω_n，V₀＝0，a＝ω；

则

则n₁+n₂＝step；

得到所述加速步数为

进一步地，所述根据加速度和减速度计算减速步数，具体为：

根据可知，n₁和n₂分别为加速步数和减速步数；

由n₁*accel＝n₂*decel，得到max_s_lim*accel＝decel_val*decel，即若max_s_lim<accel_lim则减速步数为

若max_s_lim>accel_lim则减速步数为decel_val＝-(step-accel_lim)。

进一步地，根据伺服电机的所述四种状态以及所述加速步数和减速步数，控制伺服电机的启动和停止速度，具体为：

当状态机开始启动时，开启状态机的状态为ACCEL；

根据所述加速步数，将伺服电机的速度提升到所述预设速度；

更改状态机的状态为RUN，保持匀速运行；

根据所述减速步数，将状态机的状态更改为DECEL，直至伺服电机的速度降为0；

更改状态机的状态为STOP。

另一方面，本发明的另一个实施例提供了一种伺服电机控制系统，包括第一计算模块、第二计算模块、第三计算模块、第四计算模块、设置模块和控制模块；

所述第一计算模块，用于根据预设速度计算最短定时周期；

所述第二计算模块，用于根据步距角计算定时器的初始值；

所述第三计算模块，用于计算伺服电机达到预设速度所需步数，记为加速步数；

所述第四计算模块，用于根据加速度和减速度计算减速步数；

所述设置模块，用于设置伺服电机的四种状态，分别为：STOP、ACCEL、RUN和DECEL；

所述控制模块，用于根据伺服电机的所述四种状态以及所述加速步数和减速步数，控制伺服电机的启动和停止速度。

本发明实施例的目的是提供一种伺服电机控制方法，能够有效使伺服电机启动和停止时平缓运行，提高伺服电机工作的稳定性。

附图说明

图1是本发明提供的一种伺服电机控制方法的流程示意图；

图2是本发明提供的一种伺服电机控制方法的硬件框图；

图3是本发明提供的一种伺服电机控制方法的速度控制示意图；

图4是本发明提供的一种伺服电机控制方法的速度曲线示意图；

图5是本发明提供的一种伺服电机控制方法的无匀速运动时加减速速度曲线示意图；

图6是本发明提供的一种伺服电机控制方法的有匀速运动时加减速速度曲线示意图；

图7是本发明提供的一种伺服电机控制方法的速度运行状态示意图；

图8是本发明提供的一种伺服电机控制方法的状态机运行机制示意图；

图9是本发明提供的一种伺服电机控制方法的计算时间延迟中的变量示意图；

图10是本发明提供的一种伺服电机控制系统的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的第一实施例：

请参阅图1-9。

本发明实施例提供了一种伺服电机控制方法，至少包括以下步骤：

S1、根据预设速度计算最短定时周期；

S2、根据步距角计算定时器的初始值；

S3、计算伺服电机达到预设速度所需步数，记为加速步数；

S4、根据加速度和减速度计算减速步数；

S5、设置伺服电机的四种状态，分别为：STOP、ACCEL、RUN和DECEL；

S6、根据伺服电机的四种状态以及加速步数和减速步数，控制伺服电机的启动和停止速度。

在本发明实施例中，请参阅图2，为本发明提供的硬件框图，包括MCU最小系统、供电模块、下载与仿真调试模块、串口调试模块，伺服电机驱动模块组成。微处理器UM006801是一款超低功耗处理器芯片，具有高精度的ADC,UART,SPI等外设，内部高速时钟20MHz的工作频率。伺服电机控制单元包括伺服电机控制器和伺服电机，伺服电机控制器选用台达(AELTA)ASD-B2-0421-B，电机选用台达ECMA-C20604。电机额定转速为3000r/min，最高转速为5000r/min，电机转动一圈需要160000个脉冲。本发明通过调用函数Move()设置速度控制器。函数Move()首先计算所需的所有参数并将它们存储在速度数据结构体中，然后启用计时器中断。计时器根据所需的速度斜坡生成中断，并在每个中断上调用函数Step_Counter()来移动伺服电机。伺服电机的状态STOP为停止状态，ACCEL为加速状态，RUN为运行状态，DECEL为减速状态。本发明实施例通过定步长(step)、加速度(accel)、减速度(decel)、预设速度(speed)四个参数来描述速度曲线，通过计算伺服电机运行过程中的加速步数和减速步数，根据加速步数和减速步数和状态机的状态更改，对伺服电机在运行过程中进行T型加减速，能够有效地避免速度突变的情况发生，从而能够提高伺服电机运行时的平稳性和启停过程的平稳性，减少运行噪声和提高传动机构的定位精度。

作为本发明实施例的一种具体实施方式，根据预设速度计算最短定时周期，具体为：

设置伺服电机的步长step，加速度accel，减速度decel和预设速度speed，根据步距角计算得到最短定时周期min_delay＝(ALPHA/tt)/w，其中，ALPHA为步距角，tt为定时器周期，w为预设速度。

作为本发明实施例的一种具体实施方式，根据步距角计算定时器的初始值，具体为：

根据S＝at²/2，得到伺服电机的修正量为其中S是电机走1步的距离，S＝ALPHA，a是加速度accel，t是时间；

定时器的初始值为：

srd.step_delay＝(T1_FREQ_148*sqrt(A_SQ/accel))/100)；其中，T1_FREQ_148＝(0.676*T1_FREQ)/100，A_SQ＝ALPHA*2*10000000000，T1_FREQ为定时器频率。

作为本发明实施例的一种具体实施方式，计算伺服电机达到预设速度所需步数，记为加速步数，具体为：

V_t ²-V₀ ²＝2as，若V₀＝0，则

设置加速时间为减速时间为

若V_on＝V_off＝0，V_t＝ω_n，V₀＝0，a＝ω；

则

则n₁+n₂＝step；

得到加速步数为

作为本发明实施例的一种具体实施方式，根据加速度和减速度计算减速步数，具体为：

根据可知，n₁和n₂分别为加速步数和减速步数；

由n₁*accel＝n₂*decel，得到max_s_lim*accel＝decel_val*decel，即若max_s_lim<accel_lim则减速步数为

若max_s_lim>accel_lim则减速步数为decel_val＝-(step-accel_lim)。

作为本发明实施例的一种具体实施方式，根据伺服电机的四种状态以及加速步数和减速步数，控制伺服电机的启动和停止速度，具体为：

当状态机开始启动时，开启状态机的状态为ACCEL；

根据加速步数，将伺服电机的速度提升到预设速度；

更改状态机的状态为RUN，保持匀速运行；

根据减速步数，将状态机的状态更改为DECEL，直至伺服电机的速度降为0；

更改状态机的状态为STOP。

在本发明实施例中，可以理解的是，在伺服电机开始移动的时候，若移动多个步数，状态机的状态更改为ACCEL；若只移动1步，则将状态机的状态更改为DECEL。当状态机的状态更改为ACCEL时，应用程序根据加速步数加速伺服电机，直到达到预设速度并将状态机的状态更改为RUN；或必须根据减速步数开始减速时，将状态更改为DECEL。当状态机的状态设置为RUN时，伺服电机保持匀速运行，直到必须根据减速步数开始减速，将状态机的状态更改为DECEL。在减速步数之内保持状态机在DECEL状态进行减速，然后将状态机的状态更改为STOP。可以理解的是，对于加减速过程中的每一步，必须计算一个新的时延。为了提高精度，计算过程中的余数被保留在下一次计算中，计算公式如下：

new_rest＝(2*step_delay+rest)(mod(4*accel_count+1))，其中，mod为求余运算。

请参阅图9，为计算时间延时中的变量示意图。其中，tep_count：计数步数，ACCEL启动时从0开始，DECEL完成后的计数值，与传递的命令参数步长值相同。

accel_count：用于控制加减速。在ACCEL中，accel_coun从0开始，每一步递增，直到ACCEL结束。当DECEL启动时，它被设置为decel_val，这是负的，并且每一步都增加。当它达到0时，移动结束，状态设置为停止。decel_start：开始减速。当step_count＝decel_start时，状态设置为DECEL。

实施本发明实施例，具有如下有益效果：

在本发明实施例中，可以理解的是，本发明通过调用函数Move()设置速度控制器。函数Move()首先计算所需的所有参数并将它们存储在速度数据结构体中，然后启用计时器中断。计时器根据所需的速度斜坡生成中断，并在每个中断上调用函数Step_Counter()来移动伺服电机。伺服电机的状态STOP为停止状态，ACCEL为加速状态，RUN为运行状态，DECEL为减速状态。本发明实施例通过定步长(step)、加速度(accel)、减速度(decel)、预设速度(speed)四个参数来描述速度曲线，通过计算伺服电机运行过程中的加速步数和减速步数，根据加速步数和减速步数和状态机的状态更改，对伺服电机在运行过程中进行T型加减速，能够有效地避免速度突变的情况发生，从而能够提高伺服电机运行时的平稳性和启停过程的平稳性，减少运行噪声和提高传动机构的定位精度。

本发明提供的第二实施例：

请参阅图10。

本发明实施例提供了一种伺服电机控制系统，包括第一计算模块101、第二计算模块102、第三计算模块103、第四计算模块104、设置模块105和控制模块106；

第一计算模块101，用于根据预设速度计算最短定时周期；

第二计算模块102，用于根据步距角计算定时器的初始值；

第三计算模块103，用于计算伺服电机达到预设速度所需步数，记为加速步数；

第四计算模块104，用于根据加速度和减速度计算减速步数；

设置模块105，用于设置伺服电机的四种状态，分别为：STOP、ACCEL、RUN和DECEL；

控制模块106，用于根据伺服电机的四种状态以及加速步数和减速步数，控制伺服电机的启动和停止速度。

在本发明实施例中，可以理解的是，在伺服电机开始移动的时候，若移动多个步数，状态机的状态更改为ACCEL；若只移动1步，则将状态机的状态更改为DECEL。当状态机的状态更改为ACCEL时，应用程序根据加速步数加速伺服电机，直到达到预设速度并将状态机的状态更改为RUN；或必须根据减速步数开始减速时，将状态更改为DECEL。当状态机的状态设置为RUN时，伺服电机保持匀速运行，直到必须根据减速步数开始减速，将状态机的状态更改为DECEL。在减速步数之内保持状态机在DECEL状态进行减速，然后将状态机的状态更改为STOP。可以理解的是，对于加减速过程中的每一步，必须计算一个新的时延。为了提高精度，计算过程中的余数被保留在下一次计算中，计算公式如下：

new_rest＝(2*step_delay+rest)(mod(4*accel_count+1))，其中，mod为求余运算。

请参阅图9，为计算时间延时中的变量图。其中，tep_count：计数步数，ACCEL启动时从0开始，DECEL完成后的计数值，与传递的命令参数步长值相同。

accel_count：用于控制加减速。在ACCEL中，accel_coun从0开始，每一步递增，直到ACCEL结束。当DECEL启动时，它被设置为decel_val，这是负的，并且每一步都增加。当它达到0时，移动结束，状态设置为停止。

decel_start：开始减速。当step_count＝decel_start时，状态设置为DECEL。

实施本发明实施例，具有如下有益效果：

在本发明实施例中，可以理解的是，本发明通过调用函数Move()设置速度控制器。函数Move()首先计算所需的所有参数并将它们存储在速度数据结构体中，然后启用计时器中断。计时器根据所需的速度斜坡生成中断，并在每个中断上调用函数Step_Counter()来移动伺服电机。伺服电机的状态STOP为停止状态，ACCEL为加速状态，RUN为运行状态，DECEL为减速状态。本发明实施例通过定步长(step)、加速度(accel)、减速度(decel)、预设速度(speed)四个参数来描述速度曲线，通过计算伺服电机运行过程中的加速步数和减速步数，根据加速步数和减速步数和状态机的状态更改，对伺服电机在运行过程中进行T型加减速，能够有效地避免速度突变的情况发生，从而能够提高伺服电机运行时的平稳性和启停过程的平稳性，减少运行噪声和提高传动机构的定位精度。

以上是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。