JPS6191510A - Method and apparatus for detecting present position - Google Patents

Abstract

PURPOSE:To make it possible to shorten the time required in the returning to the starting point to a large extent, by providing a stage for rotating a servo motor, a stage for counting the number of rotary position pulses, a transfer stage to memory and a stage for confirming a present position. CONSTITUTION:Main detector and a transmission mechanism having a servo motor shaft different in a variable speed ratio and a drive shaft are provided and an auxiliary detector outputting a pulse (second marker signal) Ps with the rotation of the follower shaft of a variable speed mechanism is attached and the output interval of the starting point pulse (first marker signal) Pi of the main detector and the second marker signal is calculated by counting the rotary position pulse of the main detector. The first marker signal based on the number of rotations of the main detector is discriminated by a microcom puter 7 constituted of a signal order discrimination block 70, a sub-block control block 71, an operation block 72 and memories 73, 74, 75, 76 to calculate a pres ent position by the max. two rotations of the main detector, even if the main detector is not returned to the starting point position, to complete starting point return operation. By this method, the time of the starting point return operation is shortened to a large extent.

Description

[Detailed Description of the Invention] [Field of Industrial Application] The present invention relates to a method and device for detecting the current position of a drive shaft of a numerically controlled machine such as an industrial robot, using an incremental rotary encoder as a position detector. .

[Conventional technology]

FIG. 7 is a diagram showing a conventional example of this type of numerically controlled machine. The table 21 is moved in the horizontal direction in the drawing by a pole screw 22 driven by a servo motor 22, and its position is detected by a pulse generator 24 directly connected to the shaft of the servo motor 22, and its speed is detected by a tacho generator 25.

In this case, the position of the table 21 and the contents of the counter that counts the rotational position pulses of the pulse generator 24 do not match in the initial state when the power is turned on (when the power is initially turned on or when restarting after recovery from a failure). Assuming that the table 21 is at the position indicated by the solid line when 0 current is applied, which requires a return-to-origin operation (synchronizing) to match the
The table 21 is unconditionally moved to the left in the figure at a constant speed, and the origin selection limit switch 26 is turned on (indicated by the two-dot chain line), and the position of the origin signal of the pulse generator 24 that is first output is By stopping the table 21 and resetting the contents of the counter (setting it to O), the return-to-origin operation is completed. Note that 27 and 28 are limit switches for overrun detection.

By the way, robots generally have a large number of control axes.
Furthermore, since the working tool is necessarily present within its movable range, (1) the origin position must be the least obstructive position within the movable range, that is, the end of the movable range. (
2) Since the initial state of each axis is unknown when energized, it is dangerous to perform home return operation on all axes at the same time.
There is a limitation in that the return-to-origin operation must be performed axis by axis at low speed, and as a result, the return-to-origin operation takes time and the actual operating rate of the machine decreases.

[Problem that the invention seeks to solve]

In this way, conventional numerically controlled machines such as industrial robots that use incremental rotary encoders as position detectors have the problem of taking time to return to the origin and reducing the actual operating rate of the machine. .

Therefore, an object of the present invention is to provide a method and device for detecting the current position, which greatly reduces the time required for the return-to-origin operation and improves the efficiency of the machine.

[Means for solving problems]

The present invention differs in speed change from the main detector (pulse generator), and is equipped with a speed change mechanism using a servo motor shaft as the drive shaft.
Install a sub-detector that outputs pulses (hereinafter referred to as second marker signals) as the driven shaft of this transmission mechanism rotates,
The output interval between the origin pulse of the main detector (hereinafter referred to as the first marker signal) and the second marker signal is determined by counting the rotational position pulses of the main detector, and the first marker signal is detected as the main detector. Instead of returning to the home position as in the conventional method, the current position is determined by determining the rotation number of the first marker signal, and the current position is determined in a maximum of two revolutions of the main detector, and the home return operation is completed. It is.

FIG. 6 is a diagram showing the principle of the present invention. At origin 0, the first marker signal and the second marker signal are output simultaneously. From when the marker signal of Sl is output to when the second marker signal is output, the number of rotational position pulses of the main detector of C is ΔP□, ΔP2+..., ΔPn every time the main detector rotates once. Then, these rotational position pulse numbers ΔP, ΔP2
, . . . , ΔPn is proportional to the rotation speed of the main detector. That is, if the number of rotational position pulses when the rotation speed of the main detector is i is ΔPi, then ΔPi=i·ΔP□. Therefore, the present invention has the following advantages: the speed ratio of the main detector, the number of rotational position pulses PPR per rotation of the main detector, and the number of unit variation pulses.
(Determined by the gear ratio of the main detector and transmission mechanism, the number of rotational position pulses corresponds to the phase difference between the first marker signal and the second marker signal, which is ΔP in Fig. 6) and between both marker signals. From the number of rotational position pulses in Then, the current position P (=total number of rotational position pulses from the origin/speed ratio of the main detector) is calculated.

Further, in the present invention, when the servo motor is driven at high speed rotation, the amount of overshoot of the current position during calculation of the current position P is determined and compensated for.

In summary, the first current position detection method of the present invention is as follows:
The number of rotational position pulses corresponding to the rotation of the servo motor is counted during the stage where the servo motor is rotated in a fixed direction by the current position detection start command and from the generation of one marker signal until the generation of the other marker signal. a step of counting, a step of transferring the counted number of rotational position pulses from the counter to a memory, a total number of rotational position pulses per one rotation of the main detector, the number of unit variation pulses, a gear ratio of the main transmission, It consists of the step of calculating the current position based on the number of rotational position pulses stored in the memory, the type of the first generated marker signal, and the phase difference between both marker signals.

The second current position detection method of the present invention is performed in parallel with the step of transferring the counted rotational position pulses to the memory and the step of calculating the current position in the first current position detection method. The number of rotational position pulses is counted until the calculation of the current position is completed, and the counted number of rotational position pulses for correction is transferred to the second memory, and the calculation of the current position is performed. After completion, the present position data is corrected by adding or subtracting the number of correction rotational position pulses to the current position data.

The current position detection device of the present invention includes means that is directly connected to a servo motor shaft to generate a first marker signal and incrementally generate rotational position pulses, a speed change mechanism that uses the servo motor shaft as a drive shaft, and means for generating a second marker signal in accordance with the rotation of the driven side of the conversion mechanism; a counter for counting the number of rotational position pulses; and a memory for storing the number of rotational position pulses as a count result of the counter. , a first control signal is output when the first marker signal or the second marker signal is generated, and a second control signal is output when the subsequent second marker signal or the first marker signal is generated. a signal command discrimination block that outputs a signal; and a means for generating the first marker signal; a total number of the rotational position pulses per 11 rotations; a number of rotational position pulses corresponding to the phase difference between the first and second marker signals; calculation means for calculating the current position using a unit variational pulse, the number of rotational position pulses stored in the memory, and a character signal indicating the type of the first generated marker signal;
A counter control signal that causes the counter to start a counting operation according to a first control signal output from the signal command determination block and stops the counting operation of the counter according to the second control signal; A transfer command signal to be transferred to the memory, a calculation start command signal for causing the calculation means to start calculation following the output of the transfer command signal, and the character signal are output, and when the calculation of the calculation means is completed, the and control means for inputting a computation end signal from the computation means.

〔Example〕 Embodiments of the present invention will be described with reference to the drawings.

FIG. 1 is a block diagram showing one embodiment of a current position detection device according to the present invention.

A pulse generator 3 that generates a marker signal Pi (first marker signal) and a rotational position pulse ΔPi or Pad (for correcting the calculated current position) is directly connected to the shaft of the servo motor 1. Furthermore, as shown in FIG. 2, a transmission 2 (the transmission ratio is very close to 1, but not 1) is interposed between the servo motor 1 and the pulse generator 3, and meshes with the drive side gear 2a. A slit 2c is provided on one side of the driven gear 2b, and a marker signal Ps (second marker signal) is generated by a non-contact limit switch 4 (magnetic type in this embodiment) attached to the case 2d. It has become. Note that the marker signals Pi and Ps are adjusted so that they are generated simultaneously at the control origin position. A counter 6 counts rotation pulses ΔPi or Pad output from the pulse generator 3. A controller 9 controls a servo motor l.

Signal command discrimination block 70, subprogram control block 71, calculation block 72, memories 73, 74, 75.7
8 is configured as a microcomputer 7.

The signal command discrimination block 5 receives the marker signals Pi and Ps output from the pulse generator 3 and the non-contact limit switch 4, respectively, and receives the start/end signal St and the first input signal. A character signal Sch indicating the type is output. Memory 73.74
is the number of rotational position pulses ΔP counted by the counter 6.
i and Pad are respectively stored through the data bus 8.

The calculation block 72 calculates the current position P (the number of pulses from the control origin O to the current position) according to the following calculation formula (see FIG. 6).

Here, PPR: Total number of rotating device pulses generated by the pulse generator 3 per rotation of the servo motor 1 ΔPi: Number of rotational position pulses counted during the appearance interval of the two marker signals Pi and Ps■: Gear ratio of the transmission W2 The number of unit variation pulses generated by If detected in time,
The number of rotational position pulses Pa indicating the amount of overshoot from when the counter 6 stored in the memory 74 is reset to when the calculation of the current position P is completed, based on the current position IP calculated by equation (1).
d is corrected using the following formula.

Pr = P±P ad - (2) The memory 75 is composed of NVRAM, and the constant P in the above calculation is
The values of PP and I are stored.

The current position □P or Pr obtained in this manner is stored in the memory 76 and then output to a main program block (not shown) (which controls the entire robot).

The sub-program control block 71 is controlled by the control signal 5pro from the main program pro-nk, and the start/stop signal St and character signal Sch are input from the signal command discrimination block 70.

The subprogram control block 71 outputs a start signal Cs that causes the counter 6 to start counting the rotational position pulses ΔPi or Pad according to the first start/stop signal St, that is, the marker signal Pi or Ps, and outputs a start signal Cs that causes the counter 6 to start counting the rotational position pulse ΔPi or Pad. , that is, a stop signal Sc that causes the counter 6 to stop counting the rotational position pulse ΔPi or Pad according to the other marker signal Ps or Pi, a reset signal Rt that resets the contents of the counter 6 when counting the rotational position device pulse number Pad, and Transfer command signal Ro for transferring the number of rotational position pulses ΔPi or Pad, which is the count result, to the memories 73 and 74, respectively.
Output. After outputting this transfer command signal Ro, the subprogram control block 71 outputs a calculation start signal Scd to the calculation block 72 in order to have the calculation block 72 calculate the current position P, and the calculation of the current position P is also performed. When the calculation is completed and the calculation result is written into the memory 78, a calculation completion signal Scd is input from the calculation block 72. This returns control to the main program block.

The command to start the current position detection operation is issued by the microcomputer 7.
A circuit command pulse to the controller 9 for driving the servo motor 1 is output from this circuit.

Next, the operation of this embodiment will be explained with reference to the flowchart shown in FIG.

(Process 100) When an operation start command is given to a circuit (not shown) of the microcomputer 7 from an operator or another CPU, a rotation command pulse is issued to the controller 9, and at the same time, the main program block is sent to the subprogram control block 71. A standby command 5pro is generated. The controller 9 supplies power to the servo motor 1 in response to this circuit command pulse. This causes the servo motor l to rotate in a fixed direction.

(Process 110) Pulse generator 3 directly connected to the axis of servo motor 1
Check whether the marker signal Pi or the marker signal Ps is output from the non-contact limit switch 4 provided on the driven side of the transmission mechanism 2 connected to the shaft of the servo motor 1. The servo motor 1 continues to rotate in a fixed direction until these marker signals Pi or Ps are output.

(Processing 120) When one of the marker signals Pi or Ps is output, it is input to the signal command determination block 70, and the start/levitation signal St
is output to the subprogram control block 71. The subprogram control block 71 outputs a count start signal Cs to the counter 6 in response to this start/stop signal St. Upon receiving the count start signal Cs, the counter 6 starts counting the rotational position pulse ΔPi generated by the pulse generator 3.

(Process 130) Check whether the other marker signal Pi or Ps is output.

(Process 140) When the other marker signal Pi or Ps is output, this signal is input to the signal command determination block 70, and the start/stop signal St and character signal Sch are input to the subprogram control block 71. Subprogram control pro/
The link 71 confirms that both the marker signals Pi and Ps are input, and outputs the count stop signal Cs and the transfer command signal RO to the counter 6, and also outputs the count stop signal Cs and the transfer command signal RO to the counter 6.
The calculation start signal Scd and character signal Sch are output to the input signal 2. Upon receiving the transfer command signal Ro, the value of the rotational position pulse number ΔP1, which is the count result, is transferred to the memory 73 via the data bus 8.

(Processing 150) The calculation block 72 calculates the rotational position pulse number Δ from the memory 73.
Pl, reads the value of the constant PPR,I from the memory 76, and reads the character signal S from the subprogram control block 71.
According to the channel, calculate the equation 1 (1) and calculate the current position: rLF
seek.

(Process 160) The calculation result P is written to the memory 7B, and a calculation end signal Scd indicating the end of writing is output to the subprogram control block 71, the current position detection operation is completed, and control is returned to the main program block. .

Figure 5 shows the period from when the counter stop signal Cs is output until the calculation of the current position P is completed when the servo motor l is driven at high speed rotation in Figure 1 to detect the current position in a very short time. 3 is a flowchart illustrating the operation of another embodiment of the present invention to compensate for overshoots between.

Processes 100 to 150 are the same as those in FIG. 4, so their explanation will be omitted.

(Processing 170) The subbumgram control block 71 outputs the count stop signal C.
3. Following the transfer command signal Ro, a reset signal Rt is output to reset the counter 6.

(Process 180) The subprogram control block 71 again outputs the count start signal C8 to the counter 6, and the counter 6 counts the rotational position pulse Pad.

(Process 180) It is checked whether the calculation end signal 5cal has been output from the calculation block 72 to the subprogram control block 71.

The counting operation of the counter 6 in process 170 is performed until this computation end signal Scd is output.

(Process 200) When the calculation end signal 5can is output, the subprogram control block 71 sends a count stop signal Cs to the counter 6.
, and then outputs a transfer command signal RO.

As a result, the number of rotational position pulses Pad counted by the counter 6 is transferred to the memory 74 of the microcomputer 7 through the data bus 8.

(Process 21O) The current position P obtained in process 150 is corrected by the rotational position pulse number Pad obtained in process 200 in accordance with equation 1 (2) in calculation block 72.

(Process 220) The corrected current position value Pr is written to the memory 76.

FIG. 3 shows an embodiment in which the transmission mechanism 2 is provided at the end of the feed screw 5 on the opposite side from the servo motor 1, which is convenient for remodeling an existing device.

Although the transmission mechanism 2 of this embodiment is composed of gears, it may also be composed of rollers with different diameters, a belt (timing belt), and pulleys. Furthermore, as the means for generating the marker signal Pi, an electrostatic or optical limit switch may be used.
Furthermore, a pulse generator may be attached to the driven shaft of the transmission mechanism, and these origin pulses may be used as the second marker signal.

In the transmission mechanism of this embodiment, the transmission ratio is close to 1, but not 1, but even if the transmission ratio is close to 3, but not 3, it is possible to make one revolution of the driven shaft by using three slits, etc. The same result can be obtained even if three pulse signals are outputted at equal intervals.

〔Effect of the invention〕

As explained above, the present invention provides a speed change mechanism with a different speed ratio from the main detector (pulse generator) and uses the servo motor shaft as the drive shaft, and a (second marker signal) is provided along with the driven shaft of this speed change mechanism. To install the output sub-detector, calculate the output interval between the main detector's origin pulse (first marker signal) and the second marker signal by counting the rotational position pulses of the main detector, and then attach the output to the first marker. The system determines which rotation of the main detector the signal is the first marker signal, determines the current position after a maximum of two rotations of the main detector, and completes the return-to-origin operation. The operation time is significantly shortened, improving the actual operating rate of the machine, and since there is no large return-to-origin operation, work stability is ensured.

[Brief explanation of the drawing]

FIG. 1 is a block diagram showing an embodiment of the current position detecting device according to the present invention, and FIGS. 2 and 3 show the servo motor 1, pulse generator 3, transmission mechanism 2, and the like shown in FIG. A partial cross-sectional view of the non-contact limit switch 4, FIGS. 4 and 5 are flowcharts showing the flow of current position detection in the current position detection device of FIG. 1, and FIG. 1st. FIG. 7 is a time chart showing the relationship between the second marker signals, and is a diagram showing a conventional example of a numerically controlled machine. 1: Servo motor, 2: Speed change mechanism. 3: Pulse generator, 4: Non-contact limit switch, 6: Counter 7: Microcomputer, 8: Data bus, 9: Controller, 70: Signal command discrimination block. 71; Subprogram control block, 72: Arithmetic block, 73.74, 75.76: Memory, Pi, Ps
: marker signal, ΔPi : rotational position pulse, St : start/stop signal, C3: count start/stop signal, Rt : reset signal, Ro: transfer command signal, 5ca
f: calculation start/end signal, Sch: character signal. Figure 3 Figure 4 Figure 5 Figure 6 Figure 7

Claims (1)

[Claims] 1) A method for detecting the current position of a drive shaft of a numerically controlled machine, which comprises: rotating a servo motor in a fixed direction in response to a current position detection start command; and generating one marker signal. to the generation of the other marker signal, a stage in which the counter counts the number of rotational position pulses according to the rotation of the servo motor, a stage in which the counted number of rotational position pulses is transferred from the counter to the memory, and a stage in which the main detection The total number of rotational position pulses per one rotation of the device, the unit variation pulse number which is the number of rotational position pulses corresponding to the phase difference between both marker signals, the gear ratio of the main detector, and the rotational position pulses stored in the memory. 1. A method for detecting a current position, comprising the step of calculating the current position based on the number, the type of marker signal generated first, and the phase difference between both marker signals. 2) A method for detecting the current position of a drive shaft of a numerically controlled machine, which includes the steps of rotating a servo motor in a fixed direction in response to a current position detection start command, and the steps of rotating a servo motor in a fixed direction after one marker signal is generated and then detecting the other marker signal. Until the signal is generated, the counter counts the number of rotational position pulses according to the rotation of the servo motor, the stage of transferring the counted number of rotational position pulses from the counter to the first memory, and the main detector. The total number of rotational position pulses per rotation of , the unit variational pulse number which is the number of rotational position pulses corresponding to the phase difference between both marker signals, the gear ratio of the main detector, and the rotational position stored in the first memory. A step of calculating the current position based on the number of pulses, the type of marker signal generated first, and the phase difference between both marker signals, a step of transferring the counted rotational position pulses to the first memory, and a step of calculating the current position. carried out in parallel,
A step of counting the number of rotational position pulses until the counter is reset and the calculation of the current position is completed, and transferring the counted number of rotational position pulses for correction to a second memory, and a step of calculating the current position. A current position detection method comprising the step of correcting the current position data by adding or subtracting the number of rotational position pulses for correction to the current position data after completion of the process. 3) A current position detection device for detecting the current position of a drive shaft of a numerically controlled machine, which is connected to a servo motor shaft and generates a first marker signal and incrementally generates rotational position pulses. a transmission mechanism having a servo motor shaft as a driving shaft; means for generating a second marker signal in accordance with rotation of a driven shaft of the transmission mechanism; a counter for counting the number of rotational position pulses; a first memory in which a number of rotational position pulses, which is a count result of the counter, is stored; and a first memory that outputs a first control signal when the first marker signal or the second marker signal is generated; a signal command discrimination block that outputs a second control signal when a second marker signal or a first marker signal is generated; the total number, the unit variation pulse which is the number of rotational position pulses corresponding to the phase difference between the first and second marker signals, the number of rotational position pulses stored in the first memory, and the number of first generated marker signals. a calculation means for calculating the current position based on a character signal indicating the type and a phase difference between the first and second marker signals; and a calculation means for starting a counting operation in the counter based on the first control signal inputted from the signal command discrimination block. let me,
a counter control signal that causes the counter to stop counting according to the second control signal; a transfer command signal that transfers the count result of the counter to the first memory;
Following the output of the transfer command signal, outputting a calculation start command signal and the character signal for causing the calculation means to start calculation, and inputting a calculation end signal from the calculation means when the calculation of the calculation means is completed. What is claimed is: 1. A current position detection device comprising: a control means for controlling a current position;