JPH06149332A - Interlock control system - Google Patents

Abstract

PURPOSE:To prevent the interference caused between the machines without using an interference preventing program by carrying out the interlock control after deciding the interference state between an action unable area and a mobile area based on the present position and an estimated position. CONSTITUTION:The position sensors 9 are connected to the rotary shafts of the servo motors 8 and detect the present positions respectively. The controllers 1 read the control programs of machines A and B out of each built-in memory and output the target position data PP on the motors 8 to the position command parts 2 to perform the positioning control. The position computing parts 6 convert the output of the sensors 9 into the present position data P1. The position estimating parts 11 estimate the machine positions to be set after a prescribed time based on the data P1. Then the area production parts 12 produce the area data AA and AB for the movements of both machines A and B between the present and estimated positions and output these data to an interlock control part 20. The part 20 supplies the area data AD through an area designating part 10 to decide the interference state among those areas AA, AB and AD and then carries out the avoidance processing against the interference if caused among these areas.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an interlock control system for preventing mutual interference of a machine, a tool, a work, a robot arm, etc. positioned by a positioning device.

[0002]

2. Description of the Related Art Factory automation (FA)
A typical example is a transfer line system such as an automobile production line. This transfer line system uses multiple types of processing machines (such as welding machines) along a moving line (transfer line).
And a plurality of processing machines are positioned by a positioning device, and desired processing is performed on the work moving on the transfer line.

The processing machine is equipped with a positioning device for positioning the processing head or the like at a predetermined position with respect to the moving work. As a typical positioning device,
There are servo control systems and numerical control devices that apply them. The servo control system detects a movement amount of an actuator such as a servo motor, that is, a position, controls a drive current of the servo motor according to a deviation between the detected position and a command position, and determines its rotation speed and position by PTP. (Point
t to Point) control. In addition, the numerical controller applies the above servo control system to the CP
(Continuous Path) control (contour control, synchronous operation control, etc.) can be performed. Therefore, the numerical control device is a positioning control system capable of controlling a large number of axes, and is mainly used in machine tools and the like that require high precision and high speed.

A large number of processing machines, the positioning of which is controlled by such a positioning device, are arranged on the transfer line. A programmable controller is connected to each positioning device in order to perform sequence control of the positioning operation of the processing machines arranged in large numbers along the transfer line. The programmable controller supplies a position command signal for positioning to each positioning device, and outputs an ON / OFF command for the positioning control. That is, the positioning device operates according to an on / off command and a position command from the programmable controller to position the machining head of the machining machine at a predetermined position.

Further, in addition to the processing machine, a large number of peripheral devices which are on / off controlled by a programmable controller are distributed around the transfer line. Conventionally, in such a system, in order to prevent interference between processing machines, interference between processing machines and workpieces, and mechanical interference between processing machines or peripheral equipment on the transfer line, each positioning is performed. A program for directly preventing interference was built into the operation program of the equipment, and interlocking was performed between the processing machines.

[0006]

However, in a system in which the operation sequence of the processing machine, the work shape, the installation state of the processing machine, the layout of the transfer equipment, etc. are frequently changed, the operation program is changed each time. At the same time, the interference prevention program has to be changed, and there is a problem that a great deal of time is spent on the changing work. In particular, it is a serious problem in a robot arm that freely moves in a three-dimensional space. It was. The present invention has been made in view of the above points, and an object of the present invention is to provide an interlock control system capable of preventing interference between machines without providing an interference prevention program in an operation program. And

[0007]

An interlock control system according to a first aspect of the present invention is based on a machine to be positioned, a position detecting means for detecting a current position of the machine, and a current position from the position detecting means. Positioning means for positioning the machine at a predetermined position, area specifying means for specifying an inoperable area or an operable area of the machine, the current position is input, and the machine will be positioned after a predetermined time elapses. Position predicting means for outputting a predicted position of a machine, area creating means for creating a moving area according to the shape of the machine based on the current position and the predicted position, and an inoperable area from the area designating means And the moving area from the area creating means are judged to interfere with each other, and when both areas interfere with each other, the interposition for stopping the positioning operation of the machine by the positioning means is stopped. Tsu are those composed of a click control means.

An interlock control system according to a second aspect of the present invention includes a plurality of machines that are independently positioned,
Position detecting means for respectively detecting the current positions of these machines, positioning means for respectively positioning the respective machines at predetermined positions based on the current positions from the position detecting means, and the current positions of the respective machines are inputted. Position predicting means for outputting a predicted position of the machine that will be positioned after a predetermined time has passed, area creating means for creating a moving area according to the shape of each machine based on the predicted position, and area creating From the interlock control means for determining the interference state between the movement areas of the respective machines based on the movement area of each machine from the means, and stopping the positioning operation by the positioning means of at least one of the machines determined to interfere. It is composed.

An interlock control system according to a third aspect of the present invention includes a plurality of machines that are independently positioned,
Position detecting means for detecting the current positions of these machines, positioning means for positioning the respective machines at predetermined positions based on the current positions from these position detecting means, and inoperable areas of the respective machines Alternatively, area specifying means for specifying an operable area, position predicting means for inputting the current position of each machine and outputting a predicted position of the machine that will be positioned after a predetermined time has passed, the current position and the prediction Area creating means for creating a moving area corresponding to the shape of each machine based on the position, an inoperable area or operable area from the area designating means, and a moving area of each machine from the area creating means The interference state between the moving areas of the respective machines and the interference state between the moving areas of the respective machines and the inoperable area based on It is those composed of the interlock control means for stopping the positioning operation by the positioning means.

[0010]

The machine to be positioned is a processing machine such as various machine tools or an arm of a robot. The position detecting means detects such a current position of the machine moment by moment. The positioning means positions the machine at a predetermined position based on the current position from the position detecting means. Conventionally, the positioning operation of the machine has been performed by incorporating an operation program in consideration of prevention of machine interference in such positioning means.

In the interlock control system according to the first aspect of the present invention, an area designating means, a position predicting means, an area creating means and an interlock control means are newly provided so that a machine to be positioned interferes with a work or other peripheral equipment. It is prevented in advance. The area designating means stores an area in which the machine to be positioned may mechanically interfere with the work or other peripheral equipment as an inoperable area,
Designate it as a direct inoperable area. Further, the area designating unit may store an area in which there is no possibility of mechanical interference as an operable area, and may designate the other area as an inoperable area. The position predicting means inputs the current position of the machine from the position detecting means and outputs the predicted position of the machine which will be positioned after a predetermined time has elapsed. The area creating means creates a moving area according to the shape of the machine based on the current position and predicted position of the machine. Therefore, when this moving area matches the inoperable area from the area designating means, both areas interfere with each other after a lapse of a predetermined time. Therefore, the interlock control means determines the interference state of the machine based on the inoperable area from the area designating means and the moving area from the area creating means. Stop the positioning operation. This prevents the machine from interfering with the work piece or other peripheral equipment. Further, when the operation sequence of the machine, the shape of the work, the installation state of the machine, the arrangement of other peripheral devices, etc. are changed, the inoperable area or operable area in this area designating means is rewritten accordingly. It's good enough, and rewriting the program is very easy. Also, in a preferred embodiment of the first invention, the interlock control means prevents the inoperable area and the moving area from interfering with each other after stopping the positioning operation of the machine. Positioning control by. As a result, even when the positioning operation of the machine is stopped, it is not necessary to manually perform the retracting operation of the machine, and it is not necessary to rewrite the operation program.

In the interlock control system according to the second aspect of the present invention, a position predicting means, a region creating means and an interlock control means are newly provided to prevent a plurality of positioned machines from interfering with each other in advance. ing. The position predicting means inputs the current position of each machine output from the position detecting means, and outputs the predicted position of the machine that will be positioned after a predetermined time has elapsed. The area creating means creates a moving area corresponding to each machine shape based on the current position and predicted position of the machine. Therefore, if there is a match in the moving regions of these machines, it means that the matched machines interfere with each other after a predetermined time has elapsed. Therefore, the interlock control means
The interference state between the moving areas of the respective machines is determined based on the moving area of each machine from the area creating means, and the positioning operation of the machine by at least one positioning means of the machines determined to interfere is stopped. As a result, the plurality of independently positioned machines do not interfere with each other. Further, it is not necessary to incorporate an interference prevention program in the operation program of each positioning means, and even when changing the operation sequence or positioning operation of the machine, it is not necessary to consider the relationship with other machines.
The rewriting work of the operating program becomes very easy. In a preferred embodiment of the second invention, the interlock control means stops one of the machines determined to interfere with each other, and then the moving areas of the machines determined to interfere with each other interfere with each other. When it disappears, the positioning operation by the positioning means of the stopped machine is restarted. As a result, even when the positioning operation of the machine is stopped, it is not necessary to manually perform the retracting operation of the machine, and it is not necessary to rewrite the operation program.

In the interlock control system according to the third invention, as in the first invention, the area designating means, the position predicting means, the area creating means, and the interlock controlling means are newly provided, and the same as the first invention. It is possible to prevent the plurality of machines positioned in the above from interfering with the work and other peripheral devices in advance, and to prevent the plurality of machines positioned in the same manner as the second invention from interfering with each other in advance. To prevent. That is, the third invention is a combination of the first invention and the second invention. by this,
The machine does not interfere with the work or other peripheral devices, and the plurality of machines positioned independently of each other do not interfere with each other. Further, when the operation sequence of the machine, the shape of the work, the installation state of the machine, the arrangement of other peripheral devices, etc. are changed, the inoperable area or operable area in this area designating means is rewritten accordingly. Moreover, it is not necessary to include a program for preventing interference with other machines in the operation program of each positioning means, and even when changing the operation sequence or positioning operation of the machine, other machines or workpieces Since it is not necessary to consider the relationship with the peripheral device etc., the rewriting work of the program becomes very easy.

[0014]

Embodiments of the present invention will now be described in detail with reference to the accompanying drawings. FIG. 1 is a block diagram showing a schematic configuration of a positioning control system incorporating an interlock control system which is an embodiment of the present invention. In the present embodiment, an interlock control system will be described in the case where a two-axis (X, Y) positioning control system performs positioning control of different machines A and B, respectively. In this figure, for convenience of description, only the X-axis positioning control system is shown, and the Y-axis part is omitted. Therefore, 2 for the X-axis and Y-axis
Position control units 3A and 3B, speed control units 4A,
4B, current control units 5A and 5B, position calculation units 6A and 6B,
Regarding the speed calculation units 7A and 7B, the servo motors 8A and 8B, and the position sensors 9A and 9B, X and Y are added after the reference numerals.
Will be added.

Servomotors 8AX and 8AY are used to drive machine A
The servo motors 8BX and 8BY are drive devices for moving the machine B on the XY plane. Servo motors 8AX, 8A
Y, 8BX, and 8BY are composed of, for example, synchronous AC servo motors.

Position sensors 9AX, 9AY, 9BX, 9B
Y is coupled to the rotary shafts of the servo motors 8AX, 8AY, 8BX, 8BY, respectively.
The current position of X, 8AY, 8BX, 8BY is detected absolutely. This position sensor 9AX, 9AY, 9B
As X and 9BY, for example, an inductive type phase shift type position sensor as disclosed in JP-A-57-70406 or JP-A-58-106691 is used. Output PAX of position sensors 9AX, 9AY, 9BX, 9BY,
PAY, PBX, PBY are position calculation units 6AX, 6AY,
It is input to 6BX and 6BY, where it is converted into digital current position data P1AX, P1AY, P1BX and P1BY.

The controller 1A reads the control program of the machine A from the built-in program memory and
Control the sequence operation of the servo motor 8AX,
The target position data PPA of 8AY is output to the position command unit 2A to control the positioning of the machine A. A program memory built in the controller 1A stores a sequence control program for performing sequence control, and a positioning control program including various positioning data and various positioning control parameters. Similarly, the controller 1B also reads the control program of the machine B from the built-in program memory to control the sequence operation of the machine B and outputs the target position data PPB of the servomotors 8BX and 8BY to the position command section 1B to output the machine B. Position control. The controllers 1A and 1B operate asynchronously and control the machine A and the machine B separately. However, interlock (mechanical interference) as described later
Only when the error occurs, the interlock control unit 20 functions to control the two while synchronizing with each other. The synchronization control during this interlock will be described later.

The position command unit 2A uses the servo motor 8AX based on the target position data PPA from the controller 1A.
Interpolation position command data ANX for driving (8AY)
(ANY) at predetermined time intervals one after another for the position control unit 3AX
Output to (3AY). However, the position command unit 2A is connected to the interlock control unit 20 and the locus calculation unit 13A, and cooperates with the interlock control unit 20 and the locus calculation unit 13A when mechanical interference may occur. , Avoiding the mechanical interference. Position command unit 2
B outputs interpolated position command data BNX (BNY) for driving the servomotor 8BX (8BY) to the position controller 3BX (3BY) one after another at predetermined time intervals based on the target position data PPB from the controller 1B. . However, the position command unit 2B is connected to the interlock control unit 20 and the locus calculation unit 13B, and cooperates with the interlock control unit 20 and the locus calculation unit 13B when mechanical interference may occur. , Avoiding the mechanical interference. For details of this mechanical interference avoidance operation, the interlock control unit 20 and the locus calculation units 13A, 13
Since it has a relationship with B, it will be described later.

The position control unit 3AX (3AY) has the interpolation position command data ANX (ANY) and the servo motor 8AX (8).
Current position data P1AX (P1) indicating the current position of AY)
AY) to input the interpolation position command data ANX (ANY)
And the current position data P1AX (P1AY) are respectively obtained, and speed command data FAX (FAY) corresponding to the position deviation is output to the speed control unit 4AX (4AY). The position control unit 3BX (3BY) receives the interpolation position command data BNX (BNY) and the servo motor 8BX (8B).
Y) present position data P1BX (P1B
Y) is input to obtain the position deviation between the interpolated position command data BNX (BNY) and the current position data P1BX (P1BY), and the speed command data FBX (FBY) corresponding to the position deviation is obtained as the speed control unit 4BX. Output to (4BY).

The position calculation unit 6AX (6AY) inputs the output PAX (PAY) from the rotational position sensor 9AX (9AY) and outputs it as digital current position data P1AX (P1).
It is converted into AY) and is output to the position control unit 3AX (3AY), the speed calculation unit 7AX (7AY), the position prediction unit 11A, the area creation unit 12A, and the trajectory calculation unit 13A. The position calculation unit 6AX (6AY) is a rotational position sensor 9A.
A phase signal PHAX (PHAY) for controlling the field switching position of the servo motor 8AX (8AY) is generated from the output PAX (PAY) of X (9AY), and the current control unit 5A is generated.
Output to X (5AY). Position calculator 6BX (6BY)
Is the output PBX from the rotational position sensor 9BX (9BY)
(PBY) is input, converted into digital current position data P1BX (P1BY), and the position control unit 3BX (3
BY), speed calculation unit 7BX (7BY), position prediction unit 11
B, the area creation unit 12B, and the trajectory calculation unit 13B. The position calculator 6BX (6BY) generates a phase signal PHBX (PHBY) for controlling the field switching position of the servo motor 8BX (8BY) from the output PBX (PBY) of the rotational position sensor 9BX (9BY). , To the current control unit 5BX (5BY). Position calculator 6A
The detailed configuration of X (6AY), 6BX (6BY) will be described later.

The speed calculation unit 7AX (7AY) is provided with the current position data P1AX (P1A) of the position calculation unit 6AX (6AY).
Y) is input, and based on the change amount of the current position data P1AX (P1AY) per predetermined unit time, speed data VAX (VAY) indicating the current speed of the servo motor 8AX (8AY) is calculated by digital calculation, and the speed is calculated. Control unit 4A
Output to X (4AY). Speed calculator 7BX (7BY)
Is the current position data P1B of the position calculation unit 6BX (6BY)
Input X (P1BY) and calculate speed data VBX (VBY) indicating the current speed of the servo motor 8BX (8BY) by digital calculation based on the amount of change in the current position data P1BX (P1BY) per predetermined unit time. , To the speed control unit 4BX (4BY).

The speed control unit 4AX (4AY) receives speed command data FAX (FA) from the position control unit 3AX (3AY).
Y) and speed data V from the speed calculator 7AX (7AY)
Input AX (VAY) and enter the speed command data FAX (F
AY) and speed data VAX (VAY) are found, and a servo motor 8AX (8) corresponding to the speed deviation is obtained.
AY) torque signal (current command signal) TAX (TAY)
Is output to the current control unit 5AX (5AY). The speed control unit 4BX (4BY) uses the speed command data FBX (FBY) from the position control unit 3BX (3BY) and the speed calculation unit 7BX.
The speed data VBX (VBY) from (7BY) is input, and the speed command data FBX (FBY) and the speed data VB are input.
The speed deviation from X (VBY) is obtained, and the torque signal (current command signal) TBX (TBY) of the servo motor 8BX (8BY) corresponding to this speed deviation is sent to the current controller 5BX (5).
Output to BY).

The current controller 5AX (5AY) has a torque signal TAX (TAY) and a phase signal PHAX (PHAY).
Is input to generate a three-phase PWM signal to drive the power transistor, and the servo motor 8AX (8
A drive current is supplied to each phase (U phase, V phase, W phase) of AY). At this time, the current detection isolator CT (not shown)
The current feedback signals of the U-phase and V-phase current values are fed back to the current controller 5AX (5AY). The current control unit 5AX (5AY) supplies a drive current obtained by amplifying the deviation between the torque signal (current command signal) TAX (TAY) of each phase and the current feedback signal of each phase to the servo motor 8AX (8AY). Current control unit 5BX (5
BY) is the torque signal TBX (TBY) and the phase signal P
Input HBX (PHBY), and based on it, enter the 3-phase P
Each phase (U phase, V phase, W phase) of the servo motor 8BX (8BY) is generated by generating a WM signal to drive the power transistor.
Drive current is supplied to. At this time, the current detection isolator CT (not shown) feeds back the current feedback signals of the U-phase and V-phase current values to the current controller 5BX (5BY). Current control unit 5BX (5BY)
Is the torque signal (current command signal) TBX (TB
The drive current obtained by amplifying the deviation between Y) and the current feedback signal of each phase is supplied to the servo motor 8BX (8BY).

Current control units 5AX (5AY), 5BX (5
BY) has a function of detecting control states such as servomotor overload, power supply voltage drop, overcurrent, overvoltage, and overheat. In addition, the servo status signal indicating these control states and the rating of the current amplifier. Indicating I
It has a memory for storing various data such as a D code and a motor rating code indicating the rating of the servo motor to be controlled, and the data stored in this memory has a data line (not shown) if necessary. ) To the controllers 1A and 1B.

The area designating section 10 includes servo motors 8AX,
8AY, 8BX, 8BY is a storage medium that stores area data AD indicating the inoperable area (or operable area) of the machines A and B that are moved by 8AY, and is composed of a replaceable IC card or the like. Therefore, this area designation unit 1
The inoperable area (or operable area) of the machines A and B can be changed to a desired one simply by changing the content of the area data AD in 0. Position predictor 11A, 11
B is the current position data P1AX, P1AY, P1BX, P1 from the position calculation units 6AX, 6AY, 6BX, 6BY.
Enter BY, and based on that, machines A and B after a predetermined time
The predicted position data PEA, PEB is output to the area creation units 12A, 12B. That is, since the position data changes every moment, the moving speeds of the machines A and B are obtained based on this. Therefore, the position predicting units 11A and 11B predict the positions of the machines A and B after the elapse of the predetermined time by obtaining the moving speed and multiplying it by the predetermined time. The position prediction unit 1
1A and 11B may input speed data VAX, VAY, VBX, and VBY from speed calculation units 7AX, 7AY, 7BX, and 7BY without multiplying the moving speeds of machines A and B, and multiply this by a predetermined time. .

The area creating units 12A and 12B are provided with the current position data P1AX, P1AY, P1BX, P1BY from the position calculating units 6AX, 6AY, 6BX and 6BY and the predicted position data PEAX and PEB from the position predicting units 11A and 11B.
Input Y and machine A, B based on these position data
Creates an area that moves from the current position to the predicted position and outputs it to the interlock control unit 20 as area data AA, AB.

The interlock control section 20 inputs the area data AA, AB from the area creating sections 12A, 12B and the area data AD from the area designating section 10, and determines whether the area data AA, AB, AD interfere with each other. (That is, whether mechanical interference occurs), and when it is determined that interference occurs, the interference state (area data AA and area data A)
B, interference between the area data AA and the area data AD, interference between the area data AB and the area data AD).
Control is performed and avoidance processing is performed so that the interference does not occur.

When the interlock control unit 20 determines that mechanical interference will occur, the locus calculation units 13A and 13B newly interpolate position command data AnX, AnY, BnX for avoiding the occurrence of the mechanical interference. , BnY are created and output to the position command units 2A and 2B. Therefore, when the interlock control unit 20 determines that the mechanical interference occurs, the position command units 2A and 2B determine whether the locus calculation unit 13 is operated.
Interpolation position command data AnX (An
Y), BnX (BnY) to the position control unit 3AX (3A
Y), 3BX (3BY).

The details of the mechanical interference avoiding operation performed by the interlock controller 20 will be described below with reference to the drawings. FIG. 2 shows an operation concept when the interlock control unit 20 moves the machine A to a target position while avoiding interference between the work and other peripheral devices in the middle of the movement path of the machine A. It is a figure.

In FIG. 2, the shape of the machine A on the XY plane is a square, and its movement path is A1-A5. That is, although the movement route of the machine A is A1-A5, the square portion of the machine A is actually the movement route A1-A5.
Since it moves along the path 5, the square portion of the machine A moving along the movement path A1-A5 must not interfere with other machines, works and the like. In FIG. 2, this movement route A1
-Since the inoperable area AD (area data from the area designating unit 10) of the machine A exists in the middle of A5, the interlock control unit 20 causes the square portion of the machine A to move along the movement path A1-.
When moving A5, its moving route is appropriately changed so as not to interfere with the inoperable area AD.

First, in FIG. 2 (a), the machine A is moving along the movement path A1-A5 and is located at the position A1.
Therefore, the position calculation units 6AX and 6AY output the current position data P1A1X and P1A1Y indicating the position A1. The position prediction unit 11A uses the current position data P1A1X, P1A
After a predetermined time based on 1Y, the machine A moves to the position A2.
Predicted position data PEA2
X and PEA2Y are output to the area creation unit 12A. The area creation unit 12A outputs area data AA12 as shown in FIG. 2A based on the current position data P1A1X, P1A1Y from the position calculation units 6AX, 6AY and the predicted position data PEA2X, PEA2Y from the position prediction unit 11A. . In this area data AA12, the square portion of the machine A is at position A1.
The area through which the vehicle moves from to the predicted position A2 is shown. Therefore, the area data AA12 and the inoperable area (area data from the area designating unit 10) AD must not interfere with each other. In FIG. 2A, area data AA12
And the inoperable area AD do not interfere with each other, the interlock control unit 20 does not operate at all, and the position command unit 2A
Outputs the interpolated position command data A2X, A2Y to the position control units 3AX, 3AY as usual. This allows machine A
Moves to position A2.

In FIG. 2B, the machine A is located at the position A2. Therefore, the position calculation units 6AX and 6AY are
The current position data P1A2X, P1A2Y indicating 2 is output, and the position predicting unit 11A determines that the machine A has moved to the position A3 after a predetermined time.
Is predicted to be located at, and the predicted position data PEA3X,
The PEA 3Y is output to the area creating unit 12A. The area creation unit 12A is based on the current position data P1A2X, P1A2Y from the position calculation units 6AX, 6AY and the predicted position data PEA3X, PEA3Y from the position prediction unit 11A.
The area data AA23 as shown in (b) is output. This area data AA23 indicates that the machine A has a predicted position A3 from the position A2.
Indicates the area to pass through when moving to. Therefore, this area data AA23 is an inoperable area (area designation unit 10
Area data from) AD must not interfere, but FIG.
In (b), the area data AA23 and the inoperable area AD interfere with each other. Therefore, the interlock control unit 20 outputs the stop signal STA to the position command unit 2A. The position command unit 2A which receives the stop signal STA outputs the interpolated position command data A3X and A3Y to the position control units 3AX and 3AY.
Will not be output to the previous interpolation position command data A2X,
A2Y is continuously output to the position control units 3AX and 3AY. Therefore, the machine A remains stopped at the position A2.

The position command unit 2A which receives the stop signal STA outputs to the interlock control unit 20 the interpolated position command data which is to be output thereafter. That is,
The position command unit 2A outputs the interpolated position command data A3X, A3Y corresponding to the position A3 to the interlock control unit 20. The interlock control unit 20 that receives the interpolated position command data A3X and A3Y determines whether the machine A can be located at the position A3 based on the inoperable region (region data from the region designating unit 10) AD. to decide. In this case, since the machine A cannot be located at the position A3, the interlock control unit 20 instructs the position command unit 2A to output the next interpolation position command data. Then, this time, the position command unit 2A outputs the interpolated position command data A4X, A4Y corresponding to the position A4 to the interlock control unit 20. Whether the machine A can be located at the position A4 on the basis of the inoperable region (region data from the region designating unit 10) AD in the same manner as the interlock control unit 20 to which the interpolated position command data A4X and A4Y are input. Determine whether In this case, since the machine A can be located at the position A4, the interlock control unit 20 causes the interpolated position command data A4X, A4Y and the area data AD corresponding to the position A4.
Is output to the locus calculation unit 13A.

The locus calculation unit 13A receives the interpolated position command data A4X and A4Y from the interlock control unit 20, the area data AD, and the current position data P1A2X and P1A2Y from the position calculation units 6AX and 6AY, and inputs the area data. The square portion of the machine A avoids the inoperable area specified by the data AD and the current position data P1A2X, P1A
Interpolation position command data A4 from position A2 specified by 2Y
A new movement route (trajectory) that can move to the position A4 specified by X and A4Y is calculated. That is, the trajectory calculation unit 1
3A is located at positions A1-Ab-A as shown in FIGS.
Interpolated position command data corresponding to c-A4 is sequentially output to the position command unit 2A. The position command unit 2A is the locus calculation unit 13
The interpolated position command data from A is successively transferred to the position control unit 3A.
X, 3AY is output, and the machine A is moved as shown in FIGS. 2 (c) to 2 (f). Even in this case, the area creating unit 12
A is area data AA2 through which the square part of machine A passes
Since a, AAAb, AAbc, and AAc4 are output to the interlock control unit 20, the interlock control unit 20 checks for interference with other machines and inoperable areas.

As shown in FIG. 2 (g), when the machine A avoids the inoperable area AD and reaches the position A4, the position command section 2A transfers the interpolated position command data A5X, A5Y to the position control section 3AX, 3AY, and the machine A is moved to the final target position A5. As described above, even if the inoperable area AD (area data from the area designating unit 10) of the machine A exists in the middle of the moving paths A1 to A5, the interlock control unit 20 causes the square portion of the machine A to move. A1-A5
The machine A can be moved to the final target position A5 by appropriately changing the movement route so as not to interfere with the inoperable area AD when moving.

FIG. 3 shows that, when the moving path of the machine A and the moving path of the machine B intersect with each other, the interlock control section 20 avoids the interference between the two and sets the machines A and B at the target positions. It is a figure which shows the operation concept at the time of moving.
In FIG. 3, the shape of the machine A on the XY plane is the same as that of FIG. 2, and its movement path is A1-A6. That is, although the movement route of the machine A is A1-A6, the square portion of the machine A actually moves along the movement route A1-A6. Therefore, the machine A that moves along the movement route A1-A6. The square part of must not interfere with other machines B, work, etc. The shape of the machine B on the XY plane is a rectangle larger than that of the machine A, and its movement path is B.
1-B6. That is, the movement route of the machine B is B1-
Although it is B6, since the rectangular part of the machine B actually moves along the movement paths B1-B6, the movement path B
The rectangular portion of machine B moving along 1-B6 must not interfere with other machines A, workpieces, etc. In FIG. 3, since both of them have moved along the moving paths A1-A6 of the machine A and the moving paths B1-B6 of the machine B, the interlock control unit 20 causes the machines A and B to interfere with each other. The movement operation is appropriately changed so as not to do so.

First, in FIG. 3A, the machine A is moving along the moving paths A1-A6 and is located at the position A1, and similarly, the machine B is moving along the moving paths B1-B6 and is located at the position B1.
Is located in. Therefore, the position calculation units 6AX and 6AY are configured to detect the current position data P1A1X and P1A1Y indicating the position A1.
The position calculation units 6BX and 6BY output current position data P1B1X and P1B1Y indicating the position B1. The position prediction unit 11A uses the current position data P1A1X, P1.
After a predetermined time based on A1Y, the machine A moves to the position A.
Predicted to be located at position 2, and the predicted position data PEA
2X and PEA2Y are output to the area creation unit 12A. The position prediction unit 11B uses the current position data P1B1X, P1B
After a predetermined time based on 1Y, the machine B moves to the position B2.
Position data PEB2
The X and PEB2Y are output to the area creating unit 12B.

The area creating section 12A includes position calculating sections 6AX, 6AX.
Current position data P1A1X, P1A1Y from AY and predicted position data PEA2X, PE from the position prediction unit 11A
Area data AA as shown in FIG. 3A based on A2Y
12 is output. The area data AA12 indicates an area through which the square portion of the machine A passes when moving from the position A1 to the predicted position A2. The area creation unit 12B uses the current position data P1B1X, P1 from the position calculation units 6BX, 6BY.
B1Y and predicted position data PEB from the position prediction unit 11B
Area data AB12 as shown in FIG. 3A is output based on 2X and PEB2Y. The area data AB12 indicates an area through which the rectangular portion of the machine B passes when moving from the position B1 to the predicted position B2. Therefore, the area data AA12 of the machine A and the area data AB12 of the machine B must not interfere with each other. In FIG. 2A, area data A
Since the A12 and the area data AB12 do not interfere with each other, the interlock control section 20 does not operate at all and the position command section 2A. 2B is the interpolation position command data A2X,
A2Y, B2X, B2Y are connected to position control units 3AX, 3AY,
Output to 3BX and 3BY. As a result, the machine A moves to the position A2 and the machine B moves to the position B2.

In FIG. 3B, the machine A is located at the position A2 and the machine B is located at the position B2. Therefore, the position calculation units 6AX and 6AY are configured to detect the current position data P indicating the position A2.
1A2X and P1A2Y are output, the position prediction unit 11A predicts that the machine A will be located at the position A3 after a predetermined time, and the predicted position data PEA3X and PEA3Y are output to the area creation unit 1
Output to 2A. Similarly, the position calculation units 6BX and 6BY output current position data P1B2X and P1B2Y indicating the position B2, and the position prediction unit 11B predicts that the machine B will be located at the position B3 after a predetermined time, and the predicted position data PEB3.
The X and PEB3Y are output to the area creating unit 12B. The area creation unit 12A outputs area data AA23 as shown in FIG. 3B based on the current position data P1A2X, P1A2Y from the position calculation units 6AX, 6AY and the predicted position data PEA3X, PEA3Y from the position prediction unit 11A. . This area data AA23 indicates an area that the machine A passes through when moving from the position A2 to the predicted position A3. The area creation unit 12B outputs area data AB23 as shown in FIG. 3B based on the current position data P1B2X, P1B2Y from the position calculation units 6BX, 6BY and the predicted position data PEB3X, PEB3Y from the position prediction unit 11B. . This area data AB23 indicates that the machine B has predicted the position B2 from the position B2.
The area that passes through when moving up to 3 is shown.

This area data AA23 and area data AB
23 should not interfere with each other, but in FIG. 3 (b) they do. Therefore, the interlock control unit 20 outputs the stop signals STA and STB to the position command units 2A and 2B. The position command units 2A and 2B, which receive the stop signals STA and STB, interpolate position command data A3X, A3Y and B3.
X and B3Y are position control units 3AX, 3AY, 3BX and 3B.
No longer output to Y, the previous interpolation position command data A2
X, A2Y, B2X, B2Y are position control units 3AX, 3A
Continue to output to Y, 3BX, 3BY. Therefore, the machine A stops at the position A2 and the machine B remains stopped at the position B2.

In the case of FIG. 3B, while the machine A is stopped at the position A2, the machine B can move along the movement paths B1-B6 without mechanically interfering with the machine A. With machine B stopped at position B2, machine A
It is not possible to move along the movement paths A1 to A6 without mechanically interfering with. Therefore, the interlock control unit 2
In such a case, 0 stops the machine A and moves the machine B again according to the normal movement route B1-B6. if,
Interlock control is performed when mechanical interference occurs even if the machine B is moved with the machine A stopped at the position A2 or the machine B is moved with the machine B stopped at the position B2. The unit 20 stops one of the machines, and the other machine is stopped so as not to interfere with the stopped machine.
The avoidance movement may be performed as follows.

In FIG. 3C, machine A remains stopped at position A2 and machine B is located at position B3. Therefore, the position calculation units 6AX and 6AY output the current position data P1A2X and P1A2Y indicating the position A2, and the position prediction unit 11A also predicts that the machine A will be located at the position A2 after a predetermined time, and outputs the predicted position data PEA2X and PEA2Y. It is output to the area creation unit 12A. That is, the position calculation unit 6
The AX, 6AY and the position predicting unit 11A output the same position data. On the other hand, the position calculation units 6BX, 6BY output current position data P1B3X, P1B3Y indicating the position B3,
The position prediction unit 11B predicts that the machine B will be located at the position B4 after a predetermined time, and predicts the position data PEB4X, PE
B4Y is output to the area creating unit 12B.

Area creating unit 12 in which the same position data is input
A outputs the square portion where the machine A is located at the position A2 as the area data AA. However, since this area data AA is in the stopped state of the machine A, the interlock control unit 20 uses the stored previous area data AA23 as it is for the interference check. On the other hand, the area creation unit 12B generates the area data AB34 as shown in FIG. 3C based on the current position data P1B3X, P1B3Y from the position calculation units 6BX, 6BY and the predicted position data PEB4X, PEB4Y from the position prediction unit 11B. Output. This area data AB34 indicates an area that the machine B passes through when moving from the position B3 to the predicted position B4. The area data AA23 and the area data AB34 should not interfere with each other, but they still interfere with each other in FIG. 3 (c). Therefore, the interlock control unit 20 continues to output the stop signal STA to the position command unit 2A, and the machine A
Continue to be stopped at position A2.

In FIG. 3D, machine A remains stopped at position A2 and machine B is located at position B4. Therefore, the position calculation units 6AX and 6AY and the position prediction unit 11A
(C) The same position data is output as in the previous time. On the other hand, the position calculation units 6BX and 6BY output current position data P1B4X and P1B4Y indicating the position B4, and the position prediction unit 11B predicts that the machine B will be located at the position B5 after a predetermined time, and outputs the predicted position data PEB5X and PEB5Y. It is output to the area creating unit 12B.

The area creating unit 12B includes position calculating units 6BX and 6BX.
Current position data P1B4X, P1B4Y from BY and predicted position data PEB5X, PE from the position prediction unit 11B
Based on B5Y and area data AB as shown in FIG.
45 is output. This area data AB45 indicates an area that the machine B passes through when moving from the position B4 to the predicted position B5. Therefore, the interlock control unit 20 stores the previously stored area data AA23 and area data AB4.
Interference check with 5 In FIG. 3D, since the area data AA23 and the area data AB45 do not interfere with each other, the interlock control unit 20 operates the position command unit 2
The output of the stop signal STA to A is stopped. The position command unit 2A stops outputting the previous time when the stop signal STA is no longer input, and the interpolation position command data A3X, A3
Y is output to the position control units 3AX and 3AY. The position command unit 2B outputs the interpolation position command data B5X, B5Y to the position control units 3BX, 3BY as usual. This causes the machine A to move to the position A3 and the machine B to move to the position B5.

In FIG. 3 (e), the machine A is located at position A3 and the machine B is likewise located at position B5. Therefore,
The area creation unit 12A includes the current position data P1A3X and P1A3Y from the position calculation units 6AX and 6AY and the position prediction unit 11
Based on the predicted position data PEA4X and PEA4Y from A, the area data AA34 as shown in FIG. 3 (e) is output. The area creation unit 12B outputs area data AB56 as shown in FIG. 3E based on the current position data P1B5X, P1B5Y from the position calculation units 6BX, 6BY and the predicted position data PEB6X, PEB6Y from the position prediction unit 11B. . Since the area data AA34 of the machine A and the area data AB56 of the machine B do not interfere with each other, the interlock control section 20 does not operate at all, and the position command section 2A outputs the interpolated position command data A4X and A4Y as usual. Part 2
B is interpolation position command data B6X, B6Y
Output to AX, 3AY, 3BX, 3BY. As a result, the machine A moves to the position A4 and the machine B moves to the final target position B6.

In FIG. 3 (f), the machine A is located at the position A4, and the machine B is stopped after reaching the final target position B6.
Therefore, the position calculation units 6AX and 6AY output the current position data P1A4X and P1A4Y indicating the position A4, and the position prediction unit 11A also predicts that the machine A will be located at the position A5 after a predetermined time, and the predicted position data PEA5X and PEA5Y.
Is output to the area creating unit 12A. On the other hand, the position calculator 6
BX and 6BY are current position data P1B6 indicating the position B6.
X, P1B6Y is output, the position prediction unit 11B predicts that the machine B will be located at the position B6 after a predetermined time, and outputs the predicted position data PEB6X, PEB6Y to the area creation unit 12B. That is, the position calculation units 6AX and 6AY and the position prediction unit 11A output the same position data corresponding to the stop position B6.

Area creating unit 12 in which the same position data is input
B outputs the rectangular portion in the state where the machine B is located at the position B6 as the area data AB. On the other hand, the area creation unit 1
2A is based on the current position data P1A4X, P1A4Y from the position calculation units 6AX, 6AY and the predicted position data PEA5X, PEA5Y from the position prediction unit 11B.
The area data AA45 as shown in (f) is output. This area data AA45 indicates that the machine A has predicted position A5 from position A4.
Indicates the area to pass through when moving to. Since the area data AA45 of the machine A and the area data AB6 of the machine B do not interfere with each other, the interlock control unit 20 does not operate at all, and the position command unit 2A operates the interpolated position command data A as usual.
5X, A5Y and A6X, A6Y to the position control unit 3AX,
Output to 3AY. As a result, the machine A moves to the final target position A6 via the position A5.

Next, the position sensor 9 used in this embodiment
The detailed configuration of the AX, 9AY, 9BX, and 9BY will be described. FIG. 4 is a diagram showing an absolute type position sensor composed of an inductive type phase shift type position sensor which is an example of the position sensor used in this embodiment. The details of this position sensor are known in JP-A-57-70406 or JP-A-58-106691, and therefore will be briefly described here.

The position sensor includes a stator 71a in which a plurality of poles A to D are provided at predetermined intervals in the circumferential direction (90 degrees as an example), and a stator 71a surrounded by the poles A to D.
And a rotor 71b inserted in the space. The rotor 71b has a shape and a material that change the reluctance of the poles A to D according to the rotation angle, and is, for example, an eccentric cylindrical shape. 1 for each pole A to D of the stator 71a
The secondary coils 1A to 1D and the secondary coils 2A to 2D are respectively wound. A first pair of two poles A and C and a second pair of poles B and D, which are opposed to each other in the radial direction, are coiled so as to operate differentially and are differential. It is configured to cause a reluctance change.

The primary coils 1A and 1C wound on the first pole pair A and C are excited by the sinusoidal signal sinωt and the primary coil 1 wound on the second pole pair B and D.
B and 1C are excited by the cosine signal cosωt. As a result, their combined output signal Y is obtained from the secondary coils 2A to 2D. This combined output signal Y is a primary AC signal (excitation signal of the primary coil) sinω that serves as a reference signal.
The rotation angle θ of the rotor 71b with respect to t or cosωt
Signal Y = s phase-shifted by an electrical phase angle according to
in (ωt−θ).

Therefore, when the inductive type phase shift type position sensor as described above is used, the primary AC signal sinω
It is necessary to include a reference signal generator that generates t or cosωt, and a phase difference detector that measures the electrical phase shift θ of the combined output signal Y and calculates rotor position data. The reference signal generator and the phase difference detector are composed of the position calculator 6AX,
It is provided in 6AY, 6BX, and 6BY.

FIG. 5 shows position calculation units 6AX, 6AY, 6B.
It is a figure which shows an example of X, 6BY. In FIG. 5, the position calculation unit is a reference signal generation unit that generates reference AC signals sinωt and cosωt, and a phase difference (phase shift amount) Dθ between the mutual induction voltage of the secondary coils 2A to 2D and the reference AC signal sinωt. And a phase difference detection unit for detecting

The reference signal generator comprises a clock oscillator 72, a synchronous counter 73, ROMs 74a and 74b, D / A converters 75a and 75b and amplifiers 76a and 76b, and a phase difference detector comprises an amplifier 77, a zero cross circuit 78 and a latch circuit. It consists of 79. The clock oscillator 72 generates a high-speed and accurate clock signal, and other circuits operate based on this clock signal. The synchronous counter 73 counts the clock signal of the clock oscillator 72 and outputs the count value as an address signal to the ROM 74a and the latch circuit 79 of the phase difference detector.

The ROMs 74a and 74b store amplitude data corresponding to the reference AC signal, and the synchronous counter 73
Amplitude data of the reference AC signal is generated according to the address signal (count value) from the. ROM74a is cosωt
, And the ROM 74b stores the sinωt amplitude data. Therefore, the ROMs 74a and 74b receive the same address signal from the synchronous counter 73 to generate two types of reference AC signals sinωt and cos.
Output ωt. Even if the ROM of the same amplitude data is read by the address signals having different phases, the same 2
A type of reference AC signal can be obtained.

The D / A converters 75a and 75b are the ROM 7
The digital amplitude data from 4a and 74b is converted into an analog signal and output to the amplifiers 76a and 76b. The amplifiers 76a and 76b amplify the analog signal from the D / A converter, and use it to amplify the reference AC signals sinωt and cos.
ωt is applied to each of the primary coils 1A, 1C and 1B, 1D. Assuming that the frequency division number of the synchronous counter 73 is M, that M count is the maximum phase angle 2π of the reference AC signal.
Corresponds to radians (360 degrees). That is, one count value of the synchronous counter 73 indicates a phase angle of 2π / M radian.

The amplifier 77 amplifies the combined value of the secondary voltage induced in the secondary coils 2A to 2D, and the zero cross circuit 7
Output to 8. The zero-cross circuit 78 detects a zero-cross point from a negative voltage to a positive voltage based on the mutual induction voltage (secondary voltage) induced in the secondary coils 2A to 2D of the position sensor, and outputs a detection signal to the latch circuit 79. .

The latch circuit 79 latches the count value of the synchronous counter started by the rising clock signal of the reference AC signal at the output point (zero cross point) of the detection signal of the zero cross circuit 78. Therefore, the value latched by the latch circuit 79 is exactly the phase difference (phase shift amount) Dθ between the reference AC signal and the mutual induction voltage (combined secondary output).

That is, the combined output signal Y = sin (ωt-θ) of the secondary coils 2A to 2D is given to the zero cross detecting means 78. The zero-cross detector 78 outputs the pulse L in synchronization with the timing when the electric phase angle of the combined output signal Y is zero. The pulse L is used as a latch pulse for the latch circuit 79. Therefore, the latch circuit 79 latches the count value of the synchronous counter 73 in response to the rising of the pulse L. The period in which the count value of the synchronization counter 73 makes one cycle is synchronized with one cycle of the sine wave signal sinωt. Then, the reference alternating current signal sinω is input to the latch circuit 79.
The phase difference θ between t and the composite output signal Y = sin (ωt−θ)
The count value corresponding to is latched. Therefore, the latched value is output as digital position data Dθ. The latch pulse L can also be used as a timing pulse as appropriate. In addition, the latch circuit 79
Of the values latched in, the value indicating the absolute position within one rotation of the servo motor is the digital phase data PHAX, PHA.
It is output as Y, PHBX, and PHBY and is used for controlling the switching position of the field.

Although FIG. 4 and FIG. 5 detect the range of one rotation absolutely, a plurality of such absolute sensors may be combined to detect the absolute position over multiple rotations.

In the embodiment shown in FIG. 1, the mechanical interference prevention between the two machines A and B whose positions are controlled by two axes (X axis and Y axis) has been described as an example. The number of axes to be set is not limited to two, and may be one or three, or four or five such as a robot arm, and the number of machines to be positioned is not limited to two and three or more. It may be one. When the number of control axes is three or more, the area specifying unit and the area creating unit may respectively specify and create the area of the three-dimensional space.

Further, in the above embodiment, the machine A and the machine B are
The case where the operation of the machine A is stopped in the case of mechanical interference or the like has been described, but not limited to this, both are stopped, and then the interference avoidance operation is manually performed, and the manual interference avoidance operation ends. At that point, the controller may automatically perform the automatic control again. In this case, it is possible that the machine position is significantly displaced from the target route by the manual interference avoidance operation.In such a case, find the shortest route from the machine position to the target route and move it. Good.

[0063]

As described above, according to the interlock control system of the present invention, it is possible to effectively prevent mechanical interference without providing an interference prevention program in the operation program. is there.

[Brief description of drawings]

FIG. 1 is a diagram showing a schematic configuration of a positioning control system incorporating an interlock control system of the present invention.

FIG. 2 is a diagram illustrating a case where a work or other peripheral device exists in the middle of a moving path of a machine to be positioned, when the interlock control unit of FIG. 1 avoids interference between the two and moves the machine to a target position. It is a figure which shows an operation concept.

FIG. 3 is a diagram illustrating a case where a moving path of a machine A and a moving path of a machine B to be positioned intersect each other, the interlock control unit of FIG. It is a figure which shows the operation concept at the time of moving to a target position.

4 is a diagram showing an absolute type position sensor including an inductive type phase shift type position sensor which is an example of the position sensor of FIG.

5 is a diagram showing a detailed configuration of a position calculation unit in FIG.

[Explanation of symbols]

1A, 1B ... Controller, 2A, 2B ... Position command section,
3AX, 3AY, 3BX, 3BY ... Position control unit, 4A
X, 4AY, 4BX, 4BY ... Speed control unit, 5AX, 5
AY, 5BX, 5BY ... Current control unit, 6AX, 6AY,
6BX, 6BY ... Position calculation unit, 7AX, 7AY, 7B
X, 7BY ... Speed calculator, 8AX, 8AY, 8BX, 8
BY: Servo motor, 9AX, 9AY, 9BX, 9BY
... position sensor, 10 ... area designation section, 11A, 11B ... position prediction section, 12A, 12B ... area creation section, 13A, 13
B ... Locus calculation unit, 20 ... Interlock control unit

Claims (6)

[Claims] 1. A machine to be positioned, position detecting means for detecting a current position of the machine, positioning means for positioning the machine at a predetermined position based on the current position from the position detecting means, and the machine. Area designating means for designating an inoperable area or an operable area, a position predicting means for inputting the current position, and outputting a predicted position of the machine that will be positioned after a predetermined time has passed, the current position and Area creating means for creating a moving area according to the shape of the machine based on the predicted position, and an interference state between the inoperable area from the area designating means and the moving area from the area creating means is determined. And an interlock control means for stopping the positioning operation of the machine by the positioning means when both areas interfere with each other. Control system. 2. The interlock control means performs positioning control by the positioning means of the machine after stopping the positioning operation of the machine, so that the inoperable area and the moving area do not interfere with each other. The interlock control system according to claim 1, wherein: 3. A plurality of machines that are respectively positioned independently, position detection means for detecting the current positions of these machines, respectively, and the respective machines are set to predetermined positions based on the current positions from the position detection means. Positioning means for respectively positioning, position predicting means for inputting the current position of each machine and outputting a predicted position of the machine that will be positioned after a predetermined time has passed, and shape of each machine based on the predicted position Area creating means for creating a moving area according to the, and determining the interference state of the moving areas of each machine based on the moving area of each machine from the area creating means, at least one of the machines determined to interfere And an interlock control means for stopping the positioning operation by the positioning means. 4. The interlock control means, after stopping one of the machines determined to interfere with each other, stops moving when the moving regions of the machines determined to interfere with each other do not interfere with each other. The interlock control system according to claim 3, wherein the positioning operation of the positioning means of the machine is restarted. 5. A plurality of machines positioned independently of each other, position detection means for detecting the current positions of these machines, respectively, and a predetermined position for each machine based on the current positions from these position detection means. Positioning means for respectively positioning the machine, area specifying means for specifying an inoperable area or an operable area of each machine, a current position of each machine is input, and the machine will be positioned after a predetermined time has elapsed. A position predicting unit that outputs a predicted position, a region creating unit that creates a moving region corresponding to the shape of each machine based on the current position and the predicted position, an inoperable region from the region designating unit, or Based on the operable area and the movement area of each machine from the area creating means, the interference state between the movement areas of each machine and the movement area of each machine and the inoperability An interlock control system comprising: an interlock control unit that determines an interference state with a region and stops a positioning operation by the positioning unit of the machine that is determined to interfere. 6. The interlock control means, when determining that the inoperable area from the area designating means and the moving area from the area creating means interfere with each other,
After the positioning operation of the machine is stopped, the positioning means of the machine performs positioning control so that the inoperable area and the moving area do not interfere with each other, and the moving area of each machine from the area creating means. When it is determined that the machines interfere with each other, after stopping one of the machines determined to interfere with each other, when the moving areas of the machines determined to interfere with each other no longer interfere with each other, the machine stopped again. The interlock control system according to claim 5, wherein the positioning operation by the positioning means is started.