JPS61173162A - Velocity detecting circuit - Google Patents

Abstract

PURPOSE:To detect the velocity with high precision at the time of operating at the high velocity and low velocity by using properly the prescribed two approximations according to the velocity for the velocity detection. CONSTITUTION:The 1st counter circuit 4, the 1st register 7, the 4th counter circuit 10 and the 2nd control circuit 11 are used to calculate the high velocity approximation 1. The 2nd counter circuit 5, the 1st control circuit 6, the 3rd counter circuit 9, the 2nd register 12, etc. are used to calculate the low velocity approximation II. In other words, the time DELTAT required for making a rotary encoder 2 to change by the specified quantity deltatheta is measured to calculate the velocity at the low velocity by deltatheta/DELTAt or the variation DELTAtheta of the encoder 2 within the specified time DELTAt is measured to calculate the velocity at the high velocity by DELTAtheta/DELTAt. Hereby, the velocity detection with high precision can be always performed.

Description

DETAILED DESCRIPTION OF THE INVENTION (7) Technical Field The present invention relates to a speed detection circuit for accurately determining speed in a servo control device.

(a) Prior art and its problems Servo control is a powerful form of automatic control technology that controls the positioning of robot arms and machine tools by driving motors and the like.

Servo control is simply a method of detecting the current position of an object being operated, such as a robot arm or machine tool, using a simple method, and detecting the difference (called deviation) from the target position.
Drives a power source such as a motor with an amount proportional to In this way, the object is moved to the target position and stopped at the target position. Since the deviation is 0 at the target position, the driving force is also 0.

This will be explained using a motor as a general power source as an example.

The current rotational position of the motor shaft is θ, and the target position is θ.

, the proportional constant is K, and the current value applied to the motor is determined by the following formula (%) (1). Since the motor is a DC servo motor, it can rotate in forward and reverse directions, and the rotational force can change continuously in proportion to the current, for example.

The motor will not stop at the target value with only the current in equation (1).
It simply oscillates around the target value. However, in reality, there is frictional resistance in the motor itself, the speed reducer, and the moving parts of the robot arm and machine tool that are the loads, so vibrations around the target value are attenuated and gradually converge to the target value. I can do things.

However, even then, the amount of resistance is insufficient, vibration occurs near the target value, and it often does not converge easily.

Therefore, in addition to equation (1), the current rotational speed dθ/dt
Compensation proportional to the amount is often added.

That is, in place of equation (1), the current value ■ is determined by dθ I = K((θ0−〇)−h−) (2) t. This equation is a control equation for servo control that is often used in current stages.

The rotation of the potentiometer is used to detect the position of the rotation angle θ. The rotational speed (dθ/dt) is detected by a tacho generator. Since the two detectors generate voltage signals proportional to the position and velocity, equation (2) can be realized with an analog circuit by combining a suitable operational amplifier.

Both potentiometers and tough generators are analog detection elements, so they are useful as sensors for analog circuits, but if you try to improve their accuracy, the cost of the sensor increases. Furthermore, there is also the problem of offset adjustment. Furthermore, the voltage of the detection element, the positional deviation, and the speed may not necessarily be linear, and may require linearization.

On the other hand, advances in integrated circuit technology have made microprocessors available at low cost in recent years. For this reason,
Servo control, in which the control expressed by equation (2) is calculated by a microprocessor and converted into motor current, has become popular.

This is called software servo. According to software servo, it is possible to easily add compensation for disturbances (for example, gravity, etc.) and inertial force to equation (2) by changing the software. For this reason, even more precise control can be performed.

Furthermore, according to the software servo, digital processing of the detection of angular position 0 can be realized. A pulse-type rotation amount detector, such as a rotary encoder, can be used, and the accuracy of θ detection is also improved.

In addition to the fact that the control formula can be easily changed and the detection accuracy is high, software servo does not necessarily require two detectors.

This is because the rotary encoder allows speed detection as well as position detection.

Speed is a derivative of position, and in software servo, the position at each time can be stored, so the speed dθ/dt is calculated from the previous sampling time (t) stored in memory. -Δt) value of θ(-Δt)
The difference between the current value θ(1) and the current value θ(1) is divided by Δt.

That is, shall be. In this way, a speed detector is not required.

Since Δt is the sampling time and is a constant, (2
) will be included in the coefficient of the speed compensation term in the equation. The sampling time Δt is sufficiently short, and the substitution in equation (3) has been considered appropriate. This is because it is often done to replace a differential by a difference.

The method of calculating the differential using the difference in θ has no problem when the change in θ is sufficiently large (when the speed is fast). The difference in θ gives a good approximation of the differential value.

However, when the speed is slow, the accuracy tends to be poor. A rotary encoder is used to digitize position detection, but the θ of one pulse of the rotary encoder is
If the detected position interval is ε, then small position changes smaller than ε cannot be detected at all.

Also, the position change within one sampling time Δt is ε
If it is on the order of , the error will be extremely large.

Control devices such as robots are required to operate not only at high speeds but also at low speeds and with high precision. A speed detection method with sufficient accuracy is strongly desired even during low speed operation.

The fact that the change in position θ is small means that the third term in parentheses in the control equation (2) is small. If the difference (θ.-〇) between the first term and the second term is large, there will be little problem, but when dθ/dt is small, (θ.-〇)
〇) is also small. This is because it is on the verge of converging to the target value.

If the change in θ for one pulse of the rotary encoder is ε, then the maximum error of (θ.−〇) is ε.

The maximum error for speed (Jj1 minute) is hε/Δt.

Since the sampling time Δt is sufficiently short, h/Δt often has a value much larger than 1.

Then, when the deviation (θ0-θ) is sufficiently small, the speed compensation term dθ/dt is too large, so that (θ.-〇) is buried in the error of the speed compensation term. Therefore, θ is θ. In some cases, it becomes difficult to converge.

(1) Purpose The present invention provides a speed detection circuit that can perform speed detection with sufficient accuracy during motor speed control in a software servo system, both during high-speed operation and during low-speed operation. aim at something. This is intended to improve servo control performance during low-speed operation.

2) Configuration The speed detection of the present invention uses two approximate expressions depending on the speed. Rather than using one formula in common as in the past, different formulas are used for high-speed operation and low-speed operation.

During high-speed operation, the change in angular position Δθ within a certain sampling time Δt is determined in the same manner as in the conventional method. In other words, Δθ is the measured value. When written as a formula, it becomes . This is the same as equation (3). In order to distinguish
This is called a fast approximation formula.

When operating at low speeds, a completely different method of speed detection is used. Rather than specifying a certain amount of change in terms of time, we specify it in terms of angular change. The time ΔT during which the angle θ undergoes a constant change δθ is measured. When δθ is constant, the corresponding time length ΔT is measured. This does not count the number of pulses of a rotary encoder, but rather counts the timing pulses of a timer.

Expressed as a formula, it is. In this equation, δθ is constant. The value of δθ is
Naturally, it can be defined as an integer multiple of the minimum angle & change ε of the rotary encoder. δθ=mε. m may be 1. However, the larger m is, the longer ΔT is and the smaller the error is. On the other hand, if m is increased, the time for the change in δθ becomes longer, and there is a problem in that the value of the speed (dθ/dt) is not updated easily. (5)
The formula is called a slow approximation formula for simplicity.

In high-speed approximation formula (4), the denominator Δt is constant. Therefore,
This is the same as equation (3) and can be called a differential equation.

However, in the slow approximation formula (5), the δθ of the molecule is constant,
I can't say it's a difference.

The high-speed approximation formula (4) calculates the change in the angular position θ within a certain time Δt. Therefore, when the position changes quickly, that is, when the speed is fast, the amount of change in θ is large, so the speed can be determined with high accuracy.

In the low speed approximation formula (5), the time ΔT when the angular position θ changes by a constant value δθ is measured, and the speed is determined from this. When the speed of the motor is slow, ΔT becomes large, so the speed can be determined with particularly high accuracy.

A constant speed Vc is predetermined as the boundary value between high speed and low speed, and if the speed (dθ/dt) is greater than or equal to Vc, the high speed approximation formula (4) is used, and if it is less than or equal to vc, the low speed approximation is used. Equation (5) is used.

FIG. 1 shows a block diagram of a speed detection circuit according to the present invention.

The motor 1 is connected to a robot arm or a movable part of a machine tool via a speed reducer or the like. This is a DC servo motor and can rotate forward and backward. The rotational force changes in proportion to the current value. The drive mechanism of the motor 1 is a part that changes the voltage of the motor 1 by a servo control system, but the configuration of the servo system itself may be a known one, so it is not shown here. Reducers, robot arms, etc. are also omitted.

Changes in the rotation of the motor 1 are detected by a rotary encoder 2 that is directly or indirectly connected to the motor 1.

The rotary encoder 2 controls the motor O rotation by pulse train 2.
Output by 0. This produces one pulse for each minimum rotation angle ε. Since the direction of rotation must be detected, the pulses are shifted by 1/4 period.
Output as two pulse trains. The direction of rotation can be determined depending on which of the two pulse trains precedes the other.

Noise is removed from such a pulse train 20 by the waveform shaping rotational direction determination circuit 3, and the rotational direction is determined.

The waveform shaping rotation direction determination circuit 3 outputs a pulse train 21 and a rotation direction signal 22. These pulse trains and signals are input to the first counter circuit 4 and the second counter circuit 5.

The first counter circuit 4 and the second counter circuit 5 count the number of pulses including the sign based on the pulse train 21 and the rotation direction signal 22. That is, when the rotation direction signal 22 is in the forward direction, the pulse train 21 is added to the current count value. If the rotation direction signal 22 is in the opposite direction, the pulse train 21 is subtracted from the current count value.

The first counter circuit 4 and the second counter circuit 5 are, for example,
It can be easily constructed using an up/down counter IC. Connect the rotation direction signal 22 to the Up/Down input to switch between count up and count down.

The pulse train C of the rotary encoder 2 represents the rotational displacement of the motor (or load). As already mentioned, rotational displacement is used in two ways in the present invention. One is the rotational displacement Δθ within a certain period of time Δt, which is an undetermined amount. The first counter circuit 4 counts this, and if the counted value is k and the unit angle is ε, then Δ
θ= and ε.

Rotational displacement is also used to measure the -heavy machine small angle δθ, which is used to measure how much time ΔT elapses during a constant angle change δθ.

The role of the second counter circuit 5 is to measure δ0. If δθ=mε is determined in advance, the first control circuit
The second counter circuit 5 is cleared and reset for each count.

In addition to angular position changes, the present invention also relates to time measurement.
Requires two counting circuits. The measurement of the position θ and the time t are symmetrical.

A reference frequency oscillation circuit 8 is provided for time measurement. The clock pulses generated from this have a constant period τ and provide a time reference. The clock pulse 23 is supplied to the third counter circuit 9. ! : is input to the fourth counter circuit 10.

The fourth counter circuit 10 counts the pulse train in order to obtain a certain period of time Δt. Let Δt=pτ.

The second control circuit 11 stores the value of p in advance, compares the output of the fourth counter circuit 10 with p, and clears and resets the fourth counter circuit 10 each time p is reached. Δt
The time width signal is given to the first register 7 as a set signal.

The third counter circuit 9 also measures the clock pulse 23t, which measures how much time ΔT has elapsed before a constant angle change δθ occurs. The second register 12 stores the time ΔT that has elapsed to cause a change in δθ.
remember.

That is, in order to obtain the high-speed approximation formula (4), the first counter circuit 4, the first register 1, the fourth counter circuit 10, and the second control circuit 11 are used. The second counter circuit 5, first control circuit 6, third counter circuit 9, second register 12, etc. are used to obtain the low-speed approximation formula (5).

Since the pulses of the C2-IJ-x encoder 2 are either forward or reverse, the first counter circuit 4 and the second counter circuit 5 must be up-down counters. However, the third counter 9 and the fourth counter 10 for time measurement
etc. can be just a counter.

ke) production First, we will explain how the slow approximation formula works.

Time ΔT for rotary encoder 2 to change by a certain amount δθ
(This is an unknown quantity) must be measured.

The first control circuit 6 first controls the second counter circuit 5 and the third counter circuit 5.
The contents of the counter circuit 9 are simultaneously cleared by a clear/reset signal 24, and counting is started. The second counter circuit measures a pulse train proportional to the rotation of the rotary encoder 2. The third counter circuit 9 counts the clock pulses 23 emitted every τ,
Measure time.

The first control circuit 6 monitors the count output of the second counter circuit 5, and when it reaches a predetermined number of pulses m (=δθ/ε), that is, when the rotary encoder 2 has rotated by δθ, the second register A set signal 25 is sent to 12. Count data n of the third counter circuit 3 (n=ΔT/
ε) is set in the second register 12.

As a result, the time ΔT (=nε) required for the output of the rotary encoder 2 to change by a certain amount δθ is stored in the second register 12 in the form of a count value n of the reference layer and the wave number (1/ε). , stored.

Thereafter, the first control circuit 6 sends a clear/reset signal 24 to the second counter circuit 5 and the third counter circuit 9.
is issued, the count value is returned to 0, and counting starts again.

The same thing is repeated below. By this, the constant angle δθ
The time required for rotary encoder 2 to rotate by Δ
You can find T. Since ΔT is known, the microprocessor can calculate the speed at low speed using equation (5). Since ΔT is a relatively large value, it is highly accurate.

Next, the operation for finding a high-speed approximate expression will be explained. It is necessary to find the amount of rotational change Δθ of the rotary encoder 2 within a certain time Δt.

The second control circuit 11 issues a clear/reset signal 26, clears the first counter circuit 4 and the fourth counter circuit 10, and starts counting.

The first counter circuit 4 counts the number of pulses emitted from the rotary encoder 2 and
Measure the amount of rotation change at 0.

The fourth counter circuit 10 counts clock pulses 23. A certain period of time Δt is predetermined as pτ. The second control circuit 11 monitors the count value of the fourth counter circuit 10, and when it reaches a predetermined value p, that is, when Δt (=pτ) has elapsed, the second control circuit 11 controls the contents of the first counter circuit 4 using a set signal 27. (k=Δθ/ε) is stored in the first register 7. As a result, the first Lujistar 7 is
The amount of change Δθ of the rotary encoder 2 at time t) is stored in the form of a count value k (=Δθ/ε). After this, the second control circuit 11 issues a clear/reset signal 26 to the first counter circuit 4 and the fourth counter circuit 10.
Clear/reset. The same procedure is repeated below.

Since the amount of change Δθ of the rotary encoder 2 within Δt can be known from the stored contents of the register 7, the speed can be calculated using the high-speed approximation equation (4).

The microprocessor reads the contents Δθ and ΔT of the register 7 (for high speed) and the second register 12 (for low speed), and calculates the speed using the high speed approximation formula (4) or the low speed approximation formula (5). .

Which formula to use can be determined by determining the speed threshold value Vc in advance and comparing this with (dθ/dt). However, unless we know which formula to use, we cannot calculate (dθ/dt).

Therefore, the magnitude with respect to Vc is determined based on the previous calculated value, and based on this result, high-speed approximation or low-speed approximation is selected. When (dθ/dt) crosses Vc in the vicinity of Vc, the types of approximation expressions may differ. However, the speed of the calculation result itself is almost the same whether (4) or (5) is used. In addition, in order to match not only the values themselves but also the accuracy, it is sufficient to establish the following relationship between the constants Δt1δθ and Vc: VcΔt = δθ (6). However, since it is not necessary to strictly match the accuracy, equation (6) does not have to hold true.

In the example shown in FIG. 1, the first counter circuit 4 is cleared by the clear/reset signal 26 every Δt. Therefore, only a short change Δθ in Δt time is counted from the time when time measurement starts. Therefore, in order to obtain the current angular position θ of the rotary encoder 2, a counter circuit is required separately. θ is necessary for position feedback. It appears in equation (1).

In some cases, it may be desired to use the first counter circuit 4 to also count θ. This is because one counter circuit can be saved.

In order to meet such demands, a part of the circuit may be changed as shown in FIG.

Two registers 30 and 31 are added in parallel between the first counter circuit 4 and the first register 7. The clear/reset signal 26 is not input to the first counter circuit 4. 1st
The counter circuit 4 stores the angular position θ of the rotary encoder 2.

The count value of the first counter circuit 4 is temporarily stored in the third register 30 and the fourth register 31.

The clear/reset signal 26 issued from the second control circuit 11 is input to the third register 30, and the first
The output of the counter circuit is temporarily stored in the third register 30. This is a count value corresponding to θ(1) in equation (3).

Further, the count value of the first counter circuit 4 is temporarily stored in the fourth register 31 by the set signal 27. This corresponds to θ(t−Δt) in equation (3).

The subtraction circuit 32 includes a third register 30 and a fourth register 31.
Calculate the difference between the values of . In this way, Δθ can be determined. This is temporarily stored in the first register 7.

In this way, the first counter circuit 4 can store θ and use it as the current position for position feedback.

(Force) Effect When controlling the speed of a motor using the software servo method, it is possible to detect the speed with sufficient accuracy both during high-speed operation and during low-speed operation.

Particularly during low-speed operation, since the speed value can be precisely measured, the motor etc. can be stopped accurately and quickly to the target value. It has higher accuracy at low speeds than analog measurement using a tough generator, and there is no offset.

It is much more accurate than conventional software servo speed detection. There's no hysteresis either.

[Brief explanation of drawings]

FIG. 1 is a block diagram of a speed detection circuit according to the present invention. FIG. 2 is a partial block diagram of a partially modified version of the one shown in FIG. 1...Motor 2
・・・・・・・・・ Rotary encoder 3 ・
... Waveform shaping rotation direction discrimination circuit 4 ...
...... First counter circuit 5 ......
・・ 2nd counter circuit 6 ・・・・・ 1st
Control circuit 7 ・・・・ No. 3 register 8 ・・・ Reference frequency oscillation circuit 9 ・・・
......Third counter circuit 10...
... Fourth counter circuit 11 ......
- Second control circuit 12 ...... Second register 20 ...... Pulse train 21 ...... Pulse train 22 ...... Rotation direction signal 23°
°°゛°“゛ Clock pulse 24 ・・・・・・・・・
・・Clear/reset signal 25・・・・・・・・・
Set signal 26 ・・・・・・・・・ Clear/reset signal 2
7 ......Set signal 30 ......Third register 31 ・
・・・・・・・・・ 4th register inventor
Akira Ooka Fighting Ken Hon
Tsukasa

Claims (1)

[Claims] The first one that counts the pulse train from the rotary encoder 2.
The counter circuit 4, the second counter circuit 5, and the third counter circuit 9 that counts the pulse train from the reference frequency oscillator 8.
and the fourth counter circuit 10 and the count value of the rotary encoder 2 from the count value of the second counter circuit 5 is a certain constant amount δ
The first control circuit 6 controls the third counter circuit 9 to detect a change in θ and measure the time ΔT required for the change, and the fourth counter circuit 10 calculates a certain time interval Δt from the count value. and a second control circuit 11 that controls the first counter circuit 4 to detect and measure the amount of change Δθ in the pulse train of the rotary encoder 2 within the time Δt, and the rotary encoder 2 changes by a certain amount δθ. Measure the time ΔT required to
A speed detection circuit characterized in that the speed at high speed is determined by ΔT, or by measuring the amount of change Δθ of the rotary encoder 2 within a certain time Δt, and determines the speed at high speed by Δθ/Δt.