JP2668033B2 - Moving body position / speed detection method and device and origin position return method - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Industrial field of application] The present invention uses an encoder to interpolate between encoder pulses in detecting a position / speed signal used for position / speed control of a robot or the like. The present invention relates to a method and an apparatus for increasing the resolution of a detection value at the time of sampling and reducing the detection error.

[Conventional technology]

Japanese Patent Laid-Open No. 60-216262 discloses a method and apparatus for detecting the position / speed of a moving body, which has been conventionally improved in resolution by interpolating between encoder pulses. This conventional example will be described with reference to FIGS. 42 to 44. FIG. 42 is a time chart of position detection, FIG. 43 is a flowchart of position / speed detection, and FIG. 44 is a block diagram of a position / speed detection device. The coarse position of the moving body is obtained by counting the number of pulses generated at the zero crossing position obtained by passing the encoder original signal through the waveform shaping circuit. Fine position, the 2-phase signal and the zero value signal to sample and hold at the time of sampling, and pulse width modulated by the sawtooth signal (S ₅ signal), to obtain S ₈ to S ₁₀ signal, the time width T _A through T _C Is converted into a digital value by counting with a counter, and the fine position is obtained from the following equation (1).

= Tan ^-1 _{[- (T C -T A) /} (T C -T B) ] ... (1) The position calculated as the sum of the coarse position and fine position, the method obtains the speed from the change in position and it Is disclosed.

A method of returning to the origin position of a moving body driven by a servomotor having an incremental movement amount detector is disclosed in JP-A-61-123905.
This conventional example will be described with reference to FIGS. 47 and 48.
FIG. 47 is a diagram showing a specific position device for a moving body that rotates.
FIG. 48 is a diagram showing the relationship between the regions detected during the home position returning operation of the moving body. In FIG. 47, the detection target 27 coupled to the moving body 24 has a separate origin position / overrun detection unit 92 and a region discrimination unit 93, and each unit includes four detection sensors 25a, 25b, 90, and 91. It is configured to be detected by. At the time of the home position return operation of the moving body 24, it is confirmed that the moving body 24 has not overrun as shown in FIG.
After detecting with the area discriminating sensor 91 which side is located with respect to the origin position of, it is moved without error in the direction of the origin position, and the origin position set near the true origin position is detected, and A method is disclosed in which the moving body 24 is moved in the direction of the origin, and the position where the Z phase of the encoder is detected is set as the true origin position, and the absolute position of the moving body is detected.

[Problems to be solved by the invention]

In the above prior art, since the encoder original signal and the zero-value signal are sampled and held at the time of sampling, and operations such as arctangent are performed using the values, when noise is mixed in the encoder original signal and the zero-value signal at the time of sampling, (No.
If noise is mixed to the same extent, T _C −T _A , T
Although _C fine position detection error for computing the -T _B is considered to be small, in fact it is considered an irregular noise is superposed on the signal. Therefore, when a large noise is mixed in at the time of sampling, there is a disadvantage that a fine position detection error is significantly increased.

Noise mixing of the encoder original signal etc. can be reduced to some extent by taking measures against noise such as passing through a filter. Is inevitable.

Also, in the above-described conventional technology relating to the method and apparatus for detecting the position and velocity of a moving object, coarse position and fine position detection is performed using an encoder two-phase original signal and a zero-value signal. When there is a phase shift from ゜, there has been a problem that the detection error is significantly increased. Normally, the allowable phase difference between the two-phase original encoder signals is 90 ° ± 30 °.
The detection error caused by the phase shift is shown in FIGS. 45 and 46.
This will be described with reference to the drawings. FIG. 45 shows a timing chart of position detection in the case of a phase shift of 30 ° (forward rotation phase difference 120 °, reverse rotation phase difference 60 °), and FIG. 46 shows a phase shift at the fine position 45 ° and the fine position. The relationship of the detection error is shown. The first coarse position detection error, in Figure 45, since the generation interval of the encoder pulses (T ₁ signal) is 120 ° in the corresponding encoder signal phase, 60 °, 120 °, varying 60 ° ... and 30 ° intervals Causes a one-pulse counting error. This is the direction of rotation,
The same applies when the phase shift codes are different. As for the fine position detection error, T _A, T _B, the detection value of T _C (S ₈ ~S ₁₀ signal), and significantly different from the detected values shown in FIG. 42,
A large difference also occurs in the fine position obtained as a result of the calculation of the above equation (1) assuming a phase difference of 90 °. The fine position detection error Δθ _F is represented by the following equation (2) in the case of the fine position θ _F and a phase shift.

Δθ _F is 0 when Δθ _F = 0 °, and increases as θ _F approaches 90 °. Although FIG. 46 shows the case of θ _F = 45 °, there is a maximum 25 ° fine position detection error, and the correction effect by obtaining the position as the sum of the coarse position and the fine position is insufficient.

In addition, the above-described conventional technology relating to a method of returning to the origin position of a moving body driven by a servomotor connected to an incremental encoder performs the origin position return using output signals of both an origin position detection sensor and an area determination sensor. Therefore, there was a problem that the return method became complicated.

An object of the present invention is to provide a position / speed detection method and apparatus capable of reducing a fine position detection error due to noise contamination.

It is another object of the present invention to provide a method and apparatus for reducing a position / speed detection error of a moving body when the phase difference between the encoder two-phase original signal phase shifts from 90 °. An object of the present invention is to provide a method in which the return operation is performed by a simple detection algorithm using a minimum number of sensor signals, and the phase shift can be detected without useless movement.

[Means for solving the problem]

In order to achieve the purpose of minimizing the fine position detection error due to the above-mentioned noise mixing, as a method of detecting the fine position, instead of performing A / D conversion on the encoder A-phase, B-phase signal and zero-value signal at the time of sampling, By passing each signal through a voltage-controlled oscillator, a high-frequency pulse whose frequency is variable by voltage is generated, and the total number of pulses from the time of encoder pulse generation to the time of sampling is counted (No.
In this method, the integrated value equivalent of the encoder A and B phase signals and the zero-value signal is obtained, and the fine position is calculated based on this value. As a result, even if positive and negative noises are mixed in the encoder original signal, the influence is reduced by integrating, and even if partially large noises are mixed, the ratio of the noise to the total number of pulses is small. Can be made smaller.

That is, the position / velocity detection method of a moving body according to the present invention is a sine obtained by detecting a coarse position by an encoder pulse from an encoder connected to the moving body and inputting an encoder original signal to a voltage-frequency conversion means. A fine position detecting step performed by counting the number of pulses from the time of generating the coarse position pulse using a pulse train having a wavy frequency distribution; a step of detecting the position of the moving body as the sum of the coarse position and the fine position; Detecting the speed of the moving object based on the amount of change.

Further, the present invention provides a coarse position detection step using an encoder pulse from an encoder connected to a moving object, and a coarse position detection using a pulse train having a sinusoidal frequency distribution obtained by inputting an encoder original signal to a voltage-frequency conversion means. A method for detecting the position of a moving body, comprising: a fine position detecting step of counting the number of pulses from the position pulse generation; and a step of detecting the position of the moving body as a sum of the coarse position and the fine position.

In the position / speed detection method, in order to perform interpolation between the pulse detection positions of the encoder, the number of pulse-converted pulses from the pulse detection to sampling of the encoder original signals with two 90 ° phases different from each other is counted. , A voltage-controlled oscillator with a zero value signal, from each pulse number obtained by subtracting the number of pulses obtained at the same time through a counter from each of the pulse numbers, a fine position from the encoder pulse generation position to the sampling position is calculated, It is preferable that the fine position obtained as the sum of the coarse position and the fine position is used as position information, and the speed of the moving body is detected from a change in the fine position during sampling.

In the position / velocity detection method, the encoder A and B phase original signals and the zero-value signal from the pulse detection of the encoder original signal to the first sampling (k-th sampling) can be obtained through the voltage-controlled oscillator and the counter. Each pulse number N _A ^(k) , N _B ^(k) , N _O ^(k) is stored. Unless the pulse of the encoder original signal is detected until the (k + n ⁾ -th sampling, each detection at the (k + i) -th sampling (1 in) The number of pulses The fine position from the pulse detection position of the encoder original signal to the time of sampling is calculated, and the fine position obtained as the sum of the coarse position and the fine position is used as position information, and the speed is calculated from the change in position at the time of sampling. What should be detected is good.

In the position / velocity detection method, when the position is detected, a 1/4 value determined from the positive / negative relationship of each of the encoder two-phase original signals is used.
Coarse detection based on the direction discrimination signal obtained when the periodic position is detected and the encoder 2 phase original signal is pulsed in the moving direction is combined with the fine detection by the sign detection of the fine position change amount at the encoder pulse generation interval. Things are good.

In addition, the position / speed detection device of the moving body according to the present invention,
In a position / speed detection device using an encoder connected to a moving body, a waveform shaping circuit that generates an encoder pulse and a movement direction discrimination signal from an encoder original signal, and an up / down counter that detects a coarse position by the encoder pulse, A coarse position detection unit having a temporary storage circuit for synchronizing the coarse position with the sampling time from a sampling pulse timer for generating a sampling signal every time sampling is performed, and an encoder original signal and a zero value signal as inputs. The encoder pulse and the sampling pulse serve as a gate signal, and a fine position detector including a counter for starting and ending counting of three types of pulse trains sent from the voltage-frequency conversion means, and an encoder two-phase original signal between encoder pulses A 1/4 period position detection circuit determined by the positive / negative relationship of Position detection unit, a fine position detector and
A central processing unit that controls the operation of the 1/4 cycle position detection circuit,
It is provided with.

In the position / velocity detection device, the voltage-frequency conversion means of the fine position detection unit includes a bias voltage application circuit that generates a biased output voltage, and a pulse train having a frequency that varies depending on the input voltage with the output of the bias voltage application circuit as an input. And a voltage-controlled oscillator for generating the pulse train. Preferably, the three types of pulse trains are sent from the voltage-controlled oscillator.

Further, the voltage controlled oscillator is configured by a CR oscillation circuit having a voltage variable resistor, and each configuration of the bias voltage application circuit, the voltage variable resistor, etc. so that the oscillation frequency of the voltage controlled oscillator has a temporary functional relationship with the input voltage. What constituted the element is good. Here, it is preferable that the variable voltage resistor is composed of a magnetic material whose voltage application side is wound by a coil, and a magnetoresistive element having both ends thereof in contact with each other in a minute gap and having an output terminal. In addition, multiple types of resistors or capacitors other than voltage variable resistors are provided in parallel, a frequency divider is provided at the output stage, and a specific resistor or capacitor and a specific frequency divider are provided according to the signal from the central processing unit. It is preferable that the ratio is selectable. Further, it is preferable that the voltage controlled oscillator includes means for obtaining a specific oscillation frequency variable width by selecting a specific resistance or capacitor and a specific frequency division ratio of the frequency divider according to the required resolution. In the position / speed detection device, it is preferable that the position / speed detection device of the moving body is connected to a motor control device of a motor for driving the moving body, and a program related to a motor control method is provided in the central processing unit. Here, it is preferable that the motor driven by the motor control device is for driving a robot joint, and the encoder for position detection is coupled to the motor or the driven joint.

In the position / velocity detecting device, the A-phase counter, the B-phase counter, and the zero-value counter are each constituted by a pair of counters, and when one of the counters finishes counting, the other counter starts counting. It is better to have means.

Next, in order to achieve the purpose of reducing the detection error when there is a phase shift, for the position and detection of the moving body, the pulse generation interval of the encoder is detected by the timer in the constant speed operation section of the moving body. By detecting the phase difference of the encoder two-phase original signal phase difference from 90 °, the relationship between the encoder original signal, the quarter cycle position, and the moving direction is stored. By detecting the quarter period position of the signal, two pulses (1
The coarse position is detected by counting every
The fine position (interpolation amount) is obtained by performing arctangent calculation based on the encoder two-phase original signal, the zero value signal, the moving direction, and the phase shift, and the position is obtained as the sum of the coarse position and the fine position. It is.

That is, in the method for detecting the position and speed of the moving body according to the present invention, when the position and speed at a certain time are obtained by the encoder connected to the moving body,
The coarse position is detected by counting the encoder pulses generated at the time when the phase original signal crosses the zero value, and the fine position (interpolation amount) between the encoder pulses is detected based on the encoder two-phase signal and the zero value signal. In the method of detecting the position and speed of a moving body, which detects a position as a sum of a coarse position and a fine position and detects a speed based on a change amount of the position, a time interval of an encoder pulse is measured for a constant speed operation of the moving body. In addition, the quarter period position q of the encoder determined by the sign of the encoder two-phase original signal at that interval (numbers are assigned to each moving direction so that 0 → 1 → 2 → 3 → 0 for both forward and reverse rotations) And the moving direction S (normal rotation: 1, reverse rotation: -1) is detected, and encoder pulse intervals _{tq = 0} and _{tq = 1} having specific continuous q values (for example, q = 0, 1) are obtained from the following equation. 2 phase original signal of encoder Detecting a phase shift of 90 ° -
Remember, Next, a quarter cycle position q of the encoder two-phase original signal immediately before and immediately after the change in the moving direction during the normal operation of the moving body
₁ , q ₂ ｛However, the q value at the start of normal operation is assumed to be q ₂ and the integer x
If the remainder of the integer y is expressed as mod _y (x), q ₁ = mod ₄
(Q ₂ -1)} and a quarter period position q at the time of sampling are detected, and two quarters from the reference quarter period position (eg, q = 0) are detected.
A method of correcting the count value so as to count the coarse position for each pulse is employed, and the encoder pulse count value P ₀ ,
Rotation angle / encoder pulse number = k ₂ , movement direction S ₁ , S ₂ just before and after change in movement direction (forward: 1, reverse: −1, but when normal operation starts, S ₂ : movement direction, S ₁ : The direction opposite to the moving direction), the rough position θ _R is calculated by the following equation. (I) θ _R = k ₂ ｛P ₀ + S from the time when the moving direction changes until mod ₂ (q) = 1 ₁ · mod ₂ (q ₂ )｝ (ii) After mod ₂ (q) = 1 after changing the moving direction θ _R = k ₂ ｛P ₀ −S ₂ · mod ₂ (q)｝ Fine position θ _F Is the arctangent based on the encoder two-phase original signal, zero value signal, moving direction, quarter period position and phase shift so as to interpolate the range of two encoder pulses (180 ° phase of the encoder original signal). operation (e.g., an encoder 2 Aihara signal instantaneous value e _a the time of sampling, e _B, zero value signal instantaneous value e _C, out of phase, and a moving direction S, for example, in the formula, S ,, e _A , e _B , e _C are given to obtain θ _F ), and the position θ is detected as the sum of the coarse position θ _R and the fine position θ _F.

At the position / velocity detection position, the initial operation of the moving body (when using an incremental encoder, return to origin and move to the normal operation start position; when using an absolute address encoder, move to the normal operation start position) It is preferable to measure the encoder pulse interval by using the constant velocity operation section during (movement of) and detect the phase shift of the encoder 2-phase original signal phase difference from 90 °. Further, it is preferable that the detection of the moving direction of the moving body is performed based on the rough detection obtained when the encoder original signal is pulsed and the fine detection detected from the sign change of the change amount of the fine position between the encoder pulses. In addition, the encoder pulse interval, 1/4 cycle position, and moving direction are detected and stored in the constant velocity section during normal operation of the moving object, and the phase shift from the encoder 2-phase original signal phase difference of 90 ° is calculated. Calculate the arc tangent based on the phase shift detected immediately before sampling at the time of sampling,
What detects a fine position is good. In addition, when detecting a fine position, the sine wave encoder two-phase original signal and zero value signal are passed through a voltage controlled oscillator to be converted into a pulse train having a sine wave frequency distribution, and the number of pulses from the encoder pulse detection to sampling is calculated. It is preferable to detect the fine position by counting and performing arctangent calculation from the value corresponding to the two-phase sinusoidal wave integral amount from the encoder pulse detection time to the sampling time. In addition, when detecting the fine position, the encoder two-phase original signal and zero value signal are sampled and held during sampling, A / D converted, and the arctangent calculation is performed from the two-phase sine wave instantaneous value at sampling to detect the fine position. What you do is good.

In addition, the position / speed detection device of the moving body according to the present invention,
In a position / speed detecting device for a moving body using an encoder connected to the moving body, a timer for detecting a pulse interval of the encoder, a 1/4 cycle position detecting circuit, and each moving direction S
For (forward rotation 1, reverse rotation -1), the phase difference (0) of the phase difference of the encoder two-phase original signal from 90 ° and the two-phase original signal instantaneous value ratio or integral value ratio from the quarter period position q Arc tangent data corresponding to ｛For example, when q = 0, S · sin θ _F
It is characterized by having a ROM in which data capable of searching the fine position θ _F from / cos (θ _F +)} is stored. Here, a waveform shaping circuit for generating an encoder pulse and a moving direction discrimination signal from an encoder two-phase original signal, an up / down counter for detecting a coarse position by the encoder pulse, and a sampling signal for generating the coarse position for each sampling time A coarse position detection unit having a temporary storage circuit for synchronizing with the sampling time from a sampling pulse timer to be applied, and applying a bias voltage for generating an output voltage biased with an encoder two-phase original signal and a zero-value signal as inputs. Circuit, a voltage-controlled oscillator that receives the output of the bias voltage applying circuit as input, and generates a pulse train of a different frequency depending on the input power, and counts three types of pulse trains sent from the voltage-controlled oscillator, with the encoder pulse and the sampling pulse as gate signals. Start
It is preferable to include a fine position detecting unit having a counter for ending and a central processing unit for controlling the operation of each detecting unit.

In the apparatus, a waveform shaping circuit for generating an encoder pulse and a moving direction discrimination signal from an encoder two-phase original signal, an up / down counter for detecting a coarse position by the encoder pulse, and sampling the coarse position for each sampling time A coarse position detection unit having a temporary storage circuit for synchronizing with a sampling time from a sampling pulse timer for generating a signal; a bias voltage application circuit for applying a bias voltage to an encoder two-phase original signal and a zero-value signal; A fine position detection unit including a sample holder for sampling and holding each signal in synchronization with a sampling pulse, an A / D converter for A / D converting the output of the sample holder, and operation control of each detection unit And a central processing unit for performing the above.

Further, in the device, the encoder connected to the moving body is configured by an incremental system, and a limit position detecting unit that is detected only at a movement limit position of the moving body and an origin position near a true origin position are detected. A step-like origin position detecting section, which is located at both ends of the moving limit of the moving body, with the detected body being driven integrally with the moving body while maintaining the step in the movable range of the origin position in each moving direction. And a limit position detection sensor for detecting a limit position detection unit of the object to be detected, and an origin position detection unit of the object to be detected which is disposed at the origin position near the true origin position or is at the origin position. An origin position detection sensor that detects which region is on both sides of the position, a collision member coupled to the moving body,
It is preferable that the movable body is provided with a stopper that comes into contact with the collision member and stops the movement of the movable body when the movable body moves beyond the movement limit position.

In the above device, it is preferable that the position / speed detecting device of the moving body is connected to a motor control device of a motor for driving the moving body, and a program relating to a motor control method is provided in the central processing unit. Here, it is preferable that the motor driven by the motor control device is for driving a robot joint, and the encoder for position detection is coupled to the motor or the driven joint.

As for the method of returning the origin position of the moving body, the detected body having the origin position detecting unit coupled to the moving body has a step-like step at the origin position, and the step is made within the operating range in both moving directions. With this structure, the origin position detection sensor can discriminate the origin position and the area of the moving direction relative to the origin position, and the origin return operation algorithm can be simplified. did. Also,
By detecting the phase shift during the initial operation of the moving body such as the present origin returning method, unnecessary movement of the moving body can be omitted.

That is, the method for detecting the position and speed of a moving object according to the present invention is a method for returning to the origin position of a moving object driven by a servomotor having an incremental encoder. To detect which side it is on, move the moving body in the direction of the origin, detect the phase shift during the initial operation in the constant velocity section, and determine the origin located near the true origin. The moving body is moved in the direction in which the encoder Z phase (true origin position) is detected with respect to the origin position after detection by the detection sensor, and the movement of the moving body is stopped when the encoder Z phase is detected. Things.

[Action]

The encoder connected to the moving body is provided with graduations at regular intervals in its moving part, and by irradiating light to the graduations, the movement of the graduation is detected by a photodiode (optical type) or the graduation is measured by a magnetic sensor. By detecting the movement (magnetic type), two-phase sinusoidal encoder original signals (A-phase and B-phase) having a 90 ° phase difference can be obtained. The phase lead / lag between A and B depends on the moving direction of the moving body.

This position / speed detection device converts the encoder original signal into a pulse train at its zero point position by a waveform shaping circuit,
The coarse position is calculated by counting with a counter. Also,
In order to interpolate between pulses, the encoder original signal is biased and connected to a voltage variable resistor, so that it can be used in a voltage region where the applied voltage of the voltage variable resistor and the resistance are in inverse proportion. By oscillating an oscillator having a voltage variable resistor having such characteristics, the applied voltage at the encoder original signal input terminal and the oscillator oscillation frequency have a linear function relationship.

Thus, by counting the number of oscillator oscillation pulses from the time of encoder pulse generation to the time of sampling, an amount corresponding to the integral amount of the encoder original signal at the same time can be obtained. ) Can be asked.

If no encoder pulse is generated during the sampling period, the number of integrated pulses is calculated from the previous generation of the encoder pulse, and the amount of interpolation (fine position) can be similarly calculated by performing a trigonometric function operation.

A precise position can be obtained by adding the coarse position and the fine position described above. Since the above-mentioned oscillator oscillation pulse count value shows a large value, even when a partially large noise is superimposed on the encoder original signal, a high-resolution position / speed detection which is not easily affected by the noise can be performed.

In addition, the encoder connected to the moving body is provided with a detection target grid at regular intervals, and when the moving body moves, a two-phase sinusoidal original signal centered on zero volt having a phase difference of about 90 ° is generated. Occur. Here, the sign of the phase difference differs depending on the moving direction. The encoder two-phase original signal is passed through a waveform shaping circuit to determine the zero-crossing position (approximately 1 /
A pulse is generated every four cycles), and a moving direction discrimination signal is generated. As the moving speed increases, the period of the sine wave becomes shorter, so that the number of pulses generated in a certain time increases. The position and speed of the moving object can be detected to some extent accurately only by counting the number of pulses during high-speed operation, but the number of pulses generated within the sampling period decreases during low-speed operation Therefore, it becomes necessary to perform interpolation between pulses. In the method of obtaining the arc tangent based on the sinusoidal encoder two-phase original signal to obtain the interpolation position, it is possible to obtain the interpolation position with high resolution by increasing the detection resolution. This method has an advantage in that the arc tangent operation is performed from the amount corresponding to the quotient of the difference between the two-phase signal and the zero signal at the time of sampling, so that the method is less susceptible to amplitude fluctuation and zero point drift of the encoder two-phase signal. However, when there is a phase shift from 90 ° in the phase difference of the encoder two-phase original signal, the arc tangent calculation assuming the 90 ° phase difference is performed, so that there is a disadvantage that a detection error increases.

The point focused on in the present invention is that the encoder pulse generated almost every quarter cycle is counted at every other pulse even if there is the phase shift, and that the interval between two pulses is 180 °, Coarse position detection based on encoder pulses is performed by counting every two pulses from a quarter period position of a certain reference, and fine position detection for 180 ° is detected during the initial operation by detecting the above phase shift, during normal operation sampling The tangent is calculated by taking into account the phase shift based on the 1/4 cycle position, the phase shift, the moving direction, the encoder two-phase original signal and the zero value signal. As a result, the phase shift of the encoder two-phase original signal phase difference from 90 ° is corrected, and an accurate position can be detected even during low-speed operation.

In addition, in the method of returning the origin position of the moving body, the origin position / area discriminating detection target combined with the moving body has a step-like step at the origin position, and the step is maintained within the movable range in each moving direction. With this, the origin position detection sensor can detect which side the moving body is with respect to the origin position based on the signal level of the initial position, and detect the origin position with the step-shaped step portion. Therefore, it is not necessary to separately provide an area determination sensor, and the algorithm for returning to the original position can be simplified. In addition, during the home position return operation, 90
By detecting the phase shift from ゜, the detection of the position and speed of the movement can be performed without unnecessary use of the moving body.

〔Example〕

Hereinafter, a first embodiment of the present invention will be described with reference to FIGS.
This will be described with reference to FIGS. FIG. 1 shows the position of the present invention.
FIG. 2 shows a time chart of speed detection, FIG. 2 shows a block diagram of a position / speed detection device of the present invention (the broken line part is used in the second embodiment), and FIG. FIG. 4 shows a flow chart of a position / speed detection method of the present invention, FIG. 5 shows a circuit diagram of a voltage controlled oscillator of the present embodiment, FIG. 6 shows an example of the bias voltage applying circuit and the voltage variable resistor of the present invention, and FIG. 7 shows the voltage applied to the point A when the voltage controlled oscillator of FIG. 5 is applied to the position / speed detecting device of FIG. And the oscillation frequency of the voltage controlled oscillator,
The figure shows the relationship between the resistance value of the voltage variable resistor shown in FIG. 6 and the input voltage. FIGS. 11 and 12 show the relationship between the oscillation frequency of the voltage controlled oscillator shown in FIG. 5 and the characteristic value of the passive element. Shown,
FIG. 13 shows a time chart of position detection when the sampling cycle is smaller than the encoder pulse generation cycle.

First, FIG. 2 and FIG.
This will be described with reference to the drawings. The two phases generated by the encoder connected to the moving body are such that the original encoder signal is the coarse position detector 36,
It is sent to the fine position detection unit 37. The coarse position detector 36 is composed of a waveform shaping circuit 1 and an up / down counter 2. The waveform shaping circuit 1 performs waveform shaping, pulse generation at the zero crossing of encoders A and B phases (T ₁ signal), and movement direction discrimination signal generation (S ₂ signal) of the moving body. In the up / down counter 2, since the T ₁ signal is input in two channels for each moving direction, the number of forward moving pulses and the number of backward moving pulses are counted and the sampling pulse generated by the sampling pulse timer 6 is counted. It is latched in the temporary storage circuit 3 in synchronization with the pulse.

The fine position detector 37 is composed of a bias voltage applying circuit 4, a voltage controlled oscillator 5, an A phase counter 7, a B phase counter 8 and a zero value counter 9. When the encoder original signal is sent to the fine position detection unit 37, the voltage waveform is firstly biased by the bias voltage application circuit 4. This has a function that the voltage variable resistor provided in the voltage controlled oscillator 5 sets a zero value at the center of the applied voltage level at which the applied voltage-resistance value shows a hyperbolic characteristic. The voltage controlled oscillator 5 is composed of a CR oscillation circuit including a voltage variable resistor as a resistance element, and the applied voltage of the voltage variable resistor and the oscillation frequency are set to have a linear function relationship. Therefore, when the sinusoidal encoder original signal is input to the fine position detector 37, the A and B phase bias voltage signals are converted into a pulse train having a sinusoidal frequency distribution, and the zero-value bias voltage signal has a uniform frequency. It is converted into a pulse train (see FIG. 1). The A, B-phase and zero-value counters are reset by the T ₁ signal to start counting, and are latched in the temporary memory circuit by the sampling pulse, whereby the counter output shown by the shaded area in the center of FIG. 1 is obtained. Count values are between pulses of T ₁ signals not sampled is reset without being stored. In addition the encoder pulses per direction of movement (T ₁ signal) generation section, the encoder 2
The positive / negative relationship of the phase original signal is constant, and the section position numbered by the positive / negative relationship is called a quarter period position. The 1/4 cycle position is numbered as 0 → 1 → 2 → 3 → 0 in each moving direction as shown in FIG. 3, and is detected by the 1/4 cycle position detection circuit 15. The coarse position detection unit 36, the CPU 12 for controlling the fine position detection unit 37 and the like at a higher level, the ROM 13 having the arctangent table and the like, the RAM 14 having the program area and the like are connected via the data bus 10 and the address bus 11. I have.

Next, the configuration and operation of the voltage controlled oscillator 5 will be described with reference to FIGS. 5 to 8, 11 and 12. FIG. 5 shows an example of a circuit configuration of the voltage controlled oscillator 5. This oscillation circuit has resistors 18-20, capacitor 12, inverter elements 22-24
The inverter elements 22 and 23 act as a DC power supply, and each time the potential on the input side of the inverter elements 22 and 23 accompanying charging and discharging of the capacitor 21 exceeds the threshold voltage value on each side, the inverter elements 22 and 23 The operation principle is based on the fact that the signal level on the input / output side is inverted. The inverter element 24 plays a role of waveform shaping. When a transistor / transistor / logic circuit 42S04 having an extremely short signal rise time is used as an inverter element, R ₁
+ R ₃ (Ω), R ₂ (Ω), and C ( _F ) were made variable, and the following equation was obtained between the oscillation frequency f ( _Hz ) and the above passive element value in a region away from the vicinity of the oscillation limit. It was found that the relationship of (3) was established (see FIGS. 11 and 12).

The constant in the equation (3) is a value determined from various constants such as the characteristic value and resistance value of the transistor in the transistor, the transistor, and the logic circuit, and it is considered that it changes when other inverter elements are used. The qualitative relationship between the element characteristic values is considered to be a sufficiently expressed equation, and the oscillation frequency of the present oscillation circuit is generally considered to be expressed by the following equation (4). (However, α, β, γ are positive constants) Also, as can be seen from FIGS. 11 and 12, R ₁ + R ₃ , R ₂
Is increased, the oscillation frequency f decreases, but there is an oscillation limit determined by the input / output terminal allowable current of the inverter elements 22 and 23, and the variable range of the oscillation frequency f is limited.

In FIG. 5, since R ₁ is a voltage variable resistor, R ₁
Is inversely proportional to the voltage V applied to the voltage variable resistance as shown in the following equation (5), and if R ₀₀ + R ₃ = γ, R _{2 is} constant, the above equation (4)
Has the form of the following equation (6), and a linear function relationship is established between the oscillation frequency f and the applied voltage V.

R ₁ = δ / (V + ε) + R ₀₀ (5) A magnetoresistive element made of, for example, a FeNi alloy as a material having an inversely proportional relationship between the resistance value and the applied voltage is constructed by applying a voltage to the coil on the primary side to form an electromagnet as shown in FIG. Is obtained, the hyperbolic characteristic can be approximated when the coil applied voltage V is within the range of V _α ≦ V ≦ V _β . A bias voltage is applied by, for example, a bias voltage application circuit as shown in FIG. 6 so that the intermediate value V ₀₀ of V _α and V _β becomes the bias voltage, and the coil application voltage is applied when the maximum and minimum voltages of the encoder original signal are applied. V _α
If set to be less than V _beta, the encoder original signal voltage e _j and the voltage variable resistor applied voltage V, the encoder original signal voltage
The following equations (7) and (8) are established between e _j and the oscillation frequency f.

f = ξ'ηe _j + ξ'V ₀₀ + ζ '= ξe _j + ξ (ξ', ζ ': constant value) (8) Therefore, a linear function relationship between the encoder original signal voltage and the oscillation frequency shown in FIG. Is obtained.

Next, the position / speed detection method of this embodiment will be described with reference to FIGS. 4 and 13.

First, a coarse position detection method will be described. Positive direction latched in the temporary storage circuit in synchronization with the sampling pulse,
The coarse position θR (k) is calculated by the following equation (9) from the reverse movement positions θup (h) and θdwn (k) (accurately stored in the form of the number of pulses, but treated as a position conversion amount). .

θR (k) = θup (h) −θdwn (k) (9) The positive direction movement amount has a positive sign, and the reverse direction movement amount has a negative sign.
When the encoder A and B phase original signals e _A and e _B are expressed by the following equation (10), the coarse position resolution is π / 2K _E (the number of graduations per encoder rotation is K _E ).

Here, the phase A is 90 ° phase lag with respect to the phase B, and the phase lag is 90 ° phase lead in the forward direction.

Next, a fine position detecting method will be described. As shown in FIG. 1, the A and B phase counter outputs indicate different counter outputs f (N _j ) depending on the moving direction and the quarter period position. This corresponds to the result of integrating the output of the counter having the biased sine wave frequency distribution from the 0 °, 90 °, 180 °, and 270 ° phases of the sine wave to an arbitrary position θ. Therefore,
(8) Equation (10) is used to express the frequency distribution of each section of encoder A, B phase and zero value, and the count value is calculated by the following equations (11) and (12).
It was shown to.

FIG. 1 shows an example in which the sampling period is longer than the pulse generation period of the encoder. In the figure, hatched portions indicate count values at the time of each sampling by area. In this case, if the encoder pulse does not enter within the sampling period, the sampling period is calculated by using the _count value accumulated from the time when the encoder pulse enters as the _Nj value as shown in equation (13). It can be treated as if it were longer than the period.

At the time of k-th to k + 4 sampling Further, due to the fact that crude detected by the direction discrimination signal depending upon pulsing the encoder original signal (S ₂ signal) also moving direction, to detect the presence or absence of a change in sign of the fine positional variation in encoder pulse generation interval If the fine detection is used together, it is possible to accurately grasp the time when the moving direction changes in FIG. 1 and to accurately detect the position when the moving direction changes.

Based on equation (12), equations shown in Table 1 is obtained when obtaining the fine position theta _F from i / 2 [pi position (coarse position pulse generation positions). The calculation of this formula is performed with reference to an arc tangent table provided in the ROM 13.

From the above, the coarse position θR (h) and the fine position θF (k) can be obtained, and the fine position θ (k) at the time of sampling can be obtained.
Is obtained as θ (k) = θR (h) + θF (k) (14) Even when noise is mixed in the encoder original signal, the counting error Δ of each of N _A , N _B , and N _O due to the noise mixing
Since N _A , ΔN _B , ΔN _O << N _A , N _B , N _O (see FIG. 31), the influence on the calculation / judgment results shown in Table 1 is small and the fine position detection error can be reduced.

The velocity ω (k) is the precise position θ at the time of the previous sampling.
(K-1) and fine position of the current time of sampling theta (k), is determined by the equation (15) when a counting K ₅ which is inversely proportional to the sampling period.

ω (k) = K ₅ (θ (k) −θ (k−1)) (15) According to this method, high-speed detection error due to noise mixing can be reduced.

Next, a second embodiment of the present invention will be described with reference to FIG. 2, FIG. 4, FIG. 9 and FIG. FIG. 9 is a circuit diagram of a voltage-controlled oscillator in the position / speed detection device of the second embodiment of the present invention, and FIG. 10 is an oscillator frequency variable when the voltage-controlled oscillator shown in FIG. 9 is applied to the device. FIG. 4 is a diagram illustrating a relationship between an axis and a variable resistance value. In the position / speed detection device shown in the first embodiment, since the oscillation frequency variable range is fixed as shown in FIG. 7, the resolution cannot be changed according to the application (required resolution). This solution is possible by changing the oscillation frequency variable width when the maximum and minimum voltages of the encoder original signal are applied.
The variable width changes when the oscillation frequency is changed. Assuming that the amplitude of both the frequency variable width Δf and the voltage applied to the voltage variable resistor is ΔV, Δf = ξ ′ (R ₂ ) ΔV according to equation (6). If R _{2 is} large, ξ ′ is small, and if R _{2 is} large, Δf is Δf. It will be small. Ninth
Since the voltage controlled oscillator shown in the figure has a relationship between the oscillation frequency and the characteristic value of the passive element shown in the equation (4), the resistance value R ₂
Alternatively, if the capacitance of the capacitor 21 is made variable, the oscillation frequency and its variable width can be changed. Here, in parallel a resistor having a different resistance value of a plurality of types order as the resistance R ₂ in the temporary functional relationship with the oscillation frequency, the multiplexer 28 sends a signal from the CPU 12, enables the setting according to the use and the structure The device is shown. Further, as shown in FIGS. 11 and 12, the oscillation circuit has an oscillation limit in the low frequency range, and therefore there is a limit in increasing R ₂ to lower the oscillation frequency. Therefore, the oscillation frequency of the oscillation circuit is kept high, a frequency divider 30 for reducing the oscillation frequency and its variable width is provided in the output stage, and the frequency division ratio (normally) can be switched by the multiplexer 29, so that the voltage An oscillation pulse (low resolution pulse) having a stable low frequency and a small frequency variable width can be obtained from the controlled oscillator.

FIG. 10 shows the relationship between the oscillation frequency variable width and the resistance R ₂ when the oscillation circuit configuration of FIG. 9 is used, and stable pulse oscillation and variable width change are possible over a wide frequency range. 2 and 4 show portions added to the position / speed detecting device and method when the present embodiment is applied. When this embodiment is used, the oscillation frequency variable width of the voltage controlled oscillator can be expanded and the optimum resolution according to the application can be obtained with almost no increase in complexity in both hardware and software. Although FIG. 9 shows the method of selecting one from a plurality of types as the resistance R ₂ , the voltage variable resistance R that can change the resistance value almost continuously from the CPU 12 via the D / A converter is used. _The use of ₂ enables finer setting of the oscillation frequency variable width.

Next, the detailed configuration of the counters 7 to 9 will be described with reference to FIGS.
This will be described with reference to FIG. The position must be detected by the counter from the time when the encoder coarse position pulse is generated to the time when the sampling pulse is generated when the moving body moves at high speed, and is detected at the sampling pulse generation interval when moving at low speed. Therefore, when an encoder pulse or a sampling pulse is input, it is necessary to clear or latch the counter, and it is necessary to continue pulse counting during the clear and latch times. Therefore, as shown in FIG. 14, each of the counters 7 to 9 is composed of a counter a and a counter counter b, and when an encoder pulse signal or a sampling pulse signal is input, the counter and counter counter are switched to detect the position. It is configured to perform. The latch circuit 38 is also configured so that a counting start signal (S signal) of the counter is also taken in by the latch circuit 38 so that the central processing unit determines which counter value to read out. FIG. 15 is a time chart showing the on / off operation of the counter and the counter when the sampling pulse signal or the encoder pulse is input. Both are never turned on, and counting is always performed on one side.

Next, a third embodiment of the present invention will be described with reference to FIGS.
This will be described with reference to FIG. FIG. 16 is a block diagram of a motor control device when the position / speed detection device of the present invention is applied to a control device of a motor that drives a moving body for detecting position / speed, and FIG. It is a figure which shows the structural example (horizontal articulated direct drive robot) of a mobile body when the mobile body whose position and speed are detected by a speed detection apparatus is a robot arm and a robot wrist axis.

FIG. 16 shows an example in which the position / speed detection device of the present invention is applied to a motor control device to perform position control. Up to a portion that gives a torque command to the motor control device 17 is configured by software servo, and when the position / speed of the moving body or the motor is detected at each sampling cycle, the movement locus of the moving body 39 generated in advance is determined. To achieve this, position deviation and speed deviation are detected, the dead zone of the motor,
A torque command in consideration of the torque limit is generated, and a torque current is supplied from the motor control device 17 to the motor 40.
FIG. 4 shows the flow of this operation. The torque command sent to the motor control device 17 until the moving body 39 reaches the target position is rewritten every sampling cycle, and when the position deviation with respect to the target position becomes zero. A torque command that causes the motor to generate only a holding torque for an external force (for example, gravity) on the moving body is generated. Software servo for motor control
The calculation function of 42 can also be provided in the central processing unit of the position / speed detection device 43, and various parameter values required for motor control can be set by software without changing hardware. It has the advantage of being feasible.

Next, as an example to which the present position / speed detection device is applied,
An example in which the control target is a horizontal articulated eight robot will be described with reference to FIG. In this robot, the first arm 50 and the second arm 57 are rotationally driven by the outer rotor type direct drive motors 44 and 51, and the second arm has a working tool at the tip thereof, and a wrist shaft (spline shaft) capable of vertical and rotational movements. ) 71 are provided. Regarding the vertical movement of the wrist shaft 71, the rotational power is transmitted from the motor 58 via the belt 63 to the ball screw 65.
The spline shaft 71 is rotatably mounted on the bracket 67 coupled to the ball screw nut portion in a vertical direction and is supported in the radial direction by a rolling bearing or the like. When the ball screw 65 is rotated, it moves vertically by the pitch of the screw in a state where it is prevented from rotating.

In addition, the rotation operation of the wrist shaft 71 is performed by the motor
Belt 63, spline bearing mounting member 69, spline bearing
The power is transmitted to the spline shaft 71 fitted with the spline via 70.

The main shaft and the wrist shaft are provided with an encoder of an incremental system, and a relative rotation number and a rotation angle from a reference position of the encoder are detected. In addition, detection sensors 47 and 54 for returning to the reference position and preventing overrun are provided.
(Only a part is shown) and detected objects 46, 53, 66, and 68 are provided, and stoppers 49, 56, and 72 for preventing the operation from going too far after the overrun is detected, and a collision member 48 contacted by the moving body. , 55, 73 are provided.

The direct drive motors 44 and 51 for rotating the first arm 50 and the second arm 57 do not have a speed reducer on the motor output shaft. In order to obtain the same position resolution as compared with the case of performing the above, it is required to have a high resolution that is double the reduction ratio.
In that respect, the position / detection method described in the present invention is effective because it has high resolution and variable resolution depending on the application.

Hereinafter, a first embodiment of the present invention for reducing the detection error due to the phase shift will be described with reference to FIGS. 18, 29 and 3. In the present embodiment, an example will be described in which a moving body and an encoder are coupled to a motor, and a sensor signal such as an encoder signal is taken into a position / speed detection device and position / speed detection is performed. FIG. 18 is a time chart of position / velocity detection at the time of normal operation of the moving object of the present embodiment, and FIG. 19 is a time chart particularly showing phase shift detection in the whole operation of the moving object using the incremental encoder. FIG. 20, FIG. 20 is a time chart showing the phase shift detection in particular of the overall operation of the moving body using the absolute address type encoder, FIG. 21 is a block diagram of the moving body position / speed detecting device, FIG. Is a flowchart of an initial operation (origin return operation) when an incremental encoder is used, FIG. 23 is a flowchart of an initial operation when an absolute address encoder is used, and FIG. 24 is a normal operation of the moving body. Fig. 25 is a flowchart showing the position / speed detection method at the time, Fig. 25 is a block diagram of the position / speed control device for the moving body, and Fig. 26 is equipped with an incremental encoder. Longitudinal sectional view of a device for rotating the arm-like movable body by abutting the drive motor, 27
FIG. 26 is a cross-sectional view taken along the line XXVII-XXVII of FIG. 26, FIG. 28 is a diagram showing each area of the rotary drive device shown in FIG. 26 and FIG. 27, and a method of returning to the origin position. It is a figure which shows the structure of the arctangent data table used for the shown fine position detection.

In order to reduce the position detection error when the phase difference between the encoder two-phase original signal phase shifts from 90 °, the phase shift must be detected. Encoder 2 phase original signal phase difference
In order to detect the phase shift from 90 ° without causing the moving body to perform a useless operation, the present invention detects the phase shift at the time of the initial operation of the moving body. Encoders include an incremental encoder that can detect a relative position and an absolute address encoder that can always detect an absolute position. When detecting the position of the moving body using the incremental encoder, since the absolute position of the moving body is unknown at the start of the operation, after performing the return operation to the preset home position as an initial operation, The moving body is moved to the operation start position, and normal operation starts. Therefore, if the home position return operation or the constant velocity section of the movement to the operation start position is used, it is possible to detect the phase shift without causing the moving body to perform an unnecessary operation. When the position of the moving body is detected using the absolute address type encoder, the home position return operation is not necessary, but the phase in the constant speed section at the time of the initial operation of moving the moving body to the work start position is unnecessary. If the shift is detected, the phase shift can be detected without useless operation.

The phase shift is detected by measuring the interval between encoder pulses output after waveform shaping of the encoder. The encoder pulse interval is always constant during the constant speed operation of the moving body without a phase shift, but when there is a phase shift, it is not constant but equal every other pulse.

First, an apparatus including a motor, a moving body, and an encoder that is an object of the present invention will be described. A device for rotating an arm-shaped moving body (operating range 240 °) by a direct drive motor with an incremental encoder
This will be described with reference to the drawings. The motor stator 23a is connected to the base 30, and the moving body 24, the detected object 27 for specific position detection, and the collision member 29 are connected to the motor rotor 23b. The encoder 22 includes a motor rotor 23b and a stator 2
It is arranged so that the relative position of 3a can be detected. The device equipped with the absolute address type encoder has a structure without the specific position detection target object 27 and the detection sensors 25 and 26. The shapes of the specific position detecting object 27, the collision member 29, and the stopper 28 shown in FIG. 26 will be described with reference to FIG. The object to be detected 27 is composed of an overrun detecting section 31 and an origin position detecting / area discriminating section 32 so that an origin position detecting sensor 26 is provided.
Can also be used for area determination, and all the specific positions can be detected by three sensors in combination with the overrun detection sensors 25a and 25b on both sides. As a result, the number of sensors can be reduced by one as compared with the conventional example shown in FIG. In addition, when the overrun is detected by the overrun detection sensors 25a and 25b and the mobile unit 24 performs an excessively excessive operation,
The collision member 29 connected to the moving body 24 comes into contact with the stopper 28 to stop the moving operation.

The origin position returning operation of this device will be described with reference to FIG. At the initial position, the origin position detection sensor 26 determines which side (L region, R region) the moving body 24 is relative to the origin position.
The moving object 24
Move toward the origin position. When the origin position is detected by the origin position detection sensor 26, the moving body 24 is moved in the direction of the true origin position. Then, the position where the Z phase of the encoder is detected is determined as the true origin position, and the movement is stopped. The subsequent operation is performed by detecting the relative displacement with this position as the reference position. In the present origin position return method, the operation algorithm is simplified as compared with the conventional example shown in FIG. 48, because the origin area is not detected.

Next, the position / speed detection method of the present invention will be described. The position / velocity detection according to the present invention includes detection of a phase shift from the encoder 2-phase original signal phase difference of 90 ° during initial movement of the moving body, position / velocity detection in consideration of the phase shift during normal movement of the moving body, 2 It consists of two steps. First, the description will be made with reference to FIGS. 19 to 23. Fig. 19 to No.
This will be described with reference to FIG. FIGS. 19 and 20 show time charts of position detection in the case of using an encoder of an incremental type or an absolute address type. The initial operation of the moving body includes two operations, that is, a return-to-origin operation and a movement to the normal operation start position in the uncremental method, and a movement to the normal operation start position in the absolute address method. Therefore, the phase shift detection may be performed in any operation in each method. FIG. 19 shows an example of detecting the phase shift at the time of the home return operation, and FIG. 20 shows an example of detecting the phase shift at the time of moving to the normal operation start position. As shown in the drawing, in the constant speed operation section, the time interval between the encoder pulses corresponds to the phase difference at the time of the zero crossing of the encoder two-phase original signal. Therefore, from the positive / negative relationship of the encoder two-phase original signal, the third direction in each movement direction S (forward: 1, reverse: -1)
By defining four 1/4 cycle positions q (= 0,1,2,3) as shown in the figure and detecting them by the 1/4 cycle position detection circuit in FIG. From the continuous pulse interval tq = 0 and tq = 1 from the periodic position (assuming q = 0), the following equation (16) is obtained.
Can calculate the phase shift τ.

FIG. 22 and FIG. 23 show flowcharts of the initial operation of the incremental method and the absolute address method. Two timers are used, and each time an encoder pulse is input, different timers are used to measure the time, so that a time delay can be prevented. Also, each timer is an even number in the constant velocity section,
Since the odd-numbered encoder pulse intervals are detected and stored, the average of the time intervals of the pulses at the same q value, q = 0, q = 1, is used to calculate from the equation (16), thereby obtaining the error. A small phase shift can be detected. No.
FIG. 21 shows a block diagram of the position / velocity detection device for a moving body of this embodiment.
A periodic position detection circuit 18, a phase shift detection timer 17 for detecting a time interval between encoder pulses obtained by passing the encoder original signal through the waveform shaping circuit 1, a specific position detection unit 35 required in the incremental system, and a data There is a decoder 19 for reading.

Next, position / velocity detection in consideration of the phase shift during the normal operation of the moving body will be described with reference to FIGS.
This will be described with reference to FIGS. 24, 25, and 29. FIG. 18 shows a time chart of position / speed detection of the moving body, and FIG. 21 shows a block diagram of the position / speed detection device.
Description will be made mainly using these two figures. FIG. 18 shows a case where the phase shift is 30 °, but the encoder pulse is generated at a non-uniform period even though the moving body is operating at a constant speed. (Coarse position), a counting error of ± 1 pulse occurs in a phase portion of 30 °. Therefore, in the present invention, the fact that the encoder pulse generation phase difference becomes 180 ° every two pulses is used, and the coarse position is counted every two pulses from a reference quarter period position (q = 0.2). The correction is performed (see the solid line in FIG. 18). This is the remainder of the integer x divided by the integer y mod
_When expressed as _y (x), the quarter-period positions q ₁ and q ₂ of the encoder 2-phase original signal immediately before and immediately after the change in the moving direction change,
Moving direction S ₁ , S ₂ (However, forward rotation: 1, reverse rotation: -1), encoder pulse count value P _{O (k)} at the time of k-th sampling, rotation angle / encoder pulse number = K ₂ , coarse position detection If the value is θ _{R (k)} , it is expressed by the following equation.

(I) From the time of the change of the moving direction to the time when mod ₂ (q) = 1, θ _{R (k)} = K ₂ ｛ _{PO (k)} + S ₁ · mod ₂ (q ₁ )｝ (17) (ii) After mod ₂ (q) = 1 after the change in the moving direction, θ _{R (k)} = K ₂ ｛ _{PO (k)} + S ₂ · mod ₂ (q)｝ (18) Also, equation (17) ( 18), the normal operation starts is a 1/4 cycle position at the start operation and q _2, and _{q 1 = mod 4 (q 2} -1),
The operation start time of movement direction and S _2, can be directly used by the S ₁ = -S _2.

Next, since the coarse position is counted every two pulses, a fine position (interpolation amount) within a 180 ° phase (two pulses) is detected in consideration of a phase shift in order to obtain a position at the time of sampling with high accuracy. . The method for detecting the fine position will be described. Here, the phase shift is assumed. When the moving body moves in the forward direction, the A phase of the encoder original signal has a phase delay of 90 ° + with respect to the B phase.
(See FIG. 18). in this case,
As shown in FIG. 18, when sampling and holding the encoder two-phase original signal and the zero-value signal at the time of sampling (A and B-phase voltages e _A and e _B and zero-value voltage e _C ), the moving direction S
If (positive direction: 1, reverse direction: -1), the following relationship is established between the fine position θ _{F (k)} at the k-th sampling and each signal value.

(When S _(k) = 1, 0 ≦ θ _{F (k)} ≦ 180 °, and when S _(k) = − 1, −180 ° ≦ θ _{F (k)} ≦ 0) The above θ _F value is In addition, a correction is made so that the different encoder pulse intervals are all 90 ° even at the constant speed operation due to the phase shift. Further, the moving direction S _(k) of the moving body is a change in the fine position θ _F obtained by the coarse detection detected from the direction discrimination signal obtained when the encoder original signal is pulsed by the waveform shaping circuit by the equation (19). The movement direction can be detected with high accuracy by performing correction by fine detection detected from the sign of the quantity.

Or the detected rough positions theta _{R (k)} and the fine position theta following equation from _{F (k)} (20) located theta _(k) by (21) can be determined velocity ω a _(k) (K ₅ is a sampling Is a coefficient proportional to the reciprocal of the period).

θ _(k) = θ _{R (k)} + θ _{F (k)} ... (20) ω _(k) = K ₅ (θ _(k) -θ _(k-1) ... (21) Is a method which can be applied to low-speed operation because a minute position change is detected by arctangent calculation even at low-speed operation in which no encoder pulse enters during the sampling period. Is shown in Fig. 24. Since the 1/4 period position at the start of the normal operation may be erroneously detected depending on the position where the encoder pulse is generated, 1/4 period after 1 pulse is generated
The configuration is such that the periodic position mod ₄ (q ₂ +1) is detected and the quarter period position q ₂ at the start of the operation is detected. In addition, when a sampling pulse is generated, it is interrupted, and various detections and calculations are performed.At other times, the detection and storage of the quarter cycle position and the detection of the change in the movement direction are always performed. When the direction is changed, the 1/4 period positions q ₁ and q ₂ before and after the change and the movement directions S ₁ and S ₂ are detected and stored, and are used for the calculation of the equations (17) and (18) at the time of the subsequent sampling. did. The arc tangent table of equation (19) is, as shown in FIG.
The moving direction is assigned a different address groups, by addressing the phase shift and the encoder 2 Aihara signal voltage ratio, and can be detected fine position theta _F shown in (19).

The devices used to detect the position / velocity of the moving body during normal operation are the coarse position detecting unit 15, the fine position detecting unit 16, etc. in the device shown in FIG. 21, and are synchronized with the sampling pulse. The encoder pulses are counted, the count value is taken into the central processing unit 36, and the coarse position correction shown in the equations (17) and (18) is performed. In the fine position detection, the bias and gain are adjusted by passing the encoder original signal through the bias voltage applying circuit 5 so that the input voltages of the A / D converters 7 to 9 become positive values, thereby adjusting the sampling pulse. In synchronism with the above, the sample and hold are performed by the sample holder 6 and are digitally converted by the A / D converters 7 to 9. The detected values of e _{A to} e _C are taken into the central processing unit 36, and the above detected values and the moving direction, 1 /
Fine position calculation and position / velocity calculation are performed from the four period position detection values. This device is a block diagram of a conventional example (see Fig. 44)
And slightly different configuration of micro position detection unit, but the prior art is e _A ~
e _C A / D conversion is performed by pulse width modulation, so even if the conventional fine position detection unit is applied to the fine position detection unit in Fig. 21 as it is, the position detection error is significantly reduced by the same method. can do.

FIG. 25 shows an example in which the position / velocity detecting device is connected to a control device 82 of a motor 23 for driving the moving body 24 such as a robot arm, and the position of the moving body 24 is controlled. In the detection of the position and speed of the moving object, the purpose was to detect the position and speed of the moving object.However, in this apparatus, in order to obtain the operation of the moving object 24 reaching the target position, the position deviation By generating a torque command based on the speed deviation, generating a torque current from the motor control device 82 and driving the motor 23, the torque command value is changed every sampling cycle until the moving body 24 reaches the target position. Be changed. Also, since the position command generation unit to the torque command generation unit are constituted by software servos, the central processing unit 36 shown in FIG. It can be used as a part of the position / speed control device. The above control calculation algorithm is also shown in FIG. 24 as a one-dot chain line portion.

Next, as a second embodiment of the present invention, in detecting a fine position from the encoder two-phase original signal and zero value signal, the encoder pulse generation is performed instead of detecting the instantaneous value of the encoder original signal at the time of sampling. A method and apparatus for obtaining an integral value equivalent from time to sampling as a digital value will be described with reference to FIGS. 30 to 40. FIG. 30 is a diagram showing detection of an encoder original signal instantaneous value,
FIG. 31 is a diagram showing encoder original signal integral value detection, FIG. 32 is a diagram showing encoder original signal integral value detection values during low-speed operation in which no encoder pulse is generated during the sampling period, and FIG. 33 is an encoder A diagram showing a position / speed detecting method and a moving body position control method during a normal operation of the moving body by detecting a digital amount corresponding to an integrated value of an original signal,
FIG. 34 is a block diagram of a position / velocity detection device of a moving object for detecting a digital amount corresponding to an integral value of an encoder original signal.
FIG. 36 is a diagram showing a configuration example of a voltage-controlled oscillator, FIG. 36 is a diagram showing a configuration example of a voltage variable resistor and a bias voltage applying circuit, FIG.
FIG. 38 is a diagram showing a characteristic example of a magnetoresistive element, FIG. 38 is a diagram showing the relationship between the oscillation frequency of the voltage controlled oscillator shown in FIG. 35 and the input voltage of the bias voltage applying circuit, FIG. 39 and FIG. 35
FIG. 3 is a diagram illustrating a relationship between an oscillation frequency of the voltage controlled oscillator illustrated in FIG. 1 and a passive element characteristic value.

First, referring to FIGS. 30 and 31, a comparison will be made between the case of detecting the instantaneous value of the encoder original signal and the case of detecting the integrated value from the time when the encoder pulse is generated to the time when sampling is performed. When the calculation of the equation (19) is performed by the instantaneous value detection, there is a drawback that the detection error becomes large when noise is mixed in the encoder original signal as shown in FIG. Although noise can be reduced to some extent by taking measures such as providing a filter at the output terminal of the bias voltage application circuit, it can be reduced to some extent. However, there is a limit to the reduction effect when using near a high current use environment such as welding work. . In that respect, when the integrated value detection shown in FIG. 31 is performed, even when noise is mixed in the original encoder signal, the ratio of the noise component to the detected value is small and the detection error can be reduced. Here, as shown in FIG. 34,
By passing the biased signal of the encoder original signal through a voltage controlled oscillator 40 having a linear function relationship between the input voltage and the oscillation frequency, the signal is converted into a pulse train, and is synchronized with the encoder, pulse generation, and sampling pulse by the counters 37 to 39. It is so arranged that the integral value equivalents N _A , N _B and N _C of the encoder original signal can be detected. The fine position θ _F depends on each movement direction S
It is calculated by the following formula regardless of the 1/4 cycle position q for (forward direction 1, backward direction-1). ( _KE : Number of encoder scales per rotation) (However, when S = 1, 0 ≦ θ _F ≦ 180 °, when S = −1, 1
80 ° ≦ θ _F ≦ 0) In addition, as shown in FIG. 32, when the encoder pulse does not enter within the sampling period (the speed obtained from the variation of the coarse position is called the coarse speed, the coarse speed is 0). Is (22)
Because not hold expression of relation, N _A, N _B, the following equation upon the occurrence of encoder pulses as shown in (23) as the N _C value (j =
It is necessary to obtain the total from 0) to sampling (j = n) to obtain the fine position from the equation (22).

Therefore, it is also determined whether or not the coarse speed is 0 in the flowchart shown in FIG. The other points are the same as those of the first embodiment shown in FIG.

Next, an example of the voltage controlled oscillator 40 will be described with reference to FIGS. 35 to 40. FIG. 35 shows a CR oscillator circuit composed of resistors 41 to 43, capacitors 44 and inverter elements 45 to 47. In this oscillation circuit, the inverter elements 45, 46 act as a DC power source, and the inverter elements 45, 46 are charged and discharged by the capacitor 44 each time the potential on the input / output side exceeds the threshold voltage value on each side. The operating principle is to oscillate by utilizing the inversion of the signal level on the input / output side of 46. The inverter element 47 plays a role of waveform shaping. When a transistor / transistor logic circuit 74S04 having an extremely short signal rise time is used as an inverter element, R ₁ + R ₃ (Ω), R ₂ (Ω), C (F) are made variable, and the oscillation frequency f ( Hz) and the characteristic values of the above passive elements, the relationships shown in FIGS. 39 and 40 were obtained, and it was found that the relationship of the following expression (24) was established in the region away from the oscillation limit.

The constants in the equation are values determined from various constants such as transistor, transistor, and transistor characteristic values in the logic circuit, resistance values, etc. The qualitative relationship between them is considered to be a sufficiently expressed equation, and the oscillation frequency of the present oscillation circuit is generally considered to be expressed by the following equation (25).
(However, α, β, γ are positive constants) As is clear from the equation (25), if a voltage variable resistor is used, the oscillation frequency can be changed by changing the primary voltage of the resistor. Here, a signal obtained by biasing the encoder original signal is applied to the primary side of the resistor, and the signal is converted into a pulse train by passing through the voltage controlled oscillator 40. In the formula (25), the R ₁ and voltage variable resistor as a voltage variable resistor 36
As shown in the figure, a structure is used in which a variable resistance can be obtained from both terminals of a magnetoresistive element 50 provided on a core 49 wound with a coil 48 on the primary side with a minute gap. The principle is that when the applied voltage at point A changes in FIG.
The change in the current flowing through 48 and the change in the magnetic force acting on the magnetoresistive element 50 are used to change the resistance of the magnetoresistive element. Since the magnetoresistive element generally exhibits the resistance characteristic shown in Fig. 37, if a bias voltage is applied so that it can be used in a region having a hyperbolic characteristic and R ₀₀ + R ₃ = γ is set, the oscillation frequency f of the voltage controlled oscillator and the applied voltage are A linear function relationship between the following equations (26) and (27) is established between V.

R ₁ = δ / (V + ε) + R ₀₀ …… (26) If the encoder original signal e _j is set in the bias voltage application circuit 5 to a bias voltage of V ₀₀ (= (Vα + Vβ) / 2) shown in FIG. 37 and the voltage across the coil is set to be not less than Vα and not more than Vβ, When the original signal is applied to point A in FIG. 36, the following equations (28) and (29) are established between the encoder original signal voltage e _j , the voltage V applied to the variable resistor, and the oscillation frequency f.

f = ξ'ηe _j + ξ'V ₀₀ + ζ '= ξe _j + ζ (ξ', ζ ': constant value) (29) Therefore, as shown in FIG. A functional relationship is obtained.

From this, when the moving body moves in the forward direction, the frequency and count value of the voltage controlled oscillator output pulse train corresponding to the encoder original signal are represented by the following equations (30) to (32).

1/4 period position q = 0,1 1/4 period position q = 2,3 For the reverse movement likewise also required, by the equation obtained from the equations (22) can be obtained a fine position theta _F.

Also, since the encoder resolution required by the work is different, the desired resolution can be obtained, as shown in Fig. 35.
As R _{2 is} variable, it returns to equation (27) and the oscillation frequency variable width Δf
(= Ξ '(R ₂ ) ΔV, ξ': R ₂ t is small. ΔV: A voltage variable resistance applied voltage both amplitude corresponding to both encoder voltage amplitude) A device that can be considered is considered. Also, if the R ₂ variable, since the oscillation limit (in the case of R ₂ t) FIG. 39 are shown as an oscillation frequency lower region exists, 35
As shown in the figure, pulse oscillation is performed at a high frequency at the point B, and the oscillation frequency is reduced to 1/2 ⁿ by the frequency divider 52, thereby enabling stable oscillation. The resistance value R ₂ and the frequency division ratio are determined by transmitting a signal from the central processing unit 36 of the position / speed detection device to the multiplexers 51a, 51b to select an appropriate value so that the C point oscillation frequency variable width (corresponding to the encoder resolution) can be adjusted according to the application. It is possible to make an optimal selection.

Next, a third embodiment of the present invention will be described with reference to FIG. FIG. 41 shows the initial operation and the normal operation of the moving body,
FIG. 4 is a time chart diagram of a method for detecting a position / velocity of a moving body for detecting a phase shift from 90 ° of a phase difference of an encoder two-phase original signal. In the first embodiment, an example is shown in which the phase difference from 90 ° of the encoder two-phase original signal phase difference is detected using the constant velocity section in the initial operation of the moving body. In many cases, the two-phase encoder signal phase difference
It is conceivable that the phase shift from 90 ° changes. Therefore, a phase shift is detected in a constant speed section during normal operation,
By rewriting the data detected and stored during the initial operation, it is possible to correct the phase shift suitable for long-term continuous operation and reduce the position / speed detection error. No.
In Fig. 41, the phase detection timer detects the phase not only during the initial operation but also during the constant speed section during normal operation, and the phase shift can be calculated based on equation (16) and the stored data can be corrected. .

Next, an example in which the moving object to be detected as the position / speed of the present invention is a robot arm or a robot wrist axis will be described with reference to FIG. FIG. 17 shows a horizontal articulated direct drive robot. The first arm 50 and the second arm 57 are direct drive motors.
It is driven by 44, 51, and the robot wrist vertical axis is driven by transmitting the rotation of the servo motor 58 by the belt 63 and converting the rotation and rectilinear motion by the ball screw 65.
The rotation of the servo wrist rotating shaft of the robot wrist is reduced by the rotary type reducer 61, the belt 64, the spline bearing mounting member
69, power is transmitted to the spline shaft 71 via the spline bearing 70. The movement of the wrist vertical axis is such that the bracket 67 connected to the nut part of the ball screw 65 moves up and down to restrain the bracket 67 in the vertical direction, and the spline shaft 71 freely connected in the rotational direction is opposed to the spline bearing 70. This is achieved by operating in the up-down direction due to the spline fitting.

The encoders shown in FIG. 17 are all examples of the incremental system, and the specific position detection target objects 46, 53, 66, 68 and the specific position detection sensors 47, 54 (partially shown) are provided on all four axes. ) Is provided. A stopper is provided on the first and second arm / wrist vertical axes to stop the operation of each axis when an overrun occurs.

Since the direct drive motors 44 and 51 for driving the first arm 50 and the second arm 57 do not have a speed reducer on the output shaft, the output is smaller than when a position is detected by mounting an encoder on the motor rotation shaft with a speed reducer. In order to obtain the same positional resolution on the shaft, the encoder must have a high resolution of twice the reduction ratio. That point,
The position detection method described in the present invention is effective because it has a high resolution and a variable resolution depending on the application.

〔The invention's effect〕

Since the present invention is configured as described above, it has the following effects.

(1) Since the fine position detection is performed by counting the number of pulses equivalent to the integral amount of the encoder original signal, the fine position detection error can be reduced even when a large noise is partially mixed in the encoder original signal. It is possible.

(2) The oscillation circuit that converts the encoder original signal into a pulse train can easily change the oscillation frequency variable width by an external signal, and can obtain a resolution corresponding to the application from the encoder.

(3) The present position / speed detection method enables high-resolution position / speed detection even during low-speed operation in which the sampling period is shorter than the encoder pulse generation period.

(4) By detecting the moving direction using both the coarse detection and the fine detection, the position can be accurately detected even when the moving direction changes.

(5) A position for detecting the interpolation position between encoder pulses in consideration of the phase shift even when there is a phase shift from 90 ° in the two-phase original signal phase difference of the position detection encoder of the moving body driven by the servomotor. The detection error can be reduced.

(6) Even in a low-speed operation in which no encoder pulse is generated within the sampling period, the position and speed are detected based on the encoder original signal, so that the controllability of the moving body in the low-speed operation is not impaired.

(7) Since the movement direction is performed by a combination of coarse detection and fine detection, the movement direction can be accurately detected even when the movement direction changes during low-speed movement, and the position can be accurately detected.

(8) Even when the moving body is continuously operated for a long time, by detecting a phase shift of the encoder two-phase original signal phase difference from 90 ° in the constant velocity section, it is possible to cope with a phase shift change due to a change in environmental conditions. is there.

(9) By performing the interpolation position detection between the encoder pulses using the voltage controlled oscillator, it is possible to reduce the interpolation position detection error due to the noise mixing of the encoder original signal.

[Brief description of the drawings]

FIG. 1 is a time chart of position / speed detection of the present invention,
FIG. 2 is a block diagram of the position locating device of the present invention, FIG. 3 is a diagram showing the relationship between the 1/4 period position of the encoder and the encoder original signal code, and FIG. 4 is a flowchart of the position / speed detecting method of the present invention. FIG. 5, FIG. 5 is a circuit diagram of the voltage controlled oscillator of the first embodiment, FIG. 6 is a diagram showing an example of a bias voltage applying circuit and a voltage variable resistor of the present invention, and FIG. FIG. 2 shows the relationship between the applied voltage at point A and the oscillation frequency of the voltage-controlled oscillator when the oscillator is applied to the position / speed detecting device shown in FIG. 2. FIG. 8 shows the relationship between the voltage variable resistor shown in FIG. FIG. 9 is a diagram showing a relationship between a resistance value and an input voltage, FIG. 9 is a circuit diagram of a voltage-controlled oscillator according to a second embodiment of the present invention, and FIG. 10 is a voltage when the voltage-controlled oscillator of FIG. FIG. 11 is a diagram showing the relationship between the oscillation frequency variable width of the controlled oscillator and the variable resistance value,
12 and 12 are diagrams showing changes in the oscillation frequency when the operating element characteristic values of the voltage controlled oscillator shown in FIG. 5 are changed,
Figure 13 is a time chart of position detection during low-speed operation where the sampling cycle is shorter than the encoder pulse generation cycle.
FIG. 15 is a detailed configuration diagram of the counter, FIG. 15 is an operation time chart of the counter / counter, and FIG. 16 is a diagram showing the position / speed detecting device described in the third embodiment of the present invention. FIG. 17 is a block diagram of a control device when applied to a position / speed control device, FIG. 17 is a structural diagram of an industrial robot shown as an example of a detection target of a position / speed detection device, and FIG. FIG. 19 is a time chart of a position / velocity detection method at the time of normal operation, FIG. 19 is a time chart showing a method of detecting a phase shift of a moving body using an incremental encoder of the first embodiment of the present invention, and FIG. FIG. 21 is a time chart showing a method for detecting a phase shift of a moving object using an absolute address type encoder according to the first embodiment of the present invention. FIG. 21 is a block diagram of a position / speed detecting device for the moving object according to the present embodiment. Fig. 22 uses an incremental encoder FIG. 23 is a flowchart showing an initial operation method of a moving object using an encoder of an absolute address type, and FIG. 24 is a flowchart showing an initial operation method of the moving object using an absolute address type encoder. FIG. 25 is a flow chart showing a speed detection method, FIG. 25 is a block diagram of a position / speed control device of a moving object including a position / speed detection device of the moving object of the present invention, and FIG. FIG. 27 is a longitudinal sectional view showing a structure of a device for rotatingly driving an arm-shaped moving body by a drive motor, FIG. 27 is a cross-sectional view taken along the line XXVII-XXVII of FIG. 26, and FIG. FIG. 29 is a diagram showing a position return method, FIG. 29 is a diagram showing a configuration of a table used to calculate an interpolation position between encoder pulses, and FIG.
The figure shows the detection amount when the instantaneous value detection of the encoder original signal is performed at the time of sampling, and FIG. 31 shows the detection amount when performing the integral value detection from the time of the encoder pulse generation of the encoder original signal at the time of sampling. FIG. 32 is a diagram showing a detection amount when detecting an integral value of an encoder original signal when no encoder pulse is generated during a sampling period, and FIG. 33 is a diagram showing a movement amount according to the second embodiment of the present invention. Flow chart showing the position and speed detection method during normal operation of the body,
FIG. 34 is a block diagram of a position / velocity detecting device for a moving object according to a second embodiment, FIG. 35 is a diagram showing a circuit configuration example of a voltage controlled oscillator in FIG. 34, and FIG. A diagram showing an example of a voltage application circuit and a configuration example of a voltage variable resistor,
FIG. 37 is a diagram showing characteristics of the magnetoresistive element, FIG. 38 is a diagram showing oscillation characteristics when the voltage controlled oscillator of FIG. 35 is combined with the voltage variable resistor and the bias voltage applying circuit of FIG. 36,
39 and 40 are diagrams showing the relationship between the oscillation frequency of the voltage controlled oscillator shown in FIG. 35 and the characteristic values of the passive elements, and FIG. 41 is a time chart showing the phase shift detecting method according to the third embodiment of the present invention. Figure, FIG. 42 is a time chart diagram showing a conventional moving body position / speed detection method, FIG. 43 is a flowchart diagram showing a conventional moving body position / speed detection method, and FIG. 44 is a conventional moving body detection method. Block diagram of position / velocity detector, Fig. 45 is a time chart of position / velocity detection of a moving object when the phase difference of the encoder two-phase original signals is 90 ° out of phase, and Fig. 46 is a true fine position. 45 degree encoder 2 phase original signal phase difference
Fig. 47 is a diagram showing the relationship between the phase shift from 90 ° and fine position detection error. Fig. 47 is a diagram showing the arrangement of the specific position detection sensor and the specific position detected object of the conventional moving body. Fig. 48 is the conventional movement. It is a figure which shows the origin position return method of a body.

Continuation of the front page (56) References JP-A-54-19773 (JP, A) JP-A-61-123905 (JP, A) JP-A-59-109809 (JP, A) , U) Japanese Patent Publication No. 4-658985 (JP, B2) Japanese Patent Publication No. 5-29045 (JP, B2)

Claims (27)

(57) [Claims] 1. A coarse position detecting step using an encoder pulse from an encoder connected to a moving body, and a coarse position using a pulse train having a sinusoidal frequency distribution obtained by inputting an encoder original signal to a voltage-frequency converting means. A fine position detection step of counting the number of pulses from the time of generation of the pulse, a step of detecting the position of the moving body as the sum of the coarse position and the fine position, and detecting the speed of the moving body based on the amount of change in position A method of detecting the position / velocity of a moving body, including the step of: 2. A coarse position detecting step using an encoder pulse from an encoder connected to a moving body, and a coarse position using a pulse train having a sinusoidal frequency distribution obtained by inputting an encoder original signal to a voltage-frequency converting means. A method for detecting the position of a moving object, comprising: a fine position detecting step of counting the number of pulses from the time of generation of the pulse; and a step of detecting the position of the moving object as the sum of the coarse position and the fine position. 3. The number of pulse-converted pulses from the time of pulse detection to the time of sampling of encoder original signals having two 90-degree phases different from each other in order to perform interpolation between the pulse detection positions of the encoder. Count and calculate the fine position from the encoder pulse generation position to the sampling position from each pulse number obtained by subtracting the pulse number obtained at the same time through the voltage controlled oscillator and counter from the pulse number A position / velocity detection method for a moving body, in which the precise position obtained as the sum of the rough position and the fine position is used as position information and the velocity of the moving body is detected from the change in the precise position at the time of sampling. 4. The method according to claim 1 or 3, wherein sampling is performed for the first time from pulse detection of the encoder original signal (k-th sampling).
Encoder A, B Aihara signal and a voltage controlled oscillator the zero value signal to the time of sampling), the number of pulses N _A obtained through counter ^{_{(k), N B (k}} ), stores N _O ^(k), the If there is no pulse detection of the encoder original signal until k + n sampling, set the number of each detection pulse at the k + ith sampling (1 in) The fine position from the pulse detection position of the encoder original signal to the time of sampling is calculated, and the fine position obtained as the sum of the coarse position and the fine position is used as position information, and the speed is calculated from the change in position at the time of sampling. The method of detecting the position and speed of the moving object to be detected. 5. The encoder according to claim 1, 3 or 4, wherein at the time of position detection, a quarter cycle position determined from the positive / negative relationship of each of the encoder two-phase original signals is detected, and a moving direction is obtained when the encoder two-phase original signals are pulsed. A method for detecting the position and speed of a moving object, wherein the coarse detection based on the direction discrimination signal and the fine detection based on the sign of the fine position change at the encoder pulse generation interval are combined. 6. A position / velocity detecting device using an encoder connected to a moving body, wherein a waveform shaping circuit for generating an encoder pulse and a moving direction discrimination signal from an encoder original signal, and an up / down detector for detecting a coarse position by the encoder pulse. A coarse position detector comprising a down counter, a temporary storage circuit for synchronizing the coarse position with the sampling time from a sampling pulse timer for generating a sampling signal for each sampling time, an encoder original signal and zero. A fine position detection unit having a counter for starting and ending counting of three types of pulse trains sent from the voltage-frequency conversion means, with the encoder pulse and the sampling pulse as gate signals with a value signal as an input; 1/4 cycle position detection determined by the positive / negative relation of the encoder 2-phase original signal Circuit and the coarse position detection unit, a fine position detector and the position-speed detector of a moving body, comprising: a central processing unit, a controlling operation of the 1/4 cycle position detection circuit. 7. The voltage-frequency conversion means of the fine position detecting section according to claim 6, wherein the voltage-frequency conversion means includes a bias voltage application circuit for generating a biased output voltage; And a voltage-controlled oscillator for generating a pulse train of the moving object, wherein the three kinds of pulse trains are transmitted from the voltage-controlled oscillator. 8. A bias voltage applying circuit according to claim 6, wherein said voltage controlled oscillator is constituted by a CR oscillation circuit having a voltage variable resistor, and said oscillation frequency of said voltage controlled oscillator has a linear function with an input voltage. A position / velocity detection device for a moving body, which comprises each of the constituent elements such as the voltage variable resistor. 9. The moving body according to claim 8, wherein the variable voltage resistor comprises a magnetic material wound on the coil on the voltage application side and a magnetoresistive element having both ends in contact with a minute gap and having an output terminal. Position / speed detector. 10. A resistor or a capacitor other than a voltage variable resistor is provided in parallel in a plurality of types, a frequency divider is provided in an output stage, and a specific resistor or a capacitor is provided by a signal from a central processing unit. A moving object position / speed detecting device configured to select a specific frequency division ratio of a capacitor and a frequency divider. 11. A means for obtaining a specific oscillation frequency variable width in said voltage controlled oscillator by selecting a specific resistor or capacitor and a specific division ratio of a frequency divider according to a required resolution. A position / velocity detection device for a moving body. 12. The central processing unit according to claim 7, wherein the position / speed detecting device of the moving body is connected to a motor control device of a motor for driving the moving body, and a program relating to the motor control method is provided in the central processing unit. Position / speed detector for moving objects. 13. The position / speed detecting device for a moving body according to claim 12, wherein the motor driven by the motor control device is for driving a robot joint, and the position detecting encoder is connected to the motor or the joint to be driven. . 14. An apparatus according to claim 5, wherein each of the A-phase counter, the B-phase counter and the zero-value counter comprises a pair of counters, and when the counting of one of the counters is completed, the counting of the other counter is completed. A position / velocity detecting device for a moving object, comprising means for causing a container to start counting. 15. An encoder connected to a moving body,
When calculating the position / velocity at a certain time, the coarse position is detected by counting the encoder pulses generated at the time when the encoder 2-phase original signal crosses the zero value when the moving body moves, and the encoder 2-phase signal and the zero-value signal are detected. A position / speed detection method for a moving object, wherein a position (interpolation amount) between the encoder pulses is detected based on the detected position, the position is detected as the sum of the coarse position and the fine position, and the speed is detected based on the amount of change in the position. In the above, the time interval of the encoder pulse is measured for the constant speed operation of the moving body, and the quarter period position q of the encoder determined by the sign of the encoder two-phase original signal at that interval (0 → 1 for both forward and reverse rotations) → 2 → 3 → 0 for each moving direction) and the moving direction S (normal rotation: 1, reverse rotation: −1) are detected, and a specific continuous q value (for example, q =
0,1) encoder pulse interval t _{q = 0} , t _{q = 1}
From the following equation, the phase shift of the encoder two-phase original signal from 90 ° is detected and stored, and Next, the 1/4 cycle position q ₁ of the encoder two-phase original signal immediately before and after the change when the moving direction changes during the normal operation of the moving body,
q ₂ {However, when the q value at the start of normal operation is q _2, and the remainder of the integer y of the integer x is represented by mod _y (x), q ₁ = mod
₄ (q ₂ −1)} and the 1/4 cycle position q at the time of sampling are detected, and 2 is obtained from the reference 1/4 cycle position (for example, q = 0).
A method of correcting the count value so as to count the coarse position for each pulse is employed, and the encoder pulse count value P ₀ ,
Rotation angle / encoder pulse number = k ₂ , movement direction S ₁ , S ₂ just before and after change in movement direction (forward: 1, reverse: −1, but when normal operation starts, S ₂ : movement direction, S ₁ : The direction opposite to the moving direction), the rough position θ _R is calculated by the following equation. (I) θ _R = k ₂ ｛P ₀ + S from the time when the moving direction changes until mod ₂ (q) = 1 ₁ · mod ₂ (q ₂ )｝ (ii) After mod ₂ (q) = 1 after changing the moving direction θ _R = k ₂ ｛P ₀ −S ₂ · mod ₂ (q)｝ Fine position θ _F Is the arctangent based on the encoder two-phase original signal, zero value signal, moving direction, quarter period position and phase shift so as to interpolate the range of two encoder pulses (180 ° phase of the encoder original signal). operation (e.g., an encoder 2 Aihara signal instantaneous value e _a the time of sampling, e _B, zero value signal instantaneous value e _C, out of phase, and a moving direction S, for example, in the formula, S ,, e _A , e _B , e _C are given to obtain θ _F ), and the position θ is detected as the sum of the coarse position θ _R and the fine position θ _F. Position / speed detection method. 16. An apparatus according to claim 15, wherein an initial operation of the moving body is performed (returning to an origin return operation and a normal operation start position when an incremental encoder is used, and a normal operation starts when an absolute address encoder is used). A method for detecting the position / speed of a moving body, which measures the encoder pulse interval using the constant speed operation section at the time of (movement to a position) and detects a phase shift of the encoder two-phase original signal phase difference from 90 °. 17. The method according to claim 15, wherein a moving direction of the moving body is detected by coarse detection obtained when the encoder original signal is pulsed and a fine change detected by a sign change of a fine position change amount between encoder pulses. A method for detecting the position and speed of a moving object obtained from detection. 18. The encoder two-phase original signal according to claim 15, wherein a pulse interval, a quarter-period position and a moving direction of the encoder are detected and stored in a constant speed section during a normal operation of the moving body. A method for calculating and storing a phase shift from 90 ° of the phase difference, calculating an arc tangent based on the phase shift detected immediately before sampling at the time of sampling, and detecting a fine position, thereby detecting a fine position. 19. A pulse train having a sinusoidal frequency distribution by passing a sinusoidal encoder two-phase original signal and a zero-value signal through a voltage-controlled oscillator for fine position detection according to any one of claims 15 to 18, The number of pulses from the encoder pulse detection to the sampling time is counted, and the arc tangent operation is calculated from the value corresponding to the two-phase sine wave integral from the encoder pulse detection to the sampling time to determine the fine position of the moving object. Position / speed detection method. 20. An apparatus according to claim 15, wherein at the time of fine position detection, an encoder two-phase original signal and a zero-value signal are sampled and held at the time of sampling, and A / D converted.
A position / velocity detection method for a moving body, which detects a fine position by performing an arctangent calculation from instantaneous values of a two-phase sine wave at the time of sampling. 21. A position / velocity detecting device for a moving body by an encoder connected to the moving body, wherein a timer for detecting a pulse interval of the encoder, a quarter period position detecting circuit,
For each moving direction S (forward 1 and reverse -1), the phase difference (0) of the phase difference of the encoder 2 phase original signal from 90 ° and the 2 phase original signal instantaneous value ratio or integration from 1/4 cycle position q It is characterized by having a ROM in which data capable of searching the fine position θ _F from the arctangent data corresponding to the quantity equivalent value ratio (for example, S · sin θ _F / cos (θ _F +) when q = 0) is stored. Position / velocity detection device for moving objects. 22. A waveform shaping circuit for generating an encoder pulse and a moving direction discrimination signal from an encoder two-phase original signal, an up / down counter for detecting a coarse position by the encoder pulse, and sampling the coarse position. A coarse position detector having a temporary storage circuit for synchronizing with the sampling time from a sampling pulse timer for generating a sampling signal every time; and an output biased with an encoder two-phase original signal and a zero value signal as inputs. Bias voltage application circuit for generating voltage,
The output of the bias voltage application circuit becomes an input, a voltage-controlled oscillator that generates a pulse train having a different frequency according to the input power, and the counting of the three types of pulse trains sent from the voltage-controlled oscillator is started, with the encoder pulse and the sampling pulse becoming gate signals. A position / velocity detection device for a moving object, comprising: a fine position detection unit having a counter to be terminated; and a central processing unit for controlling the operation of each detection unit. 23. A waveform shaping circuit for generating an encoder pulse and a moving direction discrimination signal from an encoder two-phase original signal, an up / down counter for detecting a coarse position by the encoder pulse, and sampling the coarse position. A coarse position detector including a temporary storage circuit for synchronizing with the sampling time from a sampling pulse timer for generating a sampling signal every time; and a bias for applying a bias voltage to the encoder two-phase original signal and the zero value signal A fine position detection unit including a voltage application circuit, a sample holder that samples and holds each of the signals in synchronization with a sampling pulse, and an A / D converter that performs A / D conversion on the output of the sample holder; A position / velocity detection device for a moving object, comprising a central processing unit for controlling the operation of the unit. 24. An encoder according to any one of claims 21 to 23, wherein the encoder connected to the moving body is of an incremental type, and a true position detecting unit for detecting only the moving limit position of the moving body and a true position detecting unit. A detected object having a step-like origin position detection unit detected at the origin position near the origin position, and maintaining the step in the movable range in each movement direction of the origin position and being driven integrally with the moving body, Limit position detection sensors disposed at both ends of the movement limit of the moving object to detect limit position detection units of the object to be detected, and an origin position detection of the object to be detected arranged at the origin position near the true origin position An origin position detection sensor that detects whether the part is at the origin position or in which area on both sides of the origin position, a collision member coupled to the moving body, and the above-mentioned when the moving body moves beyond the movement limit position. Collision part A position / speed detection device for a moving body, comprising a stopper that abuts against a material and stops the movement of the moving body. 25. A computer according to claim 21, wherein the position / speed detecting device of the moving body is connected to a motor control device of a motor for driving the moving body, and a program relating to the motor control method is provided in the central processing unit. A moving body position / speed detection device. 26. An apparatus according to claim 25, wherein the motor driven by the motor control device is for driving a robot joint, and the encoder for position detection is a position or velocity detecting device for a moving body coupled to the motor or the driven joint. . 27. A method for returning to the origin position of a moving body driven by a servo motor having an incremental encoder, wherein a detection sensor detects at which side the moving body is located with respect to the origin position at the start of operation. Move the moving body in the direction of the origin, detect the phase shift during the initial operation in the constant velocity section, detect the origin located near the true origin with the detection sensor, and then move to the origin. The moving body is moved in a direction in which an encoder Z phase (true origin position) is detected, and the movement of the moving body is stopped when the encoder Z phase is detected. Method.