POSITION/SPEED DETECTION METHOD AND APPARATUS

Abstract: A position/speed detection method and apparatus em­ ploying an encoder mounted on the rotary shaft of a motor in order to control the position and speed, wherein the coarse position Pc(K) and fine position () are simultaneously de­ tected by an encoder pulse and an encoder original signal (A,B), respectively and the detected results are combined to detect the position and speed of the motor with high preci­ sion.

Description: BACKGROUND OF THE INVENTION This invention relates to a position/speed detection apparatus in which a stable sampling time can be achieved, the sampled and held two-phase waves of an encoder original signal are pulse-width modulated, and the amplitude fluctuation of the encoder original signal is corrected. Particularly, this invention relates to a position/speed detection apparatus suitable for detecting the rotational position and speed with high precision irrespective of the amplitude fluctuation of the encoder original signal. This position/speed detection apparatus is used with drive motors and various different servo motors for NC (numeral control) apparatus, robots and so on. This invention is suitable for digital control using a microcomputer. EP-A-0145 935, relevant under Article 54(3) EPC, describes a speed detection method and an apparatus using an encoder pulse as shown in Fig. 6. Fig. 7 shows a timing chart for the speed detection. This is, a microcomputer in an arithmetic control unit is formed of a CPU 101, a ROM 102 and a RAM 103, and performs control and arithmetic operation for speed detection. The speed, V F is determined by the ratio of the number P N of encoder pulses within a constant time to the time, Td, or the following equation: V F = K₁ · P N /Td (1) where K₁ is a constant dependent on the number of encoder pulses per revolution. The encoder is generally mounted on the shaft of a moving body or rotating body, for example, a servo motor. The output signal from the encoder, or encoder pulse is formed of two-wave signals of phases A and B displaced 90° from each other as shown in Fig. 7A. An encoder pulse shaping circuit 108 shown in Fig. 6 detects the leading and trailing edges of the two- wave pulse signals and generates a T₁ signal (having 1/4 the period of the encoder pulse) as shown in Fig. 7B. A sampling timer 104 shown in Fig. 6 is connected to a data bus 111 of the microcomputer and produces a T₂ signal with a sampling time Ts for the speed detection as shown in Fig. 7C. A pulse counter 105 in Fig. 6 is connected to the data bus 111 of the microcomputer and counts the number (data) P N of encoder pulses within the sampling time Ts. The data (Fig. 7E) from the counter 105 is supplied through the data bus 111 to the microcomputer. A synchronizing circuit 107 in Fig. 6 latches the T₂ signal for sampling in response to the T₁ signal and produces a T₃ signal as shown in Fig. 7D. A time-width counter 106 as a time width timer in Fig. 6 is connected to the data bus 111 of the microcomputer as the sampling timer 104 and pulse counter 105 are connected and counts the time width of the T₃ signal from the synchronizing circuit 107. The counted data (Fig. 7F) of time interval Td is supplied through the data bus 111 to the microcomputer. In Fig. 6, 110 represents an address bus, 112 a reference clock signal line and 113 an interrupt signal line. Thus, the data of the number P N (Fig. 7E) of encoder pulses and data (Fig. 7F) of the time interval Td are arithmetically processed in accordance with Equation (1) by the microcomputer, so as to produce the speed Vp. This previously proposed speed detection method is able to detect the speed with high precision when the motor is rotated at a high speed. In this method, however, when the motor speed is reduced, it cannot be detected at each sampling time. Fig. 8 shows a timing chart for low speed. When the motor is rotated at a low speed, the sampling time Ts (Fig. 8C) may not be synchronized with the encoder pulse (Fig. 8A) (region B), or may be synchronized therewith but in this case the time interval Td (Fig. 8D) becomes very long (region A). In those cases, since the sampling time Ts is regarded as being equivalently extended, the control system has poor response. In EP-A-0 059 244 an apparatus for detecting the speed of a rotating body is disclosed which uses an angle transducer. This transducer provides an angle signal of a uniform shape which is periodically repeated. The length of the time period of said signal is determined by the distance between uniformly spaced angle positions. By counting the number of the repetitions of periods of said angle signal a coarse detection of the position of the rotating body is achieved. This result is corrected by a fine-position calculation. Thereby the value of the difference of the value of the angle signal at the begin and at the end of a sampling period is determined. In contrast to the present invention no use is made of a trigonometric relation between different waveforms provided by the transducer. In EP-A-0 059 433 the feature of bringing two signals within a speed detecting apparatus in synchronism is disclosed. This apparatus also does not apply trigonometric relationships between different signal waves derived from a transducer for calculating a fine-position of a moving body. The state of the art comprises a method and an apparatus for detecting the position and the velocity of a moving body as it is disclosed in the afore-mentioned document EP-A- 0 145 935, relevant under Article 54(3) EPC. Thereby an encoder mounted on a moving body is used. One of two of original signals of the encoder is pulse-width modulated by using a sampling carrier wave and a fine position of the encoder is detected by measuring the width of the pulses thus obtained. The apparatus for implementing the method comprises a carrier wave generation circuit, a pulse width modulation circuit which pulse-width modulates said one of the original signals coming from the encoder, a counter for measuring the pulse width of the pulse-width modulated pulses, a pulse width timer and a microcomputer. In contrast to the present invention there is no calculation for correcting the variation of the amplitudes of the encoder original signals whereas in the present invention this correction is achieved by solving a tan⁻¹ relation between actual values of the original encoder signals. SUMMARY OF THE INVENTION It is an object of this invention to provide a position/speed detection apparatus capable of precisely detecting the position and speed of a movable body by obviating the conventional drawbacks mentioned above, that is, by preventing the response of the control system from being reduced by the speed dependency of the time interval equivalent to the sampling time, to thereby have a stable sampling time, and by preventing the position and speed detection from being affected by the amplitude fluctuation of the original signal from the encoder, the change of ambient temperature and the low-speed rotation (or movement) of the movable body to be detected. To achieve this object there is provided according to the invention a position/speed detection apparatus for detecting the position and speed of a moving body having the features recited in claim 1. BRIEF DESCRIPTION OF THE DRAWINGS Fig. 1 is a block diagram of one embodiment of a position/speed detection apparatus of the invention. Fig. 2 is a timing chart for the coarse position detection. Fig. 3 is a timing chart for the fine position detection with high precision. Fig. 4 is a flow chart for the detection of position and speed. Fig. 5 is an explanatory diagram for explaining the principle of the detection of position. Fig. 6 is a block diagram of a conventional speed detection apparatus. Fig. 7 is a timing chart for the speed detection in the conventional apparatus. Fig. 8 is a timing chart for the low-speed detection in the conventional apparatus. DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of this invention will be described with reference to Figs. 1 to 5. First, the principle of this invention will be mentioned below. The position/speed detection apparatus of this invention employs an encoder connected to a moving body, and is formed of a coarse position detecting section including a waveform shaping circuit which is responsive to an original signal from the encoder to produce an encoder pulse, an up/down counter for detecting a coarse position by counting the encoder pulse and a temporary storage circuit for temporarily storing the detected coarse position in order to make it be synchronized with a sampling time from a sampling pulse timer which generates a sampling signal at each sampling time, and a fine position detecting section including a sample holder for holding the encoder original signal for the sampling time, a saw-tooth wave generating circuit which is actuated by the sampling signal from the sampling pulse timer to produce a carrier wave on which the encoder original signal held is pulse-width modulated, a pulse-width modulating circuit for modulating the encoder orignal signal, and a time-width counter for counting the pulse width, the sampling pulse timer, the temporary storage circuit and the time-width counter being connected to an arithmetic control section. The principle under which the position and speed are detected will be described below. Generally, the encoder has an original signal which changes in a sine wave, and this encoder original signal is shaped in its waveform to produce an encoder pulse. This invention employs the sine wave-shaped encoder original signal in order to detect the position and speed. A two-wave encoder original signal has an A-wave signal voltage e A and a B-wave signal voltage e B (see Fig. 2A) as expressed by e A = Esin(K E ϑE) (2) e B = -Ecos(KE ϑE) (3) where E is the amplitude of the encoder original signal, K E is a constant depending on the number of cycles of the encoder original signal per revolution of encoder and ϑ E is a predetermined fraction of an angle  indicating the fine position of the encoder. In order to correct for the fluctuation of the amplitude E and precisely detect the position from Equations (2) and (3), the amplitude E is eliminated from Equations (2) and (3) or Equations (2) and (3) are solved for the amplitude E. The elimination of the amplitude E is made by the division between Equations (2) and (3). As a result of the division, the position is expressed by the following equation:  = tan⁻¹ (-e A /e B ), ϑ E = /K E (4) where  is the fine position determined when the period of the encoder original signal is 2 π and ϑ E is a predetermined fraction of angle  indicating a fine position of the encoder position. Since Equation (4) is the reciprocal of a periodical function, the position expressed by Equation (4) has an infinite number of periods. For example, in Fig. 5 for explaining the principle of the position detection, when the A-wave signal voltage e A = B-wave signal voltage e B = d, the position  includes a great number of points ', '', ... Therefore, in addition to use of Equation (4), coarse position is simulataneously detected by using the encoder pulse having 1/2 or 1/4 the period of the encoder original signal. By the combination of these results, it is possible to precisely detect the position in a wide range. On the other hand, the determination of the amplitude E is made by adding the square of Equation (2) and the square of Equation (3). As a result, the square of the amplitude E is expressed by E² = e A ² + e B ² (5) where the theorem of sin²x + cos²x = 1 is used. Thus, the fine position is determined as From the fine position  and coarse position, it is possible to determine the position of the encoder. The circuit arrangement for executing the above method is formed of the sampling pulse timer for generating the sampling time Ts, the waveform shaping circuit for detecting the coarse position (for example, the precision of 1/4 the period of the encoder original signal), the up/down counter, the temporary storage circuit, the sample holder for detecting the fine position, the saw-tooth wave generating circuit, the modulating circuit, the time width counter and the microcomputer associated with the arithmetic control section for the arithmetic processing. The modulating circuit and the time width counter can be realized by an analog-to-digital converter. The temporary storage circuit and sample holder are used for the simultaneous generation of the coarse position data and the A-wave signal voltage and B- wave signal voltage of the encoder original signal. The speed can be determined by the amount of change of this position because the precise position data can be produced at each sampling time. Now, an embodiment of a position/speed detection method and apparatus of this invention will be described with reference to Figs. 1 to 5. Fig. 1 is a block diagram of one embodiment of a position/speed detecting apparatus of the invention, Fig. 2 is a timing chart for four-fold precision detection of position in this embodiment, Fig. 3 is a timing chart for fine position detection in this embodiment, Fig. 4 is a flow chart for the detection of position and speed in this embodiment, and Fig. 5 is an explanatory diagram useful for explaining the principle of the position detection. Like elements corresponding to those of Figs. 6 to 8 are identified by the same reference numerals. The circuit arrangement of the coarse position detecting section will be first described with reference to Fig. 1 and the timing chart of Fig. 2. A waveform shaping circuit 401 generates a thin pulse signal ( , or signal T₁ shown in Fig. 7B and Fig. 2B) at zero value of the encoder original signal and generates a signal S₂ (see Fig. 2C) for reversing the rotation of the motor as a moving body. The signal T₁ as illustrated has 1/4 the period of the encoder original signal. Thus, the precision of coarse position Pc(k) is 1/4 the period of the encoder original signal. An up/down counter 402 counts down or up depending on the signal S₂. Thus, the instantaneous coarse position Pc (see Fig. 2D) is detected by this up/down counter 402. A sampling pulse timer 404 generates a sampling signal S₃ (Fig. 2E) at each sampling time Ts. A temporary storage circuit 403 receives the sampling signal S₃ from the sampling pulse timer 404 to make the instantaneous coarse position Pc from the up/down counter 402 synchronized therewith, and stores it as the coarse position data Pc(k) (see Fig. 2F). This temporary storage circuit 403 also stores the rotation-reversing signal S₂ from the waveform shaping circuit 401 together with the coarse position Pc(k). The circuit arrangement of the fine position detecting section will be described with reference to Fig. 1 and the timing chart of Fig. 3. A saw-tooth wave generating circuit 405 is actuated by the sampling signal S₃ (see Fig. 3C) to generate a saw-tooth wave signal S₅ (Fig. 3B) as a carrier wave and a modulation permission signal S₄ (see Fig. 3D) indicative of the effective region of the saw-tooth wave signal S₅. The signal S₄ is generated at the same time as the signal S₅ and stopped when the signal S₅ reaches a reference voltage Vr. A sample holder 406 holds the values of the A-wave signal voltage e A and B-wave signal voltage e B of the encoder original signal at the time of the sampling signal S₃ during the time in which the signal S₄ is produced from the saw-tooth wave generating circuit 405, and produces the A-wave signal voltage e A as an A-wave held signal S₆ (see Fig. 3B) and the B-wave signal voltage e B as a B-wave held signal S₇ (see Fig. 3B). A modulating circuit 407 pulse-width modulates the A-wave held signal S₆ (see Fig. 3B) on the modulation permission signal S₄ and saw-tooth wave signal S₅ supplied from the saw-tooth wave generating circuit 405 and produces the A-wave pulse-width modulated signal S₈ (see Fig. 3E). Another modulating circuit 408 similar to the modulating circuit 407 pulse-width modulates the B-wave held signal S₇ on the modulation permission signal S₄ and saw-tooth wave signal S₅ supplied from the saw-tooth wave generating circuit 405 and produces a B-wave pulse-width modulated signal S₉ (see Fig. 3F). Still another modulating circuit 409 generates a reference value for correcting the effect of the change of the slope of the saw-tooth wave signal S₅ produced from the saw-tooth wave generating circuit 405 due to the change of the ambient temperature of the apparatus. That is, this modulating circuit 409 modulates the zero voltage not depending on the ambient temperature on the modulation permission signal S₄ and saw-tooth wave signal S₅ produced from the saw-tooth wave generating circuit 405 and produces a zero reference value pulse-width modulated signal S₁₀ (see Fig. 3G). A time width counter 410 counts the A-wave time width T A of the A-wave pulse-width modulated signal S₈ from the modulating circuit 407. Another time width counter 411 counts the B-wave time width T B of the B-wave pulse width modulated signal S₉ from the modulating circuit 408. Still another time width counter 412 counts the zero value time width Tc of the zero reference value pulse width modulated signal S₁₀ from the modulating circuit 409. This time width counter 412 also generates an interrupt signal S₁₁ (see Fig. 3K) and supplies it through an interrupt signal line 113 to the microcomputer CPU 101. The transmission of data from or to the microcomputer will be described below. The sampling pulse timer 404 is connected to the microcomputer through the address bus 110 and data bus 111 and thus it is controlled to set the sampling time Ts by the microcomputer. The temporary storage circuit 403 and each of the time width counters 410, 411, 412 are connected to the microcomputer through the address bus 110 and data bus 111, and thus supply their data (see Figs. 3H, 3I and 3J) coarse position Pc(k), A-wave time width T A , B-wave time width T B and zero reference value time width T C to the microcomputer. The microcomputer calculates the position and speed from the received data as will be described with reference to the timing chart of Fig. 4. The microcomputer is actuated by the interrupt signal S₁₁ from the time width counter 412, or the interrupt processing, to make reception of data and calculation. That is, at step 710 in Fig. 4 the microcomputer receives the coarse position Pc(k) from the temporary storage circuit 403, the A-wave time width T A from the time width counter 410, the B-wave time width T B from the time width counter 411 and the zero reference value time width Tc from the time width counter 412. A step 720, the fine position  of Equation (4) is calculated. At this time, first the ratio of the A-wave signal voltage e A of the encoder original signal to the B-wave signal voltage e B is obtained. In other words, if the output value of the saw-tooth wave generating circuit 405 reaches the reference voltage Vr at time zero, the value e s at a time t before time zero is given by e s = Vr - a·t (8) where a is the gradient of the saw-tooth wave signal S₅ during the period in which the modulation permission signal S₄ is produced. The A-wave signal voltage e A , B-wave signal voltage e B and zero value 0 can be determined by substituting their time width into Equation (8) as follows: e A = Vr - a·T A (9) e B = Vr - a·T B (10) 0 = Vr - a·T C (11) Substituting of the reference voltage Vr of Eq. (11) into Eqs. (9) and (10) will yield the following equations: e A = a(T C - T A ) (12) e B = a(T C - T B ) (13) The calculation of Equations (12) and (13) are made by reference to a table of numbers incorporated within the microcomputer for reducing the time to be taken in the calculation. At step 730, decision is made of whether the fine position  is a positive or negative value. If it is negative, at step 740 the constant K₃ is set to 1, and if it is positive, at step 745 the constant K₃ is set to zero. Then, at step 750, the position is detected by combining the coarse position Pc(k) and fine position . The position detection through these steps 730 to 750 will be described with reference to Fig. 5. First, the modes of the position on the encoder original signal are classed as in Fig. 5. The ratio (e A /e B ) of the A-wave signal voltage e A to B-wave signal voltage e B , of encoder original signal is expressed by the trigonometric tan function. The fine position  to be determined is positive when the position is mode 2, and the origin is mode change point M 0-2 . Then the position is mode 3, the fine position is negative, and the origin is mode change point M 3-1 . When the position is mode 1, the fine position is positive and the origin is mode change point M 3-1 . When the position is mode 0, the fine position is negative and the origin is mode change point M 0-2 . Therefore, since the coarse position Pc(k) indicates the origin of the fine position  when the position is mode 2 and mode 1, the coarse position Pc(k) and the fine position  are converted to the actual encoder position and then added to each other. Also, since the coarse position Pc(k) is coarse position 1 smaller than the origin of the fine position  when the position is mode 3 and mode σ, the origin of the fine position  which can be indicated by the sum of the coarse position Pc(k) and 1, or Pc(k) + 1, and the fine position  are converted to the actual encoder position and then added to each other. The way to calculate the position Φ(K) is thus dependent on the mode of position because it is associated with the positive and negative sign of the fine position . Thus, as in the flow chart of Fig. 4, decision is made of whether the fine position  is positive or negative, and the equation for the position Φ(K) his changed in accordance with the result of the decision. This equation is given as Φ(k) = K₂{Pc(k) + K₃} + K₄ (16) where K₂ and K₄ are constants dependent on the frequency of the encoder original signal per revolution of the encoder, and constant K₃ is zero when the fine position  is positive (or when the position is mode 2 and mode 1) and 1 when the fine position  is negative (,or when the position is mode 3 and mode 0). At step 760 in Fig. 4, the change of the position (K) at each sampling time Ts is calculated and then the speed ω(k) is detected by calculating the following equation: ω(k) = K₅{Φ(k) - Φ(k-1)} (17) where K₅ is a constant depending on the sampling time Ts. Then, at step 770 (RTI, or return from interrupt), the microcomputer returns to the main task by the interrupt process for detection of position and speed. In this way, one cycle of the detection of position and speed is finished. According to this embodiment of this invention, the position and speed can be determined with high precision irrespective of the motor speed, the amplitude fluctuation of the encoder original signal and the change of ambient temperature. While in this embodiment the variation of the encoder original signal of two periodical waves of different phases is cancelled out at step 720 in order to correct the amplitude fluctuation of the encoder original signal, the amplitude fluctuation may be corrected after summing the square of Eq. (2) and square of Eq. (3) and solving it for the amplitude E. According to this invention, the speed in a wide range can be detected at each sampling time, and particularly the response in the low-speed control which was poor in the prior art can be improved. Moreover, the still condition (,or zero speed) which could not be detected in the prior art can be detected, and the position can be detected with such a precision as 1/4 the period of the encoder original signal or below. Furthermore, the position and speed can be detected with high precision irrespective of the amplitude fluctuation of the encoder original signal and change of ambient temperature, and therefore the servo motor can be better controlled in its position and speed.

Claims: A position/speed detection apparatus for detecting the position and speed of a moving body comprising: an encoder mounted on the shaft of a moving or a rotating body for providing an encoder original signal consisting of two periodical original A- and B-waves having different phases displaced by a constant value; a waveform shaping circuit (401) for generating an encoder pulse signal (T₁) and a rotation reversing signal (S₂) indicative of the reversing of the rotation of said body; an up/down counter (402) for counting the pulses of said encoder pulse signal (T₁) for detecting a coarse position (Pc), one out of the up counter and the down counter being selected in accordance with said rotation reversing signal (S₂); a sampling pulse timer (404) for generating a sampling signal (S₃); a temporary storage circuit (403) for storing the data of said coarse position (Pc), said storing being synchronized with said sampling signal (S₃); means for detecting a fine position; a sample holder (406); whereby said temporary storage circuit (403) and said sampling pulse timer (404) are connected to an arithmetic control section (101, 102, 103) for detecting the position and speed of said moving body; wherein said sample holder (406) holds the values of the A-wave signal voltage e A and of the B-wave signal voltage e B of the encoder original signal during the time of a modulation permission signal (S₄) indicating a holding period which starts at the beginning of said sampling signal (S₃) to thereby provide an A-wave held signal (S₆) and a B-wave held signal (S₇) and a first modulating circuit (407) for pulse-width modulating the A-wave held signal (S₆) voltage to produce an A-wave pulse-width modulated signal (S₈) and a first time width counter (410) for counting an A-wave time width (T A ) which starts at a time point when a saw-tooth wave (S₅) reaches said A-wave held signal (S₆), a second modulating circuit(408) for pulse-width modulating the B-wave held signal (S₇) voltage to produce a B-wave pulse-width modulated signal (S₉) and a second time width counter (411) for counting a B-wave time width (T B ) which starts at a time point when the saw-tooth wave (S₅) reaches said B-wave held signal (S₇) and a third modulating circuit (409) for pulse-width modulating a voltage value which corresponds to a zero reference value to produce a zero reference value pulse-width modulated signal (S₁₀) and a third time width counter (412) for counting a zero reference value time width (T C ) which starts at a time point when the saw-tooth wave (S₅) reaches said zero reference value, whereby a saw-tooth wave generating circuit (405) is actuated by said sampling signal (S₃) to generate said saw-tooth wave (S₅) as a carrier wave, and said modulation permission signal (S₄), and whereby said first, second and third time width counters (410, 411, 412) are connected to said arithmetic control section (101, 102, 103) for calculating a fine position  of said moving body by using the following relations e A / e B = - tan (K E ϑ E )  = K E ϑ E  = tan⁻¹ (- e A /e B ) where E is the amplitude of the encoder original signal, K E is a constant depending on the number of cycles of the encoder original signal per revolution of the encoder, and ϑ E is a predetermined fraction of the angle  indicating the fine position of the encoder, e A , e B are the actual values of A- and B-wave signals respectively. A position/speed detection apparatus according to claim 1, wherein each of said first, second and third modulating circuits (407, 408, 409) in combination with said corresponding first, second and third time width counters (410, 411, 412) are realized by analog-to-digital converters. A position/speed detection apparatus according to claim 1, wherein the variation of the amplitude of the encoder original signal is corrected by solving the equation ­ where E is the amplitude of the encoder original signal, K E is a constant depending on the number of cycles of the encoder original signal per revolution of the encoder, ϑ E is a predetermined fraction of the angle  indicating the fine position of the encoder. A position/speed detection apparatus according to claim 3, wherein the amplitude of the encoder original signal is determined on the basis of the following relations: where E is the amplitude of the encoder original signal, K E is a constant depending on the number of cycles of the encoder original signal per revolution of the encoder, e A and e B are the instantaneous values of said A- and B-wave signals and ϑ E is a predetermined fraction of the angle  indicating the fine position of the encoder.

Inventors: MORINAGA, SHIGEKI, SUGIURA, YASUYUKI, TAKAHASHI, TADASHI, WATABE, MITSURU
Applicants: HITACHI, LTD.
Application year: 1985
Main classification: G01P3
IPC codes: G01D5, G01P3