KR0160673B1 - Location control device and methdo of twin xy robot - Google Patents

Abstract

The present invention relates to a position control method and apparatus of a twin XY robot.

The present invention includes a main controller and a position controller for controlling the positions of the Y1 and Y2 axes, and the position control apparatus of the twin XY robot which transmits the results controlled by the main controller and the position controller to the servo driver. It is characterized by the fact that a plurality of feedforward gain adjusters are installed between the servo drive and the servo driver to eliminate the positional deviation according to the load load. Thus, the positional deviation between Y1 and Y2 axes can be reduced by half. This prevents damage and noise from the device due to mechanical torsion, ensures the stability of the system, and reduces the settling time for high speed operation.

Description

Position Control Method and Device of Twin XY Robot

1 is a schematic configuration diagram of a conventional twin XY robot.

2 is a block diagram of a schematic synchronization control algorithm of a conventional twin XY robot.

3 is a flowchart of a schematic position control system of a conventional twin XY robot.

4 is a block diagram of a synchronization control algorithm in the position control apparatus of a twin XY robot according to the present invention.

5 is a system flow chart of the position control device of the twin XY robot according to the present invention.

6 is a feedforward gain table for each axis for explaining the calculation of the gain of the Y1 axis and the Y2 axis in the position control method of the twin XY robot according to the present invention.

* Explanation of symbols for main parts of the drawings

21,41: Main controller 22a, 42a: (Y1-axis) position controller

22b, 42b: (Y2-axis) position controller 43a, 43B: Feedforward gain regulator

The present invention relates to a method for controlling the position of a robot. In detail, in a twin XY robot, the feedforward speed control of each axis is varied in consideration of the load load between the Y1 and Y2 axes according to the movement of the load. A method for controlling the position of a twin XY robot that can minimize the positional deviation by preventing the increase of the positional deviation between the Y1 and Y2 axes according to the load load, which is a problem of the prior art, and further performing the optimal control. will be.

Today, as the assembly and manufacture of products by robots in the industrial field is gradually increasing, the functions of the robot control system are gradually diversified and advanced.

According to the present invention, the feed forward gain is varied according to the position of the head moving due to the movement of the X axis during the synchronous driving of the twin XY robot, and thus the deviation value between the Y1 and Y2 axes generated during the movement of the Y axis is changed. As a technique for minimizing the mechanical twist of the twin XY robot and realizing a fast settling time, it is particularly suitable for a system that performs position control at high speed while performing synchronous control.

1, the device configuration of a conventional twin XY robot is schematically shown.

Referring to this, the conventional twin XY robot 10 is provided with one X axis 11 in the horizontal direction, and the Y1 axis 12a and the Y2 axis 12b in the vertical direction are provided at both ends of the X axis 11. Each is fixedly installed. In addition, motors 13a and 13b for moving the respective axes are respectively provided at the lower ends of the Y1 axis 12a and the Y2 axis 12b, and each motor 13a and 13b is a servo driver for driving the motor. 14a and 14b are provided, respectively. The servo drivers 14a and 14b are electrically connected to a controller 15 which controls the system as a whole.

Then, the operation of the conventional twin XY robot having such a configuration will be described with reference to FIGS. 1, 2, and 3.

2 is a block diagram of a synchronous control algorithm of a conventional twin XY robot, and FIG. 3 is a flowchart of a position control system of a conventional twin XY robot.

In the operation of the conventional twin XY robot having the configuration as shown in FIG. 1, the positional deviation between the Y1 axis 12a and the Y2 axis 12b generated during the movement causes mechanical twist. This leads to some problems. Therefore, in the twin structure, the performance of the entire system depends on how much the synchronous control of the Y1-axis 12a and the Y2-axis 12b can be performed in parallel. For this reason, the conventional twin XY robot position control system employs a synchronous control algorithm as shown in FIG. 2 to synchronously control two Y axes. In FIG. 2, e1 and e2 represent position error values of the Y1 axis 12a and the Y2 axis 12b, and V1 and V2 represent the position controllers 22a and 22b of the Y1 axis 12a and the Y2 axis 12b. Is the output, i.e., the input values of the servo drivers 23a and 23b. In addition, f1 and f2 represent the actual positions of the Y1 and Y2 axes received from the servo drivers 23a and 23b. The position control system of the conventional twin XY robot having such a synchronization control algorithm operates as in the flowchart of FIG.

That is, the target position data transmitted from the main controller 21 is read (31). Then, the position error, which is the difference between the target position data and the current position data fed back from the servo driver, is calculated, and the position error is used to perform P (proportional), I (integral), and D (differential) control. (32). Then, it determines whether the currently controlled axis is Y1 axis or Y2 axis, and if it is Y1 axis, sends PID control result to D / A (Digital / Analog) converter (not shown). (33).

On the other hand, for the synchronous control compensation of the Y2 axis, the position deviation of the Y1 axis and the Y2 axis is calculated (34), and the deviation compensation result multiplied by this position deviation and the compensation gain and the PID control result of the Y2 axis are added to the D / A converter. (35). The D / A converter then converts the controlled result into an analog speed voltage and inputs it to the servo driver (36). Accordingly, the servo driver controls the drive speed of the system according to the input speed command to reach the desired target position (37). This moves the system and control is complete.

However, the position control system of the conventional twin XY robot as shown in the synchronous control block diagram of FIG. 2 compensates for the change in the load load on the Y1 and Y2 axes due to the movement of the X axis, that is, the movement of the load. Because of this, there is a lot of deviation between Y1 and Y2 axes when the robot is driven with the load biased toward Y1 or Y2 axis. It is pointed out that one of the problems is that such a deviation causes mechanical torsion, which causes mechanical damage and noise to cause the system to not operate stably.

The present invention was created in order to improve the above problems, taking into account the load load between the Y1 and Y2 axes according to the movement of the load, the feedforward speed control is carried out in accordance with the variable load load of each axis according to the load load It is an object of the present invention to provide a method and apparatus for controlling a position of a twin XY robot which can prevent an increase in the positional deviation between the Y1 and Y2 axes and minimize the positional deviation.

In order to achieve the above object, a position control method of a twin XY robot according to the present invention includes: reading target position data transmitted from a main controller; Obtaining a position error which is a difference value between the target position data and the current position data fed back from the servo driver; Performing P (proportional), I (integral), and D (derivative) control using the position error and reading position data of an X axis; Determining whether the currently controlled axis is the Y1 axis or the Y2 axis, and in the case of the Y1 axis, adding the PID control result and the output of the feedforward gain regulator to the digital / analog converter; In the case of the Y2 axis in the determination, the position deviation of the Y1 axis and the Y2 axis is obtained, and the output of the feed forward gain regulator, the deviation compensation result multiplied by the position deviation and the compensation gain, and the PID control result of the Y2 axis are added to the digital / analog converter. ; Converting the controlled result into an analog speed voltage and inputting the result to the servo driver; And controlling the driving speed of the system according to the input speed command to reach a desired target position.

Further, in order to achieve the above object, the position control apparatus of the twin XY robot according to the present invention includes a main controller for controlling the system as a whole, and a respective position controller for controlling the positions of the Y1 and Y2 axes. In the twin XY robot position control device which transmits the result controlled by the controller and each position controller to the servo driver, a plurality of feed forwards are provided between the main controller and the servo driver to solve the positional deviation according to the load load classification. Its feature is that the gain regulator is installed.

Therefore, the positional deviation of Y1 and Y2 axes can be reduced by half, which prevents damage and noise of the device due to mechanical torsion, ensures the stability of the system, and reduces the settling time. This becomes possible.

Hereinafter, exemplary embodiments of the present invention will be described in detail with reference to the accompanying drawings.

4 is a block diagram of a synchronization control algorithm in the position control apparatus of the twin XY robot according to the present invention.

Referring to this, the position control device 40 of the twin XY robot of the present invention is provided with a main controller 41 for controlling the system as a whole, and receives a predetermined command signal from the main controller 41 and receives the Y1-axis and Two position controllers 42a and 42b are provided for controlling the position of the Y2 axis, respectively. In addition, between the main controller 41 and the servo drivers 44a and 44b for driving the motors of the Y1-axis and the Y2-axis, each of which is in charge of the Y1-axis and the Y2-axis, respectively, in order to eliminate the positional deviation according to the division of the load load. Two feedforward gain adjusters 431 and 43b are installed.

Then, the operation of the position control device of the twin XY robot of the present invention having the above configuration will be described with reference to FIG. 4 and FIG.

5 is a system flowchart of the position control device of the twin XY robot according to the present invention.

4 and 5, the system of the position control apparatus of the twin XY robot according to the present invention first reads the target position data transmitted from the main controller 41 (51). Then, the position error, which is the difference between the target position data and the current position data fed back from the servo drivers 44a and 44b, is calculated, and the position error is used to control P (proportional), I (integral), and D (differential). The position data of the X axis is read (52). Then, it determines whether the currently controlled axis is the Y1 axis or the Y2 axis, and in the case of the Y1 axis, adds the PID control result and the output of the feedforward gain regulator 43a to the D / A converter (not shown) (53, 54). ). In addition, in the case of the Y2 axis, as a result of the determination, the position deviation of the Y1 axis and the Y2 axis is calculated for the synchronous control compensation (55), and the output of the feed forward gain adjuster 43b is used to eliminate the position deviation according to the load load equation (56). ), The deviation compensation result obtained by multiplying the position deviation of the Y1 axis and the Y2 axis by the compensation gain and the PID control result of the Y2 axis are added to the D / A converter (57). Here, to further supplement the description with respect to the feed forward gain adjusters 43a and 43b, the feed forward gain adjusters 43a and 43b may be used to minimize the positional deviation between the Y1 and Y2 axes according to the movement of the load. The feedforward gain values Y1 and Y2 according to the position of the X-axis (load position) are first found by using a gain table as shown in FIG. That is, in the gain table of FIG. 6, the feedforward gain Y1 on the Y1 axis and the feedforward gain Y2 on the Y2 axis according to the movement of the load may be expressed by the following equations.

Here, A is the gain in the x-axis direction depending on the position of the load, B is the gain in the y-axis direction according to the position of the load, i represents the stroke.

When the feedforward gain Y1 on the Y1 axis and the feedforward gain Y2 on the Y2 axis are obtained by the above equation relationship, the input target position is differentiated into a speed component, and then the value is obtained as the feedforward gain. It is multiplied by (Y1) (Y2) to output to the servo driver input.

On the other hand, the D / A converter converts the controlled results into analog speed voltages and inputs them to the drivers 44b (58). Accordingly, the servo driver controls the drive speed of the system according to the input speed command to reach the desired target position (59). This moves the system and control is complete.

Although the description of the present invention so far has been described based on the feedforward controller, the present invention can of course be applied not only to the feedforward controller but also to the PID controller in the position controller.

As described above, the position control method and apparatus of the twin XY robot according to the present invention can reduce the positional deviation between the Y1 axis and Y2 axis by a feedforward gain adjuster in half, thereby resulting in a mechanical twist Damage and noise can be prevented, system stability can be secured, and settling time can be reduced to enable high speed operation.

Claims (4)

Reading the target position data transmitted from the master controller; Obtaining a position error which is a difference value between the target position data and the current position data fed back from the servo driver; Performing P (proportional), I (integral), and D (derivative) control using the position error and reading position data of an X axis; Determining whether the currently controlled axis is the Y1 axis or the Y2 axis, and in the case of the Y1 axis, adding the PID control result and the output of the feedforward gain regulator to the digital / analog converter; In the case of the Y2 axis in the determination, the position deviation of the Y1 axis and the Y2 axis is obtained, and the output of the feed forward gain regulator, the deviation compensation result multiplied by the position deviation and the compensation gain, and the PID control result of the Y2 axis are added to the digital / analog converter. ; Converting the controlled result into an analog speed voltage and inputting the result to the servo driver; And controlling the drive speed of the system according to the input speed command to reach a desired target position. 2. The method of claim 1, wherein the output of the feedforward gain adjuster modulates the feedforward gain divider. : Y1 and Y2 are the feed forward gains of Y1 and Y2 axes, A is the gain in the x-axis direction according to the load position, B is the gain in the y-axis direction according to the load position, and i represents the stroke. A method for controlling the position of a twin XY robot, which is obtained by multiplying the obtained feedforward gain by the position deviation. Position control of twin XY robot that has main controller to control the whole system and each position controller to control the position of Y1-axis and Y2-axis, and transmits the result controlled by main controller and each position controller to servo driver. An apparatus for controlling a position of a twin XY robot, wherein a plurality of feed forward gain adjusters are provided between the main controller and the servo driver in order to eliminate the positional deviation according to the division of load load. The position control apparatus of the twin XY robot according to claim 3, wherein at least one feed forward gain adjuster is installed in each axis to cover the Y1 and Y2 axes, respectively.