JPH05188068A - Servo motor speed detecting device - Google Patents

Abstract

PURPOSE:To provide a speed detecting device capable of detecting the speed closer to the actual rotating speed of a servo motor at the sampling time for speed detection. CONSTITUTION:The clock pulse number in each output pulse cycle of a pulse encoder PG is counted by a counter 13. A CPU 17 computes the rotating speed of a servo motor in two encoder output pulse cycles from the inverse number of these cycles immediately before sampling and before that, on the basis of the clock pulse count value latched to a first register 14. The rotating speed of the servo motor at the sampling time is then computed from the obtained servo motor rotating speed in the encoder output pulse cycles immediately before sampling and before that, the count value in these two cycles, and the clock pulse count value to be latched to a second register 16 until sampling after the generation of encoder output pulses immediately before sampling.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a servomotor speed detecting device for detecting the rotational speed of a servomotor based on the output pulse of a pulse encoder attached to the servomotor.

[0002]

2. Description of the Related Art An example of a conventional speed detecting device of this type is
It is disclosed in JP-A-59-160766. This conventional speed detection device includes a counter that counts the period of each output pulse from a pulse encoder attached to a servomotor using a clock pulse that is generated at a constant period, and a register that stores the counted output pulse period. And a memory that stores the reciprocal of the output pulse cycle in advance as a data table.The latest encoder output pulse cycle in a fixed sampling cycle is read from the register, and the encoder output pulse cycle is stored in the address of the memory. The conversion is performed and the reciprocal of the output pulse period is read from this memory to detect the rotation speed of the servo motor.

[0003]

However, in the above-mentioned conventional speed detecting device, the servo motor rotation speed is calculated based on the encoder output pulse cycle immediately before sampling in the sampling cycle. Is being accelerated or decelerated,
There is a problem that an error occurs between the actual servo motor rotation speed at that time at the time of sampling for speed detection and the calculated servo motor rotation speed, and, for example, in an articulated robot, the motion locus accuracy thereof deteriorates. ..

The present invention has been made in view of the above points, and even when the servomotor is accelerated or decelerated, the actual servomotor rotation speed at that time at the sampling point for speed detection. It is an object of the present invention to provide a speed detection device for a servo motor, which can detect a speed closer to.

[0005]

In order to achieve the above object, a speed detecting device for a servo motor according to a first aspect of the present invention rotates a servo motor based on an output pulse of a pulse encoder attached to the servo motor. In a speed detection device for detecting a speed, a clock pulse counter for counting the number of clock pulses in each cycle of output pulses of a pulse encoder, and a first value for latching a count value by the clock pulse counter every time an output pulse of the pulse encoder is generated. A register, a second register that latches the count value of the clock pulse counter based on a sampling signal, and an encoder output pulse immediately before and before sampling based on the clock pulse count value latched by the first register From the reciprocal of the cycle, The rotation speed of the servo motor is calculated, the servo motor rotation speed in the encoder output pulse cycle immediately before and before sampling, the clock pulse count value latched in the first register in these two cycles, and the second Arithmetic processing means for calculating the rotation speed of the servo motor at the time of sampling from the clock pulse count value when the sampling signal latched in the register is generated.

Further, a speed detecting device for a servo motor according to a second aspect of the present invention is a speed detecting device for detecting a rotation speed of a servo motor based on an output pulse of a pulse encoder attached to the servo motor. A frequency divider that divides a pulse, a clock pulse counter that counts the number of clock pulses in each cycle of output divided pulses of the pulse encoder that is divided by the frequency divider, and a count value of the clock pulse counter 1st to latch each time the encoder output divided pulse is generated
Register, a second register for latching the count value of the clock pulse counter based on a sampling signal, and an encoder output immediately before and before sampling based on the clock pulse count value latched by the first register. The rotation speed of the servo motor in these two cycles is calculated from the reciprocal of the divided pulse cycle, and the servo motor rotation speed in the encoder output divided pulse cycle immediately before and before sampling is stored in the first register in these two cycles. The rotation speed of the servo motor at the time of sampling is calculated from the latched clock pulse count value, the clock pulse count value when the sampling signal is latched in the second register, and the frequency division ratio information of the frequency divider. And an arithmetic processing unit. Than it is.

[0007]

In the servo motor speed detecting device according to the first aspect of the invention, the clock pulse counter counts the number of clock pulses in each cycle of the output pulses of the pulse encoder, and the first register is generated every time the output pulse of the pulse encoder is generated. A clock pulse count value indicating the number of clock pulses in the cycle of the encoder output pulse is latched in. On the other hand, based on the sampling signal, the second register latches the clock pulse count value indicating the number of clock pulses from immediately after the generation of the encoder output pulse immediately before the sampling to the sampling.

Then, based on the clock pulse count value latched in the first register by the arithmetic processing means,
The rotation speed of the servo motor in these two cycles is calculated from the reciprocal of the encoder output pulse cycle immediately before and before the sampling. Servo motor rotation speeds in the encoder output pulse cycle immediately before and before the obtained sampling, clock pulse count values latched in the first register in these two cycles, and sampling latched in the second register The rotation speed of the servo motor at the time of sampling is calculated from the clock pulse count value when the signal is generated. This makes it possible to detect a speed that is closer to the actual servo motor rotation speed at the time of sampling.

Further, in the speed detecting device for the servo motor according to the second aspect of the present invention in which the output pulse of the pulse encoder is divided by the frequency divider, the number of clock pulses in each cycle of the output divided pulse of the pulse encoder is The clock pulse counter counts, and a clock pulse count value indicating the number of clock pulses in the cycle of the encoder output divided pulse is latched in the first register every time the output divided pulse of the pulse encoder is generated. On the other hand, based on the sampling signal, the second register latches the clock pulse count value indicating the number of clock pulses from immediately after the generation of the encoder output divided pulse immediately before sampling to the sampling.

Then, based on the clock pulse count value latched in the first register by the arithmetic processing means,
The rotation speed of the servo motor in these two cycles is calculated from the reciprocal of the encoder output divided pulse cycle immediately before and before the sampling. The servo motor rotation speed in the encoder output divided pulse cycle immediately before and before the obtained sampling, the clock pulse count value latched in the first register in these two cycles, and latched in the second register The rotation speed of the servo motor at the time of sampling is calculated from the clock pulse count value when the sampling signal is generated and the frequency division ratio information of the frequency divider. As a result, even when the servomotor is accelerated or decelerated in a higher speed region, a speed closer to the actual servomotor rotation speed at that time at the time of sampling can be detected.

[0011]

EXAMPLES The present invention will be described below based on examples. 1 is a block diagram showing the structure of a speed detecting device for a servo motor according to an embodiment of the first invention, and FIG. 2 is a diagram for explaining the operation of the speed detecting device shown in FIG.

In FIG. 1, 11 is a waveform shaping circuit,
The pulse signal from the pulse encoder PG connected to the shaft of the servo motor (not shown) is shaped by the waveform shaping circuit 11 and input to the first flip-flop 12 as the encoder output pulse signal E _Pi ((a in FIG. 2). )reference). A clock pulse signal C _P from a clock pulse generator (not shown) is also input to the first flip-flop 12 (see FIG. 2B). The first flip-flop 12 synchronizes with the rising edge of the clock pulse signal C _P when the encoder output pulse signal E _{Pi is} input,
This is for generating a set encoder pulse signal (hereinafter referred to as SETEP signal) and a reset encoder pulse signal (hereinafter referred to as RSTEP signal) to be supplied to the clock pulse counter 13 ((c) in FIG. 2).
And (d)).

The clock pulse counter 13 to which the clock pulse signal C _P is input is reset in synchronization with the rising edge of the RSTEP signal, and in the period corresponding to the cycle of each encoder output pulse signal E _Pi , that is, each encoder output. The number of clock pulse signals C _P in the cycle of the pulse signal E _Pi is counted ((e) in FIG. 2).
reference). The count value of the clock pulse counter 13 is synchronized with the rising edge of the SETEP signal in the first register.
It is designed to be latched by 14 (see FIG. 2 (f)).

On the other hand, a sampling pulse signal S _P is generated from a sampling pulse generator (not shown) (see (g) of FIG. 2), and the sampling pulse signal S _P and the clock pulse signal C _P are generated. It is configured to be input to the second flip-flop 15. Second
The flip-flop 15 synchronizes with a rising edge of the clock pulse signal C _P when the sampling pulse signal S _{P is} input, and a set sampling signal (hereinafter, referred to as SETSP signal) and a reset sampling signal (hereinafter, RS).
It is called a TSP signal. ) And (see (h) of FIG. 2). The RSTSP signal is a signal used for initialization and the like.

The second register 16 connected to the clock pulse counter 13 is an S register from the second flip-flop 15 described above.
The count value of the clock pulse counter 13 is latched in synchronization with the rising edge of the ETSP signal (see (i) in FIG. 2).

A central processing unit (hereinafter referred to as a CPU) 17 as an arithmetic processing means, a memory 18 as a storage means.
And the SETEP signal and S as the interrupt signal.
When the ETSP signal is input, the arithmetic processing necessary for calculating the rotation speed of the servo motor at the sampling point is executed according to a predetermined control program as described later, and the rotation speed signal V _S is used for servo motor speed control (not shown). It is to be output to the department.

Each time the CPU 17 receives an interrupt request by the SETEP signal, each encoder output pulse E _Pi , i = 1 to 1 latched in the first register 14 at that time.
A count value C _i , i = 1 to n indicating the number of clock pulses C _P in the cycle of n is read. Also, when an interrupt request by the SETSP signal is received, at that time, the second register
The clock pulse count value CS, which is latched in 16, indicating the number of clock pulses from the time the encoder output pulse E _Pn immediately before sampling occurs to the time sampling is read.

Next, with reference to FIGS. 1 to 4, a procedure for calculating the servo motor rotation speed by the CPU in the speed detecting device having the above-described structure will be described below. FIG. 3 shows FIG.
FIG. 4 is a flow chart for explaining the procedure of calculating the servo motor rotation speed by the CPU of the speed detection apparatus shown in FIG. Is.

When an interrupt request by the SETEP signal output from the first flip-flop 12 is received by the encoder output pulse E _Pi input, at that time, the first register
Each encoder output pulse E latched by 14
A clock pulse count value C _i indicating the number of clock pulses C _P in the period of _Pi is read (step S101). Then, based on this clock pulse count value C _i , the rotation speed V _i of the servo motor in that cycle is calculated from the reciprocal of the cycle of the encoder output pulse E _Pi , and this rotation speed V _i is calculated.
_i and the clock pulse count value C _i are stored in the memory 18 (step S102). Such an operation is sequentially repeated every time an interrupt request by the SETEP signal is generated (i = 1 to n).

The rotation speed V _i (rp) of the servo motor
m) is calculated by the following equation. V _i = [1 / (C _i · TC)] · 60 / P where i = 1 to n, TC is the period (seconds) of the clock pulse C _P , and P is one revolution of the pulse encoder PG. The number of pulses.

When an interrupt request by the SETSP signal output from the second flip-flop 15 is received by the input of the sampling pulse signal S _P in this sampling cycle, the sampling latched in the second register 16 at that time is received. A clock pulse count value CS indicating the number of clock pulses from immediately after the occurrence of the encoder output pulse _EPn to the sampling immediately before is read (step S201).

This clock pulse count value CS and the memory 18
Was stored in a servo motor speed V _n-1 in the servo motor speed V _n and the period of the encoder output pulse E _pn-1 before the in the period of the encoder output pulses E _Pn sampling immediately before, these two From the clock pulse count values C _n and C _n-1 in the cycle, the rotation speed V _S of the servo motor at the time of sampling is calculated by the following equation (step S202). V _S = _{[(V n -V n-1)} / (C n -C n-1) ] - (-
CS) + V _{n In} this case, when the servo motor is driven at a constant speed, V _S = V _n .

The above-mentioned operation is performed every sampling cycle. As a result, even when the servo motor is accelerated or decelerated, it is possible to detect a speed closer to the actual servo motor rotation speed at that time at the sampling point for speed detection,
For example, by applying it to the control of an articulated robot,
The motion locus accuracy can be improved.

FIG. 5 is a block diagram showing the structure of a servomotor speed detecting device according to an embodiment of the second invention.
The configuration is substantially the same as that shown in FIG. 1 except that the output pulse from the pulse encoder is divided by a frequency divider.

In FIG. 5, 21 is a waveform shaping circuit,
The pulse signal from the pulse encoder PG connected to the axis of the servo motor (not shown) is shaped by the waveform shaping circuit 21 and then input to the frequency divider (programmable frequency divider) 29.
The frequency is divided by this frequency divider 29. The encoder output divided pulse signal EB _Pi from the frequency divider 29 is the first flip-flop.
Entered in 22. In the first flip-flop 22,
A clock pulse signal C _P from a clock pulse generator (not shown) is input. The first flip-flop 22 is synchronized with the rising of the clock pulse signal C _P when the divided pulse signal EB _{Pi is} input, and a set encoder pulse signal (hereinafter, referred to as a SETEBP signal) and a reset given to the clock pulse counter 23. This is for generating an encoder pulse signal (hereinafter referred to as an RSTEBP signal).

The clock pulse counter 23 to which the clock pulse signal C _P is input is reset in synchronization with the rising edge of the RSTEBP signal, and each divided pulse signal EB is reset.
The number of clock pulse signals C _{P in} the period corresponding to the period of _Pi , that is, in the period of each encoder output divided pulse signal EB _Pi is counted. The count value of the clock pulse counter 23 is the first flip-flop 22.
The signal is latched in the first register 24 in synchronization with the rising edge of the SETEBP signal.

On the other hand, the sampling pulse signal S _P and the clock pulse signal C _P from the sampling pulse generator (not shown) are input to the second flip-flop 25. The second flip-flop 25 synchronizes with the rising edge of the clock pulse signal C _P when the sampling pulse signal S _{P is} input, and a set sampling signal (hereinafter, SETSP signal) and a reset sampling signal (hereinafter, RSTSP signal). To generate and. The RSTSP signal is
This is a signal used for controlling the frequency dividing operation of the frequency divider 29.

The second register 26 connected to the clock pulse counter 23 latches the count value of the clock pulse counter 23 in synchronization with the rising edge of the SETSP signal from the second flip-flop 25.

The CPU 27 is provided with a memory 28, and by executing the SETEBP signal and the SETSP signal as interrupt signals, executes the arithmetic processing necessary for calculating the rotation speed of the servo motor at the sampling time according to a predetermined control program. The rotation speed signal V _S is output to a servo motor speed control unit (not shown).

Each time the CPU 27 receives an interrupt request by the SETEBP signal, each encoder output pulse EB _Pi , i = 1 latched in the first register 24 at that time.
Count values C _i , i = 1 to n indicating the number of clock pulses C _P in a cycle of up to n are read. Further, when an interrupt request by the SETSP signal is received, a clock pulse count value latched in the second register 26 at that time, which indicates the number of clock pulses from immediately after the generation of the encoder output divided pulse immediately before sampling to the sampling CS
Is designed to read.

On the other hand, 30 is a preset register, and this preset register 30 is for setting the frequency division ratio information given from the CPU 27 to the frequency divider 29 described above. If the frequency division ratio of the frequency divider 29 is represented by 1 / N, the frequency division ratio information is represented by the frequency division value N in this embodiment.

The encoder output divided pulse signal EB _Pi from the divider 29 is input to the first flip-flop 22 and also to the divided pulse counter 31. The frequency division pulse counter 31 is reset in synchronization with the rising edge of the RSTSP signal from the second flip-flop 25 and counts the number of encoder output frequency division pulse signals EB _Pi in the sampling cycle.

The count value of the divided pulse counter 31 is latched in the third register 32 in synchronization with the rising edge of the SETSP signal from the second flip-flop 25. Then, the encoder output divided pulse signal EB _Pi in the sampling period latched in the third register 32
The encoder output divided pulse count value indicating the number of
When an interrupt is generated by the SP signal, it is read by the CPU 27. The CPU 27 determines predetermined frequency division ratio information in the next sampling cycle based on the encoder output frequency division pulse count value, and performs an operation of giving the predetermined frequency division ratio information to the preset register 30.

Also in the speed detecting device constructed as described above, the CPU is operated in the same manner as the speed detecting device shown in FIG.
Based on the clock pulse count value latched in the first register 24, the rotation speed of the servo motor in these two cycles is calculated from the reciprocal of the encoder output frequency division pulse cycle immediately before and before sampling by 27. Then, the servo motor rotation speed in the encoder output divided pulse cycle immediately before and before sampling, the clock pulse count value latched in the first register 24 in these two cycles, and the sampling signal generated in the second register 26 are generated. The rotation speed of the servo motor at the time of sampling is calculated from the clock pulse count value at time and the frequency division ratio information of the frequency divider.

As a result, according to the speed detecting device having the frequency divider for dividing the output pulse of the pulse encoder, even when the servomotor is accelerated or decelerated in a higher speed range, the speed at the sampling time is increased. It is possible to detect a speed that is closer to the actual servo motor rotation speed at that time.

[0036]

As described above, according to the speed detecting device for a servo motor according to the first aspect of the present invention, from the reciprocal of the encoder output pulse period immediately before and before the sampling in the sampling period, the servo motor in these two periods is detected. The rotation speed is calculated, and the servo motor rotation speed in the encoder output pulse cycle obtained immediately before and before the sampling, the clock pulse count value in these two cycles, and the time immediately after the encoder output pulse immediately before sampling until the sampling Since the rotation speed of the servo motor at the time of sampling is calculated from the clock pulse count value, even if the servo motor is accelerated or decelerated, it can be calculated at the time of sampling for speed detection. Can be detected speed close to the actual servo motor speed, by applying the speed detection device, for example, in the control of the articulated robot,
The motion locus accuracy can be improved.

Further, according to the speed detecting device for the servo motor of the second aspect of the present invention, since the servo motor has a structure provided with a frequency divider for dividing the output pulse of the pulse encoder, the servo motor can operate in a higher speed range. Even when acceleration or deceleration driving is performed, it is possible to detect a speed that is closer to the actual servo motor rotation speed at the time of sampling.

[Brief description of drawings]

FIG. 1 is a block diagram showing a configuration of a speed detection device for a servo motor according to an embodiment of the first invention.

FIG. 2 is a diagram for explaining the operation of the speed detection device shown in FIG.

FIG. 3 is a flowchart for explaining a procedure of calculating a servo motor rotation speed by a CPU of the speed detection device shown in FIG.

FIG. 4 is another flowchart for explaining the procedure of calculating the servo motor rotation speed by the CPU of the speed detection device shown in FIG.

FIG. 5 is a block diagram showing a configuration of a speed detection device for a servo motor according to an embodiment of the second invention.

[Explanation of symbols]

11, 21 ... Waveform shaping circuit 12, 22 ... First flip-flop
13, 23 ... Clock pulse counter 14, 24 ... First register 15, 25 ... Second flip-flop 16, 26 ... Second register 17, 27 ... CPU 18, 28 ... Memory 29 ... Divider
30 ... Preset register 31 ... Divided pulse counter
32 ... 3rd register PG ... Pulse encoder

Claims (2)

[Claims] 1. A speed detection device for detecting a rotation speed of a servo motor based on an output pulse of a pulse encoder attached to a servo motor, wherein a clock pulse counter for counting the number of clock pulses in a cycle of each output pulse of the pulse encoder. A first register that latches the count value of the clock pulse counter each time an output pulse of a pulse encoder is generated; a second register that latches the count value of the clock pulse counter based on a sampling signal; Based on the clock pulse count value latched in the register of, the servo motor rotation speed in these two cycles is calculated from the reciprocal of the encoder output pulse cycle immediately before and before sampling, and the encoder output pulse immediately before and before sampling is calculated. Lap Wherein the servo motor speed, these two periods in the first
Clock pulse count value latched in the register
And a processing means for calculating the rotation speed of the servo motor at the time of sampling from the clock pulse count value when the sampling signal latched in the second register is generated, and a speed detection device for the servo motor. .. 2. A speed detecting device for detecting a rotation speed of a servo motor based on an output pulse of a pulse encoder attached to a servo motor, wherein the frequency divider divides the output pulse of the pulse encoder, and the frequency divider. A clock pulse counter that counts the number of clock pulses in each cycle of output divided pulses of the pulse encoder divided by; and a first value that latches the count value of the clock pulse counter every time the output divided pulse of the pulse encoder is generated. A register, a second register for latching the count value of the clock pulse counter based on a sampling signal, and an encoder output portion immediately before and before sampling based on the clock pulse count value latched by the first register. From the reciprocal of the circular pulse period, The rotation speed of the motor is calculated, the servo motor rotation speed in the encoder output divided pulse cycle immediately before and before sampling, the clock pulse count value latched in the first register in these two cycles, and the second Arithmetic processing means for calculating the rotation speed of the servo motor at the time of sampling from the clock pulse count value when the sampling signal latched in the register and the frequency division ratio information of the frequency divider are provided. Servo motor speed detector.