阅读说明：本技术 一种伺服电机控制方法及系统 (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.)

1. a kind of control method of servo motor, which is characterized in that at least include the following steps:

The most short timer period is calculated 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 starting and Stop speed.

2. control method of servo motor as described in claim 1, which is characterized in that described most short fixed according to pre-set velocity calculating When the period, specifically:

The step-length step of servo motor, acceleration accel, deceleration decel and pre-set velocity speed are set, according to step angle Most short timer period m in_delay=(ALPHA/tt)/w is calculated, wherein ALPHA is step angle, and tt is timer week Phase, w are pre-set velocity；ALPHA=(2*3.14159)/SPR, SPR are that servo motor turns around required step number.

3. control method of servo motor as described in claim 1, which is characterized in that described to calculate timer according to step angle Initial value, specifically:

According toThe correction amount for obtaining servo motor isWherein S is motor The distance of 1 step is walked, S=ALPHA, a are acceleration accel, and t is the time；

The initial value of timer are as follows:

Srd.step_delay=(T1_FREQ_148*sqrt (A_SQ/accel))/100)；Wherein, the T1_FREQ_148 =(0.676*T1_FREQ)/100, A_SQ=ALPHA*2*10000000000, T1_FREQ is timer frequency.

4. control method of servo motor as described in claim 1, which is characterized in that the calculating servo motor reaches default speed Step number needed for spending, is denoted as acceleration step number, specifically:

If V₀=0, then

Setting the acceleration time beDeceleration time is

If V_on=V_off=0,V_t=ω_n, V₀=0, a=ω；

Then

Thenn₁+n₂=step；

Obtaining the acceleration step number is

5. control method of servo motor as described in claim 1, which is characterized in that described according to acceleration and deceleration calculation Deceleration step number, specifically:

According toIt is found that n₁And n₂Respectively accelerate step number and deceleration step number；

By n₁* accel=n₂* decel, obtains max_s_lim*accel=decel_val*decel, and even max_s_lim < Accel_lim step number of then slowing down is

Step number of slowing down if max_s_lim > accel_lim is decel_val=- (step-accel_lim).

6. control method of servo motor as described in claim 1, which is characterized in that 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, specifically:

When state machine starts starting, the state of open state machine is ACCEL；

According to the acceleration step number, the speed of servo motor is promoted to the pre-set velocity；

The state for changing state machine is RUN, and remain a constant speed operation；

According to the deceleration step number, the state of state machine is changed to DECEL, until the speed of servo motor is reduced to 0；

The state for changing state machine is STOP.

7. a kind of servo control system, which is characterized in that calculated including the first computing module, the second computing module, third Module, the 4th computing module, setup module and control module；

First computing module, for calculating the most short timer period according to pre-set velocity；

Second computing module, for calculating the initial value of timer according to step angle；

The third computing module reaches step number needed for pre-set velocity for calculating servo motor, is denoted as acceleration step number；

4th computing module, for according to acceleration and deceleration calculation deceleration step number；

The setup module is respectively as follows: STOP, ACCEL, RUN and DECEL for four kinds of states of servo motor to be arranged；

The control module is watched according to four kinds of states of servo motor and the acceleration step number and deceleration step number, control Take motor starts and stops speed.

Technical field

The present invention relates to technical field of motors more particularly to a kind of control method of servo motor and system.

Background technique

Manufacturing process of the chip from Silicon Wafer to finished product needs extremely harsh technique, and technical process needs technology one stream Equipment, extreme vacuum state and increasingly automated.A large amount of high-precision automatic equipment can be used in technical process, these Other than dependent on powerful processor or controller, the precision of executing agency is also very important the precision of equipment One link.The end link of chip fabrication processes is the detection of chip, and the automatization level of detection platform is also to a certain degree Reflect the development level of chip manufacturing process.The main function of chip detection platform is to detect non-defective unit chip and defective products core Piece is last procedure of chip production storage.Chip detection platform include Ban Liao mechanism, charging and discharging mechanism, bad material and The mechanisms such as non-defective unit material holding structure, the execution of these mechanisms are realized by Serve Motor Control.The control of detection platform servo motor The efficiency that accuracy and speed flatness detects chip is particularly important, while motor frequent start-stop during high speed exercise, The phenomenon that not losing step, stall and overshoot when direction changes and frequency mutates also becomes crucial.Servo motor have torque, Speed and three kinds of position control mode need to use the position of registration for each mechanism in chip detection platform Set control mode.

The position control mode of servo motor determines velocity of rotation generally by the frequency of externally input pulse Size determines the angle of motor rotation by the number of pulse, and also some servos can be by communication modes directly to speed Generate after assignment is calculated by controller algorithm specific pulse signal to servo motor with displacement, motor rotation is certain Angle or circle number, this position quantity become corresponding displacement by transmission mechanism.

Either by the pulse of external input specific frequency size and certain number, or directly to speed and be displaced into Row assignment, the two are all very short from the accelerator for starting to given speed and the moderating process time for decelerating to stopping in motor Temporarily, the frequency of pulse signal one is from zero to maximum, and one is from being up to zero, and frequency discontinuity is very severe, servo motor The phenomenon that step, stall and overshoot may be lost during the two, the final stationarity for influencing whole system.

Summary of the invention

The purpose of the embodiment of the present invention is that provide a kind of control method of servo motor, can effectively make servo motor starting and It is gently run when stopping, improving the stability of servo motor work.

To achieve the above object, on the one hand, An embodiment provides a kind of control method of servo motor, until Less the following steps are included:

The most short timer period is calculated 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, opening for servo motor is controlled Dynamic and stopping speed.

It is further, described that the most short timer period is calculated according to pre-set velocity, specifically:

The step-length step of servo motor, acceleration accel, deceleration decel and pre-set velocity speed are set, according to step Most short timer period m in_delay=(ALPHA/tt)/w is calculated in elongation, wherein ALPHA is step angle, and tt is timer Period, w are pre-set velocity.

Further, the initial value that timer is calculated according to step angle, specifically:

According to S=at²/ 2,The correction amount for obtaining servo motor isWherein S is Motor walks the distance of 1 step, and S=ALPHA, a are acceleration accel, and t is the time；

The initial value of timer are as follows:

Srd.step_delay=(T1_FREQ_148*sqrt (A_SQ/accel))/100)；Wherein, the T1_FREQ_ 148=(0.676*T1_FREQ)/100, A_SQ=ALPHA*2*10000000000, T1_FREQ is timer frequency.

Further, the calculating servo motor reaches step number needed for pre-set velocity, is denoted as acceleration step number, specifically:

V_t ²-V₀ ²=2as, if V₀=0, then

Setting the acceleration time beDeceleration time is

If V_on=V_off=0,V_t=ω_n, V₀=0, a=ω；

Then

Thenn₁+n₂=step；

Obtaining the acceleration step number is

It is further, described according to acceleration and deceleration calculation deceleration step number, specifically:

According toIt is found that n₁And n₂Respectively accelerate step number and deceleration step number；

By n₁* accel=n₂* decel obtains max_s_lim*accel=decel_val*decel, even max_s_ Lim < accel_lim step number of then slowing down is

Step number of slowing down if max_s_lim > accel_lim is decel_val=- (step-accel_lim).

Further, it is watched according to four kinds of states of servo motor and the acceleration step number and deceleration step number, control Take motor starts and stops speed, specifically:

When state machine starts starting, the state of open state machine is ACCEL；

According to the acceleration step number, the speed of servo motor is promoted to the pre-set velocity；

The state for changing state machine is RUN, and remain a constant speed operation；

According to the deceleration step number, the state of state machine is changed to DECEL, until the speed of servo motor is reduced to 0；

The state for changing state machine is STOP.

On the other hand, another embodiment of the present invention provides a kind of servo control systems, including the first calculating Module, the second computing module, third computing module, the 4th computing module, setup module and control module；

First computing module, for calculating the most short timer period according to pre-set velocity；

Second computing module, for calculating the initial value of timer according to step angle；

The third computing module reaches step number needed for pre-set velocity for calculating servo motor, is denoted as acceleration step number；

4th computing module, for according to acceleration and deceleration calculation deceleration step number；

The setup module is respectively as follows: STOP, ACCEL, RUN and DECEL for four kinds of states of servo motor to be arranged；

The control module, for according to four kinds of states of servo motor and the acceleration step number and step of slowing down Number, control servo motor starts and stops speed.

The purpose of the embodiment of the present invention is that provide a kind of control method of servo motor, can effectively make servo motor starting and It is gently run when stopping, improving the stability of servo motor work.

Detailed description of the invention

Fig. 1 is a kind of flow diagram of control method of servo motor provided by the invention；

Fig. 2 is a kind of hardware block diagram of control method of servo motor provided by the invention；

Fig. 3 is a kind of speed control schematic diagram of control method of servo motor provided by the invention；

Fig. 4 is a kind of rate curve schematic diagram of control method of servo motor provided by the invention；

Fig. 5 be a kind of control method of servo motor provided by the invention without uniform motion when acceleration and deceleration rate curve illustrate Figure；

Fig. 6 is a kind of signal of acceleration and deceleration rate curve when having uniform motion of control method of servo motor provided by the invention Figure；

Fig. 7 is a kind of speed operating status schematic diagram of control method of servo motor provided by the invention；

Fig. 8 is a kind of state machine operating mechanism schematic diagram of control method of servo motor provided by the invention；

Fig. 9 is the variable schematic diagram in a kind of calculating time delay of control method of servo motor provided by the invention；

Figure 10 is a kind of structural schematic diagram of servo control system provided by the invention.

Specific embodiment

Following will be combined with the drawings in the embodiments of the present invention, and technical solution in the embodiment of the present invention carries out clear, complete Site preparation description, it is clear that described embodiments are only a part of the embodiments of the present invention, instead of all the embodiments.It is based on Embodiment in the present invention, it is obtained by those of ordinary skill in the art without making creative efforts every other Embodiment shall fall within the protection scope of the present invention.

The first embodiment of the present invention:

Please refer to Fig. 1-9.

The embodiment of the invention provides a kind of control method of servo motor, at least include the following steps:

S1, the most short timer period is calculated according to pre-set velocity；

S2, the initial value that timer is calculated according to step angle；

S3, calculating servo motor reach step number needed for pre-set velocity, are denoted as acceleration step number；

S4, according to acceleration and deceleration calculation deceleration step number；

S5, four kinds of states that servo motor is arranged, are respectively as follows: STOP, ACCEL, RUN and DECEL；

S6, according to four kinds of states of servo motor and accelerate step number and deceleration step number, control servo motor starting and Stop speed.

In embodiments of the present invention, referring to Fig. 2, being hardware block diagram provided by the invention, including MCU minimum system, confession Electric module, downloading and simulation debugging module, AccessPort module, servo motor drive module composition.Microprocessor UM006801 It is a super low-power consumption processor chips, there is high-precision ADC, the peripheral hardwares such as UART, SPI, the work of inner high speed clock 20MHz Working frequency.Servo motor control unit includes servomotor controller and servo motor, and servomotor controller selects Delta (AELTA) ASD-B2-0421-B, motor select Delta ECMA-C20604.Rated motor revolving speed is 3000r/min, maximum speed For 5000r/min, one circle of motor rotation needs 160000 pulses.The present invention is by calling function Move () that speed control is arranged Device processed.Function Move () calculates required all parameters first and is stored in speed data structural body, then enables Timer interrupts.Timer is generated according to required speed ramp and is interrupted, and function Step_ is called in each interruption Counter () carrys out shift servo motor.The state STOP of servo motor is halted state, and ACCEL is acceleration mode, and RUN is fortune Row state, DECEL are deceleration regime.The embodiment of the present invention passes through fixed step size (step), acceleration (accel), deceleration (decel), pre-set velocity (speed) four parameters describe rate curve, pass through calculate it is in servo motor operational process plus Trot number and deceleration step number were running servo motor according to accelerating step number and deceleration step number and the state of state machine to change The case where carrying out T-type acceleration and deceleration in journey, velocity jump can be effectively prevented from generation, when so as to improve servo motor operation Stationarity and shutdown process stationarity, reduce running noises and improve transmission mechanism positioning accuracy.

As a kind of specific embodiment of the embodiment of the present invention, the most short timer period is calculated according to pre-set velocity, specifically Are as follows:

The step-length step of servo motor, acceleration accel, deceleration decel and pre-set velocity speed are set, according to step Most short timer period m in_delay=(ALPHA/tt)/w is calculated in elongation, wherein ALPHA is step angle, and tt is timer Period, w are pre-set velocity.

As a kind of specific embodiment of the embodiment of the present invention, the initial value of timer is calculated according to step angle, specifically Are as follows:

According to S=at²/ 2,The correction amount for obtaining servo motor isWherein S is Motor walks the distance of 1 step, and S=ALPHA, a are acceleration accel, and t is the time；

The initial value of timer are as follows:

Srd.step_delay=(T1_FREQ_148*sqrt (A_SQ/accel))/100)；Wherein, T1_FREQ_148 =(0.676*T1_FREQ)/100, A_SQ=ALPHA*2*10000000000, T1_FREQ is timer frequency.

As a kind of specific embodiment of the embodiment of the present invention, calculates servo motor and reaches step number needed for pre-set velocity, It is denoted as acceleration step number, specifically:

V_t ²-V₀ ²=2as, if V₀=0, then

Setting the acceleration time beDeceleration time is

If V_on=V_off=0,V_t=ω_n, V₀=0, a=ω；

Then

Thenn₁+n₂=step；

Obtaining acceleration step number is

As a kind of specific embodiment of the embodiment of the present invention, according to acceleration and deceleration calculation deceleration step number, tool Body are as follows:

According toIt is found that n₁And n₂Respectively accelerate step number and deceleration step number；

By n₁* accel=n₂* decel obtains max_s_lim*accel=decel_val*decel, even max_s_ Lim < accel_lim step number of then slowing down is

Step number of slowing down if max_s_lim > accel_lim is decel_val=- (step-accel_lim).

As a kind of specific embodiment of the embodiment of the present invention, according to the four of servo motor kinds of states and accelerate step number With deceleration step number, control servo motor starts and stops speed, specifically:

When state machine starts starting, the state of open state machine is ACCEL；

According to step number is accelerated, the speed of servo motor is promoted to pre-set velocity；

The state for changing state machine is RUN, and remain a constant speed operation；

According to deceleration step number, the state of state machine is changed to DECEL, until the speed of servo motor is reduced to 0；

The state for changing state machine is STOP.

In embodiments of the present invention, it is to be understood that when servo motor starts mobile, if mobile multiple steps Number, the state of state machine are changed to ACCEL；If only moving 1 step, the state of state machine is changed to DECEL.When state machine When state is changed to ACCEL, application program is according to accelerating step number to accelerate servo motor, until reaching pre-set velocity and by state machine State be changed to RUN；Or when must reduce speed now according to deceleration step number, state is changed to DECEL.When the state of state machine When being set as RUN, servo motor remains a constant speed operation, until that must be reduced speed now according to deceleration step number, by the state of state machine It is changed to DECEL.Hold mode machine slows down in DECEL state within deceleration step number, then more by the state of state machine It is changed to STOP.It is understood that for each step during acceleration and deceleration, it is necessary to calculate a new time delay.In order to improve Precision, the remainder in calculating process is retained in be calculated next time, and calculation formula is as follows:

New_rest=(2*step_delay+rest) (mod (4*accel_count+1)), wherein mod is complementation fortune It calculates.

Referring to Fig. 9, to calculate the variable schematic diagram in time delays.Wherein, step number, ACCEL tep_count: are counted When starting since 0, count value after the completion of DECEL is identical as the command parameter step value of transmitting.

Accel_count: for controlling acceleration and deceleration.In ACCEL, accel_coun is since 0, and each step is incremented by, directly Terminate to ACCEL.When DECEL starting, it is arranged to decel_val, this is negative, and each step all increases.When it When reaching 0, movement terminates, and state is set as stopping.Decel_start: it reduces speed now.Work as step_count=decel_ When start, state is set as DECEL.

The implementation of the embodiments of the present invention has the following beneficial effects:

In embodiments of the present invention, it is to be understood that the present invention is by calling function Move () that speed control is arranged. Function Move () calculates required all parameters first and is stored in speed data structural body, then enables timing Device interrupts.Timer is generated according to required speed ramp and is interrupted, and calls function Step_Counter () in each interruption Carry out shift servo motor.The state STOP of servo motor is halted state, and ACCEL is acceleration mode, and RUN is operating status, DECEL is deceleration regime.The embodiment of the present invention passes through fixed step size (step), acceleration (accel), deceleration (decel), presets Speed (speed) four parameters describe rate curve, pass through the acceleration step number and deceleration calculated in servo motor operational process Step number carries out T-type in the process of running to servo motor and adds according to accelerating step number and deceleration step number and the state of state machine to change The case where slowing down, velocity jump can be effectively prevented from generation, so as to improve stationarity when servo motor operation and open Stop the stationarity of process, reduce running noises and improves the positioning accuracy of transmission mechanism.

Second embodiment provided by the invention:

Please refer to Figure 10.

The embodiment of the invention provides a kind of servo control systems, including the first computing module 101, second to calculate mould Block 102, third computing module 103, the 4th computing module 104, setup module 105 and control module 106；

First computing module 101, for calculating the most short timer period according to pre-set velocity；

Second computing module 102, for calculating the initial value of timer according to step angle；

Third computing module 103 reaches step number needed for pre-set velocity for calculating servo motor, is denoted as acceleration step number；

4th computing module 104, for according to acceleration and deceleration calculation deceleration step number；

Setup module 105 is respectively as follows: STOP, ACCEL, RUN and DECEL for four kinds of states of servo motor to be arranged；

Control module 106 controls servo for the four kinds of states and acceleration step number and deceleration step number according to servo motor Motor starts and stops speed.

In embodiments of the present invention, it is to be understood that when servo motor starts mobile, if mobile multiple steps Number, the state of state machine are changed to ACCEL；If only moving 1 step, the state of state machine is changed to DECEL.When state machine When state is changed to ACCEL, application program is according to accelerating step number to accelerate servo motor, until reaching pre-set velocity and by state machine State be changed to RUN；Or when must reduce speed now according to deceleration step number, state is changed to DECEL.When the state of state machine When being set as RUN, servo motor remains a constant speed operation, until that must be reduced speed now according to deceleration step number, by the state of state machine It is changed to DECEL.Hold mode machine slows down in DECEL state within deceleration step number, then more by the state of state machine It is changed to STOP.It is understood that for each step during acceleration and deceleration, it is necessary to calculate a new time delay.In order to improve Precision, the remainder in calculating process is retained in be calculated next time, and calculation formula is as follows:

New_rest=(2*step_delay+rest) (mod (4*accel_count+1)), wherein mod is complementation fortune It calculates.

Referring to Fig. 9, to calculate the variogram in time delays.Wherein, step number, ACCEL starting tep_count: are counted When since 0, count value after the completion of DECEL is identical as the command parameter step value of transmitting.

Accel_count: for controlling acceleration and deceleration.In ACCEL, accel_coun is since 0, and each step is incremented by, directly Terminate to ACCEL.When DECEL starting, it is arranged to decel_val, this is negative, and each step all increases.When it When reaching 0, movement terminates, and state is set as stopping.

Decel_start: it reduces speed now.As step_count=decel_start, state is set as DECEL.

The implementation of the embodiments of the present invention has the following beneficial effects:

In embodiments of the present invention, it is to be understood that the present invention is by calling function Move () that speed control is arranged. Function Move () calculates required all parameters first and is stored in speed data structural body, then enables timing Device interrupts.Timer is generated according to required speed ramp and is interrupted, and calls function Step_Counter () in each interruption Carry out shift servo motor.The state STOP of servo motor is halted state, and ACCEL is acceleration mode, and RUN is operating status, DECEL is deceleration regime.The embodiment of the present invention passes through fixed step size (step), acceleration (accel), deceleration (decel), presets Speed (speed) four parameters describe rate curve, pass through the acceleration step number and deceleration calculated in servo motor operational process Step number carries out T-type in the process of running to servo motor and adds according to accelerating step number and deceleration step number and the state of state machine to change The case where slowing down, velocity jump can be effectively prevented from generation, so as to improve stationarity when servo motor operation and open Stop the stationarity of process, reduce running noises and improves the positioning accuracy of transmission mechanism.

It is the preferred embodiment of the present invention above, it is noted that for those skilled in the art, Various improvements and modifications may be made without departing from the principle of the present invention, these improvements and modifications are also considered as this hair Bright protection scope.