KR930007776B1 - Linear interpolating method of robot - Google Patents

Abstract

A linear interpolation method for robot comprises several steps including obtaining the positions of the robot on X-Y plain, obtaining the object positions on X-Y plain, calculating the deviations of respective shafts, calculating the distances to move at every sampling time and the length of linear lines, calculating the linear motion velocities to get the velocity field of exponential function and moving distances of X and Y shafts, calculating position increments on joint coordinate and the object positions of respective shafts, and calculating the positional deviations of the shafts at an instance and controlling them in proportional integration or defferentiation to use as a signal for motor driving.

Description

Linear interpolation method of robot

1 is a conceptual diagram of linear transfer of the end effector of a robot in the X-Y plane of a horizontal articulated robot.

2 is a graph of a case in which the feed speed of a straight line is constant during a straight line transfer in the X-Y plane on a rectangular coordinate.

3 is a graph in which the feedrate of a straight line is exponentially calculated during the straight line transfer in the X-Y plane on the rectangular coordinates.

4 is a graph showing the increment of the straight line to be transferred at every sampling time in the absence of acceleration and deceleration.

5 is an exponential velocity distribution diagram during linear transfer.

6 is a block diagram for realizing the linear interpolation method of the robot of the present invention.

7 is a flowchart showing the operation procedure of the present invention.

* Explanation of symbols for main parts of the drawings

1: microprocessor 2: digital-to-analog converter

3: Servo controller 4: Servo motor

5: taco generator 6: encoder

7: up / down counter 8: timer

The present invention relates to a method for linearly transporting the end-effector of the arm of an articulated robot in the XY plane of the Cartesian coordinate system. The present invention relates to a linear interpolation method of a robot capable of high-speed movement as well as shortening the sampling time by improving exponential function acceleration and deceleration by the discrete time state equation.

In general, when a robot interpolates a straight line to a certain distance, and operates a point-to-point (PTP) by the minute distance of the straight line to drive the robot in the path of the divided straight line However, this method has a problem in that the accuracy of linear movement is lowered when the robot starts and ends the drive.

In addition, the welding or painting work using the robot or when there is an obstacle in the movement path of the robot, there is a problem that must follow a given path in an extremely limited working environment.

Accordingly, the present invention has been made in view of the above and the above problems, and an object of the present invention is to accelerate and decelerate the speed distribution by an exponential function during linear transfer of the robot, thereby improving the accuracy of the linear movement at the start and end of the robot. To provide a linear interpolation method.

Another object of the present invention is to provide a linear interpolation method of a robot which can improve the accuracy of a straight line and at the same time can perform a high speed straight line transfer by realizing the exponential function acceleration / deceleration by a discrete time state equation.

In order to achieve the above object, the linear interpolation method of the robot according to the present invention includes reading a current value in the joint coordinates of a horizontal articulated robot and obtaining a position in the XY plane, and reading a final target value in the joint coordinates. A step for obtaining a target position value in the XY plane, a step for calculating a position deviation of each axis in the XY plane, a step for finding the length of the straight line to be moved at every sampling time in the absence of the length and acceleration / deceleration, and an exponential function Steps to find the velocity at the time of linear movement to obtain velocity distribution, to calculate the moving distance of X and Y axes, to obtain the position increment amount in joint coordinate system, to find the target position of each axis at a certain point, and to It is characterized by consisting of the steps to obtain the position deviation and control the proportional integral derivative to use as a motor drive signal.

Hereinafter, an embodiment of the present invention will be described in detail with reference to the drawings.

1 is a conceptual diagram of the linear transfer of the end effector of the robot in the XY plane of the horizontal articulated robot, and FIG. 2 is a graph of the case where the linear feedrate is uniform during the linear transfer in the XY plane of the rectangular coordinates. Fig. 1 is a graph in which the feedrate of a straight line is exponential in the straight line transfer in the XY plane on the rectangular coordinate, and Fig. 4 is a graph showing the increment of the straight line to be transferred at every sampling time in the absence of acceleration and deceleration. 5 is an exponential velocity distribution diagram for linear transfer. 6 is a block diagram for realizing the linear interpolation method of the robot of the present invention, and FIG. 7 is a flowchart showing the operation procedure of the present invention.

1 shows a robot with two arms (a ₁ , a ₂ ) drawing a straight line from the current position (C1 (xi, yi)) to the target position (C ₂ (xf, yf)) in the XY plane of the rectangular coordinates. as written by showing the case of transfer, it is attached to the servomotor 4 and the point (a) with point (B) for driving each arm (a _1, a _2).

Also, FIG. 2 shows the lengths of the straight lines to be transferred at every sampling time when the feed speed of the straight lines is constant in the XY plane on the rectangular coordinates. Two point operation is achieved.

3 shows the length of the straight line to be transported every sampling time when the feed rate of the straight line is exponentially transferred in the X-Y plane on the rectangular coordinate.

Next, FIG. 6 is explained.

In Fig. 6, reference numeral 1 denotes a microprocessor for outputting a speed command of the servomotor 4, and reference numeral 2 denotes a digital for converting a speed command of a digital value output from the microprocessor 1 into an analog value. As a digital / analog converter, the converted analog value is inputted to a servo driver (not shown) to output a command signal and a drive speed signal for driving the servomotor 4 in the forward and reverse directions.

Further, (3) indicates that the feedback signal is returned when the current feedback signal Ix of the servomotor 4 and the speed feedback signal Vax detected by the taco generator 5 in accordance with the driving speed of the servomotor 4 are fed back. Is a servo control unit for servo control of the servo motor 4, and (7) up or down counts a pulse signal corresponding to the rotation amount of the servo motor 4 output from the encoder 6, thereby An up / down counter for inputting a position to the microprocessor 1, and (8) is a timer for interrupting the microprocessor 1 to determine the current position of the robot at regular intervals only for one axis. It is explained.

Next, FIG. 7 will be described with reference to FIGS. 1 to 6.

When the linear interpolation method of the robot according to the present invention is started, first, in step S1, as shown in FIG. 1, the present value (θ ₁ , θ ₂ ) in the joint coordinates of the horizontal articulated robot is determined by the up / down counter (7). ), And at step S2, the position (xi, yi) in the XY plane is obtained using the following equation.

xi = a ₁ cosθ ₁ + a ₂ cos (θ ₁ + θ ₂ ) ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''(One)

yi = a ₁ sinθ ₁ + a ₂ sin (θ ₁ + θ ₂ ) ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''(2)

to be.

Where a _1: length of the first arm of the robot, a _2: the length of the second arm of the robot.

When the position (xi, yj) in the XY plane is obtained, the process proceeds to step S3 to read the final target values θ ₃ and θ ₄ in the joint coordinates of the robot, and in step S4 the final target values θ read in the step S3. ₃ , θ ₄ ) to obtain the target position values (xf, yf) in the XY plane.

xf = a ₁ cosθ ₃ + a ₂ cos (θ ₃ + θ ₄ ) ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''(3)

yf = a ₁ sinθ ₃ + a ₂ sin (θ ₃ + θ ₄ )''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''(4)

Accordingly, as shown in FIG. 2, the positional deviation of each axis in the X-Y plane is obtained in step S5, and the amount of movement xp in the x-axis direction is

xp = xf-xi '' '' '' '' '' '' '' '' '' '' '' '' '' '' '' '' '' '(5)

Obtained by, and the movement amount yp in the y-axis direction

yp = yf-yi '' '' '' '' '' '' '' '' '' '' '' '' '' '' '' '' '(6)

Obtained by

Since the positional deviation (xp, yp) of each axis in the X-Y plane is obtained at step S5, the length Sp of the straight line is determined at step S6.

Where I is the integer part and R _{1 is the} fractional part

The length of the straight line to be moved at every sampling time when there is no acceleration / deceleration as shown in FIG. Sp) is obtained.

In other words, when the speed of the straight line is called V [mm / sec] and the time for periodically interrupting the microprocessor 1 by the timer 8, that is, when the sampling time is Ts [sec], The length of the straight line to be moved at every sampling time ( Sp) It can be represented by Sp = V × Ts.

Accordingly until the final straight position The number of times to go to Sp (N) Is obtained in step S8 (where Is a quotient, R2 is the remainder). For example, the straight line length Sp is 300.5 [mm], the straight line feed rate V is 100 [mm / sec], and the sampling time Ts is 4 In the case of [msec], the length of the straight line to be moved at every sampling time in the absence of the acceleration / deceleration ( Sp) Sp = V + Ts = 100 × 10 ^-3 = 0.4 [mm] The number of times to go to Sp (N) to be.

In other words, it has to be moved 751 times by 0.4mm, and only 75mm needs to be moved for the 752th time.

Therefore, in step S10, the speed at the time of linear transfer is calculated, and when it is calculated for every sampling time, the speed distribution at the time of linear transfer becomes exponential.

That is, as shown in FIG. 3, the speed Vsp (k) during linear transfer is

Vsp (k) = Vsp (k-1) + [ Sp (k) -Vsp (k-1)] / A '''''''''''''''''''' (8)

(Where Vsp (0) = 0, K = 0,1,2,3 ……)

In the above formula (8) Sp (k) is 0.4 mm for the 75th time, 0.1 mm for the 751th time, and zero after the 753th time.

In addition, the constant A is a constant representing the time constant of the exponential function. If A is too small, rapid acceleration and deceleration are required, and if A is too large, the positioning time becomes long, so the value of the constant A is appropriately determined according to the characteristics of the machine. Should be.

On the other hand, since the velocity Vsp at the straight line transfer obtained in step S10 has a division term, compensation is made in step S11 so as to reach the end of the straight line exactly.

That is, at any K point [ If the remainder of Sp (k) -Vsp (k-1)] / A is R (k), Instead of Sp (k + 1) After formulating Sp (k + 1) + R (k), the above formula (8) is calculated at the K + 1th time.

After the compensation for the division term in step S11, the flow advances to step S12 to determine the moving distances of the x-axis and the y-axis.

That is, the moving distance of the X axis (Xsp (k))

The travel distance of the y axis (Ysp (K))

to be.

Next, in step S13, the positional increment pulse in each joint coordinate system is calculated | required as follows.

Accordingly, in step S14, the target position for each axis at an arbitrary K time point is

Obtained by

Since the target position for each axis at an arbitrary K point is obtained in step S14, the positional deviation of each axis at any K point is determined in step S15. In other words,

P x (k) = Pxθ (k) -Cxθ (k)

P y (k) = Pyθ (k) -Cyθ (k)

(Where Cxθ (k) and Cyθ (k) are the current position values of the robot read by the up / down counter 7 for each axis at an arbitrary K time point).

Then, in step S16, Proportional Integral Differential (PID) control is performed on the positional deviation amount of each axis.

Where T is the sampling time, Dx out and Dy out are the values input to the digital-to-analog converter 7 of each axis, Kpx and Kdy are the proportional gains of each axis, and Kix and Kiy are the integral gains of each axis.

Kdx and Kdy are derivative gains of each axis.

Therefore, the above-mentioned Dx out (k) and Dy out (k) are output to the digital / analog converter 2, and the values converted into analog values by the digital / analog converter 2 are input to the servo controller 3 to supply the motor. (4) to drive.

Subsequently, in step S17, the value of K + 1 is substituted into K, and the flow proceeds to step S18, where the length of the straight line for each sampling time when the value of K has no acceleration / deceleration ( It is judged whether or not the number N of Sp) is equal. If the result of the determination is the same (Yes), the length of the straight line for each sampling time when there is no acceleration in step S19 is determined. The remaining (R2) value obtained by obtaining the number N of Sp) is obtained. In step S20, the value of K is the length of the straight line for each sampling time ( It is determined whether it is greater than the number N of Sp). If the determination result is large (Yes), the flow advances to step S21 to calculate the value by substituting zero to the length of the straight line? Sp and determines whether or not it is the final target position in step S22.

If it is determined in step S22 that the final target position is satisfied (Yes), the robot is terminated. If the target position is not the final target position (No), the robot returns to step S10 and the subsequent operation is repeated.

On the other hand, in step S18, the value of K is the length of the straight line for each sampling time ( If it is not equal to the number N of Sp) (if K ≠ N or No), the process proceeds directly to step S20 to perform a subsequent operation, where the value of K is the length of the straight line for each sampling time. ( If it is not greater than the number N of Sp) (No), the flow advances to step S22 to determine whether the robot is the final target position.

On the other hand, the embodiment is for a horizontal articulated robot, but the present invention can be applied to the orthogonal robot, vertical articulated robot, of course.

As described above, according to the linear interpolation method of the robot, the arm end of the robot, that is, the end effector, moves along the straight line in the XY plane of the rectangular coordinate, so that the velocity distribution of the straight line is executed by the exponential function acceleration / deceleration by the discrete time state equation. It not only shortens the time and improves linear accuracy, but also has the excellent effect of linear movement at high speed.

Claims (1)

Reading the current value in the joint coordinates of the horizontal articulated robot and finding the position in the XY plane, reading the final target value in the joint coordinates, and calculating the target position value in the XY plane, and the angular axis in the XY plane. Steps to find the positional deviation of, step to find the length of the straight line to be moved at every sampling time if there is no length and acceleration / deceleration, Comprising the following steps: finding the weight of the position in the joint coordinate system and finding the target position of each axis at a certain point; and calculating the positional deviation of each axis at the fixed point, controlling the proportional integral, and using it as a motor drive signal. Linear interpolation method of the robot.