KR0161002B1 - Circular interpolation method of robot - Google Patents

Abstract

According to the present invention, when the starting point, the intermediate point and the end point position of the circular arc are read, when the circular interpolation is executed, the angle of the small arc is Δθ and when the angle of the circular arc is θ during the circular interpolation, Δθ is (θ-ΣΔθ ( i) a judgment step for recognizing whether or not acceleration / deceleration is greater than the first step; a first coordinate calculation step for obtaining coordinates of an arc at every sampling time if there is no acceleration / deceleration in the determination step; and an acceleration / deceleration at the determination step. The second coordinate calculation step of calculating the X and Y coordinates of the circular arc in Cosθ and Sinθ at each sampling time, and the acceleration and deceleration of each axis after the coordinates of the circular arc are obtained in the second calculation step. A third coordinate calculation step for obtaining coordinates, an error compensation step for compensating for an error occurring when obtaining coordinates on an arc in the third coordinate calculation step, and a left circular arc on which the error is compensated for in the step. The target position calculation step of obtaining the target position of each axis for circular interpolation at any point of time at each sampling time, and the current position value of each axis read through the target position calculation step are obtained, and then the position deviation of the X and Y axes is calculated. It consists of a control step for controlling the servo motor by performing proportional integral derivative feedforward control on the position deviation calculation step to be obtained and the position deviation inserted in the position deviation calculation step. It moves along this arc and the velocity distribution of the arc becomes exponential function, so smooth movement is possible without vibration at the beginning and the end of the circle, and the sampling time is increased because the exponential function is accelerated and decelerated by the discrete time state equation. Since it can be reduced, the degree of arc is improved.

Description

Circular interpolation method of robot

1 is a graph showing the speeds Vx and Vy of the X and Y axes during circular interpolation in the X-Y plane.

FIG. 2 is a conceptual diagram showing an arc to be moved at every sampling time when the velocity of the circle is exponentially accelerated and decelerated in the X-Y plane.

3 is a conceptual diagram showing the length of an arc to be moved every sampling time in the absence of acceleration and deceleration in the X-Y plane.

4 is a schematic diagram of a position control system for realizing a robot circular interpolation method according to the present invention;

5 is a conceptual diagram for calculating an arc every sampling time when there is no acceleration and deceleration in the present invention.

6 is a data flow diagram of the robot circular interpolation method according to the present invention.

* Explanation of symbols for main parts of the drawings

1: Microprocessor 2: Digital / 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 circular interpolation method of a robot, and more particularly, to a method for circular arc transfer from the current position to the target position of the arm tip (End-Effictor) in the articulated robot having two or more arms.

In general, when the robot interpolates along an arc, the velocity distributions Vx and Vy along the circle in the X-Y plane are shown in FIG.

Is displayed. Where A represents the maximum velocity of each axis during circular interpolation.

As can be seen from the graph shown in FIG. 1, since the Y-axis changes speed in the form of Sin function at the starting point of the circular arc, no vibration occurs, but the velocity of the X-axis changes in the form of Cosine function, so the speed is the maximum velocity at zero. The vibration suddenly changes to A, and vibration occurs. Similarly, the vibration of the X and Y axes should be zero (ZERO) in Equation (1) when stopping at any point of the arc (any point between 0 and 360 degrees). Because of the high speed circular interpolation, the amount of vibration is large and the distortion of the circle is also severely generated.

Therefore, the present invention prevents the vibration occurring at the beginning and the end of the circular arc during high speed circular interpolation by performing the speed distribution in the form of exponential function during acceleration of circular arc in order to solve the above-mentioned drawbacks. In addition, it is possible to provide a robotic circular interpolation method that enables high-precision circular interpolation by reducing the sampling time by realizing the speed distribution in the form of exponential acceleration / deceleration.

In order to achieve the above object, the robot circular interpolation method according to the present invention reads the starting point, the intermediate point, and the end point position of the circular arc and the angle of the circular arc during circular interpolation is Δθ and the angle of circular arc during circular interpolation is θ. In this case, a judgment step of judging whether or not Δθ is greater than (θ-ΣΔθ (i)) to recognize acceleration or deceleration, and the first step of obtaining arc coordinates for each sampling time when there is no acceleration / deceleration in the determination step. A coordinate calculation step, a second coordinate calculation step of calculating X and Y coordinates on the arc with Cosθ and Sinθ every sampling time when there is acceleration / deceleration in the determination step, and after obtaining the coordinates on the arc in the second calculation step, The third coordinate calculation step of obtaining the coordinates on the arc when the acceleration / deceleration is performed by using the exponential function acceleration and deceleration is compensated for, and the error occurred when obtaining the coordinates on the arc in the third coordinate calculation step. Is an error compensation step, a target position calculation step of obtaining a target position of each axis for circular interpolation at any point in time at every time, and the coordinates of the circular arc whose error is compensated for, and reading through the target position calculation step. After calculating the current position value of each axis, the position deviation calculation step of calculating the position punctuation of the X and Y axes, and the proportional integral derivative control and the feedforward control are performed on the position deviation calculated in the position deviation calculation step. Characterized in that the control step for controlling the speed of the motor.

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

1 is a diagram illustrating the velocity distribution of the X and Y axes during circular interpolation in the XY plane, and illustrates the relationship between the occurrence of vibrations at the starting point and an arbitrary end point of the circle, and FIG. 2 is an exponential acceleration / deceleration of the velocity distribution during circular interpolation on a rectangular coordinate. In this case, the incremental amount to be moved in the XY plane at every sampling time is shown, and FIG. 3 shows the incremental amount to be moved in the XY plane at every sampling time during circular interpolation without acceleration and deceleration. 4 is a position control system used in the present invention, (1) is a microprocessor for outputting a speed command, and (2) a digital command for converting a speed command of a digital value output from the microprocessor to an analog value. / Analog converter. And (3), the servo control unit for controlling current and speed flowing through the motor by returning the output of the taco generator 5 to the current Ix flowing in the motor 4 and the voltage Vox generated in proportion to the speed of the motor. (6) is an encoder for generating a pulse in accordance with the rotation of the motor, (7) is an up / down counter as a counter for increasing or decreasing the pulse generated in the encoder (6) in accordance with the rotation direction of the motor microprocessor (1). (8) is a timer for interrupting the microprocessor 1 at regular intervals, and each of the components is described only for one axis.

Next, a circular interpolation method of a robot having a position control system as shown in FIG. 4 will be described in detail according to the data flow diagram shown in FIG.

The data flow diagram shown in FIG. 6 describes a method of interpolating a ½ circle. To perform circular interpolation, as shown in step S1, the positions of the starting point (A), the middle point (B) and the end point (C) on the arc are read and the circular arc is read. Read the interpolation direction. Find the point on the arc to be moved when there is no acceleration / deceleration at the current position A (X1, Y1) as follows.

Therefore, the second point on the arc is obtained as follows.

Therefore, if you rewrite equation (3) using equation (2), you will get something like equation (4).

Since CosΔθ SinΔθ is constant, it has a constant value.

In general, the equation (4) is written as follows.

Where K is increased for each sampling time.

Where x (0) = X1, Y (0) = Y1, X (0) is X coordinate X1 of the starting point of the arc and Y (0) is Y coordinate Y1 of the starting point of the arc.

According to the above formula, it is determined in step S2 that (θ-ΣΔθ (i)) Δθ is satisfied when the angle of the circular arc is θ while the circular arc angle is Δθ. In step S2, if (θ-ΣΔθ (i)) Δθ, then in step S3, X (K + 1) and (K + 1) of the above formula are converted into Cosθ and Sinθ, and (θ-ΣΔθ (i)) Δθ, i.e., when there is no acceleration / deceleration, in step S4, the point (intermediate target position) on the arc can be obtained by the above expression (6), and has the form shown in FIG. 3 in the XY plane. Therefore, after calculating the position distribution of the X and Y axes on the arc in the absence of acceleration and deceleration according to the equation (6), in step S5, when the coordinate values on the arc of each axis are calculated by the following equation, the velocity distribution of the circle on the XY plane is zero. As shown in FIG. 2, the exponential acceleration and deceleration forms.

Here, A is an integer. If A is large, the time constant of the exponential function becomes large, and the acceleration / deceleration time becomes long. If A is small, the time constant of the exponential function becomes small and the acceleration / deceleration time becomes short. Therefore, in step S6, in order to be optimal according to the mechanical load requested to the servomotor, an accurate circular interpolation is made only by compensating for an error caused by division in calculating Equations (7) and (8).

That is, when [X (K + 1)-Fox (K)] / A is executed, the quotient is Q1 (K) and the remainder is R2 (K). Calculate Eq. 7 by replacing K + 1) + R1 (K). In the same way, when [Y (K + 1)-Foy (K)] / A is executed for the Y axis, the quotient is Q2 (K) and the remainder is R2 (K). 1) Calculate Equation 8 by replacing Y (K + 1) + R2 (K) instead.

In order to perform circular interpolation at any point in time at each sampling time, the target position of each axis is obtained by the following equation.

Therefore, if the current position value of each axis read by the up / down counter for each axis at any time K is Cx (K), Cy (K), in step S8, the position deviation Pεx (K), Pεy (K) is calculated as below.

Therefore, in step S9, proportional, integral, and differential feedforward control is performed on the positional deviation of each axis, and is calculated as follows.

Where T is the sampling time, Dx (K) and Dy (K) are the values input to the digital / analog converter on the X and Y axes, Kpx and Kpy are the proportional gains of each axis, and Kix and Kiy are the Integral (I) gain. In addition, Kdx and Kdy are derivative gains of each axis, and Kfx and Kfy are Feedforward gains of each axis. The proportional integral (PID) and the feedforward gain of each axis are made to optimize the motion state of the mechanical load attached to the servomotor. Furthermore, in step S10, it is determined whether the final target position is applied. If it is the last position, the circular interpolation is completed.

According to the present invention described above, the fingertips of the robot move along the arc in the XY plane of the rectangular coordinates, and the velocity distribution of the arc becomes an exponential function, so that smooth operation is possible without vibration at the beginning and the end of the circle. Since the exponential function acceleration and deceleration is performed by the state equation, the sampling time can be reduced, and the degree of arc is improved.

Claims (1)

After reading the starting point, the intermediate point, and the end point of the circular arc, when the circular interpolation is executed, the angle of the circular arc is Δθ and the angle of the circular arc is θ during the circular interpolation, where θθ is (θ-ΣΔθ (i)). A judging step of judging whether the acceleration or deceleration is greater by determining whether it is larger, a first coordinate calculation step of obtaining coordinates on the arc every sampling time when there is no acceleration / deceleration in the judgment step, and every sampling time when acceleration / deceleration is present in the determination step. The second coordinate calculation step of calculating the X and Y coordinates of the arc every time by Cosθ and Sinθ, and the coordinates of the arc in each of the second calculation step are calculated by exponential acceleration and deceleration of each axis. An error compensation step for compensating for an error occurring when obtaining coordinates on the arc in the third coordinate calculation step, the third coordinate calculation step, and coordinates on the arc in which the error is compensated for in the error compensation step. The target position calculation step of obtaining the target position of each axis for circular interpolation at any point of time at each sampling time, and the current position value of each axis read through the target position calculation step are obtained, and then the position deviation of the X and Y axes is calculated. A control step of controlling the speed of the servo motor by performing a proportional integral derivative control and a feedforward control on the position deviation calculation step to be obtained and the position deviation calculated in the position deviation calculation step. Circular interpolation method.