JPH077005B2 - Speed detector for article transfer device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION (Industrial field of application) The present invention relates to an article transfer device that moves an article gripper to transfer an article.
The present invention relates to a speed detecting device for detecting at low cost.

(Prior Art) Conventionally, the article transfer of this kind of article transfer apparatus has been performed by using the speed control apparatus shown in FIG. In FIG. 2, 30 is a transfer command signal line, 31 is a position detector, 32 is a reference speed generator, 33 is a speed comparator, 34 is a transfer mechanism, and 35 is a speed detector.

When the transfer command is transmitted from the host device to the reference generator 32 via the transfer command signal line 30, the reference value of the speed corresponding to the transfer distance is input to the speed comparator 33. At this time, the transfer mechanism 34
Is stopped, its speed detector 35 is zero. Therefore, the speed comparator 33 applies an output equal to the reference value of the speed to the transfer mechanism 34 to accelerate the transfer mechanism 34. The speed of the transfer mechanism 34 that started to move is detected by the speed detector 35, the speed comparator 33 again compares the speed reference values, and the acceleration / deceleration force is transmitted to the transfer mechanism 34 according to the excess or deficiency and transferred. The speed is controlled so as to follow the reference speed. On the other hand, since the reference speed value is generated from the reference speed generator 32 so as to decrease as it approaches the target position, the transfer speed of the transfer mechanism 34 that follows this reference value becomes lower as it approaches the target position. The speed is reduced to zero at the target position. In such speed control, the stop accuracy at the target position depends on the speed detection accuracy, so it is important to develop a high-accuracy speed detection device in order to improve the stop position accuracy. The following two types of devices are known as conventional speed detection devices.

A mechanical device that obtains a speed signal proportional to the rotation speed of the motor (hereinafter referred to as the speed) by a generator attached to the motor shaft.

An electric device used for positioner speed detection of a magnetic disk device, etc., which is an electric device for synthesizing an integral of a measured value of a motor current and a derivative of a position signal to obtain a speed signal.

(Problems to be Solved by the Invention) In the mechanical device, since the sensor is directly attached to the motor shaft, even if the motor speed changes due to frictional force or the like, it can be accurately detected. However, since the detector uses a hardware (hardware), there is a drawback that the detector cost and the mounting amount increase.

On the other hand, in the electric device, since the speed is detected by using the arithmetic function of the electric circuit, the detector cost and the mounting amount can be reduced. However, in the speed region where the position change is gradual, the differential value thereof becomes small, so that the integral value of the motor current becomes the main component of the speed signal. However, in such a low speed region, the motor generated torque is reduced by the frictional force, and the current of the motor current that overcomes the frictional force and actually contributes to the rotation of the motor is reduced. Is larger than the actual speed, and a speed detection error occurs. Also, in an actual system, electric circuit characteristics such as a power amplifier and an integrator gain change, so that an actual motor current with respect to a drive signal changes. Therefore, when the speed is detected by applying the electric method, it is necessary to measure the motor current in order to eliminate the influence of the fluctuation of the electric circuit characteristics on the speed detection accuracy. Further, recently, a control circuit is configured with a microprocessor to reduce the circuit cost and the mounting amount. In such digital signal control, the detection signal needs to be discretized, and therefore an analog-digital converter (ADC) has been used for current measurement. However, as is well known to those skilled in the art, since the cost of the ADC is proportional to the conversion speed and the conversion accuracy, an expensive ADC is required to perform speed detection with high speed and high accuracy. There was a drawback that the circuit was expensive.

(Object of the Invention) An object of the present invention is to provide a low-cost and high-accuracy speed detection device for an article transfer device, which does not use an expensive ADC and can improve the detection accuracy in a low speed region. is there.

(Means for Solving the Problems) In order to achieve the above object, the present invention has an article gripping section for gripping an article, a drive mechanism for moving the article gripping section up and down or left and right, and the article gripping section. In an article transfer apparatus having a position detector for detecting the position of a part, a means 1 for calculating a predicted speed value after one sampling time from a driving signal and a speed estimated value, means for accumulating the predicted value, driving signal, A means for calculating a position predicted value after one sampling time from the speed estimated value and the position estimated value, a means for accumulating the predicted value, a position sensor for measuring the sampling time and measuring the motor position after one sampling time has elapsed. It is characterized by comprising means for calculating a position prediction error from the measured position measured and the position prediction value, and means for correcting the speed prediction value with the position prediction error.

(Operation) According to the present invention, the speed prediction value after one sampling time is calculated from the drive signal to the constant current amplifier and the speed estimation value,
The position prediction value after one sampling is calculated and accumulated from the drive signal, the speed estimation value and the position estimation value, and the prediction error is calculated from the position actually measured after one sampling time and the prediction value at this position. Correct the predicted speed value with the prediction error.

(Embodiment) FIG. 1 is a diagram for explaining an embodiment of the present invention, in which 1 is a drive signal input line, 2 is a first coefficient unit (coefficient 1/2), and 3 is a first addition value switch. , 4 is a first adder, 5 is a speed accumulator, 6 is a second addition value switcher, 7 is a second adder, 8 is a position accumulator, 9 is a third adder, and 10 is Position comparator, 11 is a second coefficient unit (coefficient 2), 12 is an estimated speed signal line, 13 is an estimated position signal line, 14 is a measured position signal line, 15 is a constant current amplifier, 16 is a DC motor, 17 is Rotation angle detector, 18 up / down counter, 19 zero-order hold circuit, 20 addition value switching signal generator, 21 article gripping part, 22 wire rope, 23 drive pulley, 24 idler pulley .

The article gripping portion 21 directly connected to the wire rope 22 is frictionally driven by a drive pulley 23 directly connected to the motor 16.
In the drive mechanism having such a mechanical structure, the position signal of the article gripping portion 21 is detected by combining the rotation angle detector 17 directly connected to the drive pulley 23 and the up / down counter 18. The drive signal output from the zero-order hold circuit 19 is amplified by the constant current amplifier 15 to accelerate the motor 16.
If the radius of the drive pulley 23 is 1, and the frictional force acting on the drive mechanism is ignored, the article gripper accelerated by the motor 16
The predicted values of the velocity Vp (k) and the position Pp (k) of 21 can be expressed by the following equation.

Vp (k) = Ka × (Kt / J) × T × I (k−1) + Ve (k−
1) …… (1) Pp (k) = Ka × (Kt / J) × T ² × I (k-1) / 2 + Ve (k
-1) × T + Pe (k-1) (2) where T: sampling time, I (k-1): motor current at time k-1, Kt: motor torque constant, J: drive mechanism Inertial load, Ka; Motor torque constant, Ve (k-1), Pe (k-1): Estimated values of velocity and position at time k-1.

In the article transfer device having a constant coefficient in which Ka, Kt, J and T do not change with time in the equations (1) and (2),
(Kt / J) × T, normalized equation (2) in Ka × (Kt / J) × T 2/2, respectively normalized speed Vo (k), the normalized position Po
When called as (k), the equations (1) and (2) become the following equations.

Vo (k) = I (k−1) + Voe (k−1) (3) Po (k) = I (k−1) / 2 + Voe (k−1) × T + Poe
(K-1) (4) On the other hand, when the measured position at time k is Pa (k), the position prediction error εP (k) becomes Pa (k) -Po (k). Here, considering that the relationship between the speed and the position of the motor driven by the constant current is V × T / 2 = P (5), the speed prediction error εV when the position prediction error is εP. Is obtained from the equation (5) as the following equation.

[epsilon] V = 2 * [epsilon] P / T (6) Therefore, the speed estimation value at the time k to be obtained is given by the following equation by correcting the prediction value with the prediction error.

Voe (k) = I (k-1) + Voe (k-1) + 2 × εP / T
(7) Further, the position estimation value is Poe (k) = I (k−1) / 2 + Voe (k−1) × T + Poe
(K−1) + εP (8) (In the following explanation, the coefficient oe
Is omitted. In the present invention, the above speed estimation calculation is carried out in the embodiment shown in FIG. That is, since the first addition value switch 3 is switched to the drive signal line 1 side 3a by the addition switch signal generator 20 at time k-1, the first adder 4 drives the drive signal I (k- 1) and the output V (k−1) of the speed accumulator 5 in which the estimated speed value is accumulated are added to obtain the expression (3).
The predicted speed value at time k corresponding to is calculated and then stored in the speed accumulator 5. At the same time, the second addition value switch 6 adds the sum of the output V (k-1) of the speed accumulator 5 and the half value of the drive signal I (k-1) to the third addition value switcher 6.
Since it is switched to the output side 6a of the adder 9 of
This adder 7 adds the sum signal and the output P (k-1) of the position accumulated value 8 in which the position estimated value at time k-1 is accumulated, and the position at time k corresponding to equation (4) is added. The predicted value is calculated and then stored in the position accumulator 8.

At time k after one sampling time has elapsed, the additional value switching signal generator 20 switches the first and second additional value switching units 3 and 6 to the 3b and 6b sides, respectively, and at the same time measures the position.
Next, the position comparator 10 calculates a position prediction error from the measured value of the position transmitted via the signal line 14 and the above-described position predicted value transmitted via the signal line 13. This prediction error is transmitted to the second addition value switching device 6b in order to correct each prediction value of velocity and position, and at the same time, the amplitude is set to 2 by the coefficient unit 11.
It is amplified twice and transmitted to the 3b side of the first addition value switch 3. As a result, each predicted value calculated at time k is corrected and calculated by the adders 4 and 7 using the position prediction error,
Then, the velocity estimated value and the position estimated value shown in the equations (7) and (8) are accumulated in the velocity accumulator 5 and the position accumulator 8, respectively.

When the above estimation calculation is completed, the addition value switching signal generator 20 again sets the first and second addition value switching devices 3 and 3 to 3 times, respectively.
Switching to the a, 6a side, the predicted values of the velocity and position at time k + 1 are calculated and stored again by the method described above.

As described above, in this embodiment, not the measured value of the motor current but the predicted value of the velocity position after one sampling time is calculated from the drive signal to the constant current amplifier, and the predicted value and the measured value of the position at the next sampling time are calculated. The prediction error is calculated from and the prediction value is corrected by this prediction error to obtain the estimated value of the speed, so that an expensive ADC that directly measures the motor current is unnecessary. Further, since the predicted value of the speed is corrected by the prediction error at every sampling time, the speed can be accurately detected even in the region where the position change is gentle.

(Effects of the Invention) As described above, according to the present invention, the predicted value of the speed and position after one sampling time is calculated from the driving signal to the constant current amplifier, and the predicted value of the position and the actual measurement are performed at the next sampling time. Since the prediction error is calculated from the value and the prediction value is corrected by this prediction error to obtain the estimated value of the speed, an expensive ADC that directly measures the motor current is unnecessary. Further, since the predicted value of the speed is corrected by the prediction error at every sampling time, the speed can be accurately detected even in the region where the position change is gentle.

[Brief description of drawings]

FIG. 1 is a block diagram illustrating an embodiment of the device of the present invention, and FIG. 2 is a block diagram of a conventional speed control device. 1 ... Drive signal input line, 2 ... First coefficient unit (coefficient 1 /
2) 3 ... First addition value switcher, 4 ... First adder, 5 ... Speed accumulator, 6 ... Second addition value switcher, 7 ... Second adder, 8 ... position accumulator, 9 ... third adder, 10 ... position comparator, 11 ... second counter (counting 2), 12 ... estimated speed signal line, 13 ... estimated position signal line , 14 …… Measurement position signal line, 15 …… Constant current amplifier, 16
...... DC motor, 17 …… Rotation angle detector, 18 …… Up
Down counter, 19 ... Zero-order hold circuit, 20 ... Addition value switching signal generator, 21 ... Article gripping part, 22 ... Wire rope, 23 ... Drive pulley, 24 ... Idler pulley.

 ─────────────────────────────────────────────────── ─── Continuation of the front page (72) Inventor Takeo Oi 1015 Kamiodanaka, Nakahara-ku, Kawasaki City, Kanagawa Prefecture, Fujitsu Limited (72) Inventor Teruhide Yoshizoe 1015, Kamiodanaka, Nakahara-ku, Kawasaki, Kanagawa Prefecture Fujitsu Limited Within

Claims (1)

[Claims] 1. An article gripping section for gripping an article, a drive mechanism including a constant current amplifier and a motor for moving the article gripping section up and down or left and right, and a position of the article gripping section for measuring a motor position. In the article transfer apparatus having a position detector for detecting the speed, a means for calculating a speed prediction value after one sampling time from the drive signal to the constant current amplifier and the speed estimation value, and the speed prediction value are accumulated. Means, means for calculating a position predicted value after one sampling time from the drive signal, the speed estimated value and the position estimated value, means for accumulating the position predicted value, measuring sampling time, and the position after the one sampling time Means for calculating a position prediction error from the measurement position measured by the detector and the position prediction value,
A speed detection device for an article transfer device, comprising means for correcting the speed prediction value with the position prediction error.