KR940002207B1 - Linear compensating method of robot - Google Patents

Abstract

The linear interpolating method comprises the steps of: obtaining the current position and target position, comparing the movement distance of X axis and the movement distance of Y axis on the X-Y coordinates, and obtaining the movement number; obtaining the movement distance for every sampling time; and calculating the position increment amount for every sampling time and calculating the movement pulse of respective joints; and calculating the position deviation, controlling proportional integration differentiation to the position deviation, and outputting it as a driving control signal of a submotor, thereby reducing sampling time.

Description

Linear interpolation method of robot

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

2 is a graph of the amount of movement of the X and Y axes in the rectangular coordinates X-Y plane.

3 is a velocity distribution plot for the X and Y plots for linear travel on a Cartesian coordinate X-Y plane.

4 is a graph showing the distance to be moved on the X-Y coordinate in the absence of acceleration and deceleration.

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

Figure 6 is a graph showing the positional change according to the acceleration and deceleration of the S-shaped characteristic curve according to the present invention.

7 is an acceleration and deceleration graph of the S-shaped characteristic curve according to the present invention.

8 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 transferring an end-effector of an articulated robot arm in the XY plane of a Cartesian coordinate system. In particular, the velocity distribution during linear transfer is a secondary function having an S-shaped characteristic curve. This improves the start accuracy and end accuracy of the robot's driving start and end points, and accelerates and decelerates the quadratic function characteristic curve by using discrete-time equations, thereby reducing the sampling time and improving the accuracy of the straight line. In addition, the present invention relates to a linear interpolation method of a robot capable of moving at high speed as well.

In general, the robot is driven along the path of the divided straight line by performing point-to-point (PTP) operation by the micro-distance of the straight line in the state where the straight line is decomposed to a certain distance during the linear interpolation of the robot. There was a method, but such a method had a problem in that the accuracy of linear movement was lowered when the robot started and stopped.

Accordingly, the present invention has been made in view of the above problems, and an object of the present invention is to start and operate a robot by accelerating and decelerating the speed distribution to a quadratic function having an S-shaped curve near the start and end points of the robot. And to provide a linear interpolation method of the robot to improve the accuracy of the linear movement at the end of the drive.

Another object of the present invention is to improve the accuracy of the straight line and improve the accuracy of the straight line by simultaneously implementing acceleration and deceleration by the discrete-time equation. To provide an interpolation method.

In order to achieve the above object, the linear interpolation method of the robot according to the present invention includes a current position and a target position in the XY plane (kinematic system) by kinematic equations from the current position and the final target position in the joint coordinate system of the robot. After calculating, compare the magnitude of the moving distance of the X axis and the moving distance of the Y axis in the XY plane, and the number of movements from the amount of movement every sampling time when there is no acceleration and deceleration corresponding to the linear feed speed with respect to the axis with the large movement distance. S-shape from the movement count calculation step to find the, the movement distance calculation step to find the movement distance for every sampling time when there is no acceleration / deceleration from the movement count for the axis with short movement distance, and the movement amount of each axis without acceleration / deceleration. Positioning at every sampling time by discrete time equations to perform quadratic acceleration and deceleration with characteristic curves After calculating the quantity, the movement pulse calculation step of calculating the movement pulse of each joint by the reverse kinematics equation of the robot arm, the current position of each axis is detected, the position deviation is calculated, and the proportional integral to the position deviation After the derivative (PID) control, characterized in that the drive step of the servo motor to output the drive control signal of the servo motor.

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 movement of the robot arm end effector in the XY plane, and FIG. 2 is a graph of the movement amount of the X and Y axes in the rectangular coordinates XY plane, and FIG. 3 is a graph for the linear movement in the rectangular coordinates XY plane. FIG. 4 is a velocity distribution diagram for X and Y coordinates, and FIG. 4 is a graph showing distances to be moved on XY coordinates when there is no acceleration and deceleration, and FIG. 5 is a block diagram for realizing the linear interpolation method of the robot of the present invention.

6 is a graph showing the positional change according to the acceleration and deceleration of the S-shaped characteristic curve according to the present invention, and FIG. 7 is a graph of the acceleration and deceleration of the S-shaped characteristic curve according to the present invention, and FIG. It is a flowchart showing the following.

1 shows a robot with two arms (a ₁ , a ₂ ) drawing a straight line from the current position (c ₁ (xi, yi)) to the target position (C ₂ (xf, yf)) in the XY plane of the rectangular coordinates. In this case, the servo motor 4 for driving the respective arms is attached to the points A and B, respectively.

로 항상 일정해야 한다.2 and 3, in order to draw a straight line in the XY plane, the ratio of the feed rate of the X axis and the feed rate of the Y axis is

It should always be constant.

That is, in order to draw a straight line in the X-Y plane, the ratio of the distance to be transported every sampling time on each coordinate must be constant. Therefore, the shorter the sampling time, the higher the linear accuracy.

In FIG. 5, (1) is a microprocessor, (2) is a digital / analog converter (Digita1 / Analog converter) for converting a speed command of a digital value output from the microprocessor (1) into an analog value. The analog value converted by the digital / analog converter 2 is input to the servo controller 3 to output a command signal and a drive speed signal for driving the servo motor 4 in the forward and reverse directions.

On the other hand, the servo controller 3 is fed back the current feedback signal (Ix) of the servo motor 4, the speed feedback signal detected by the ride generator 5 in accordance with the drive speed of the servo motor (4) Vax) is fed back to servo control the servomotor 4.

In the drawing, reference numeral 7 denotes the microprocessor 1 by counting up or down counting the pulse signal output from the encoder 6 which outputs the pulse signal according to the rotation amount of the servomotor 4. (8) is a timer for interrupting the microprocessor 1 at regular intervals. In the above description, each constituent device is described only for one axis.

Next, a linear interpolation method of the robot according to the present invention will be described with reference to FIGS. In FIG. 8, S means a step, and only one joint drive is demonstrated below with an example.

When the linear interpolation method of the robot according to the present invention is started, first, the present value (θ ₁ , θ ₂ ) in one joint coordinate of the horizontal articulated robot, as shown in FIG. It reads by (7), and the position (xi, yi) in an XY plane is calculated | required by following formula (1) and (2) in step S2.

xi = a ₁ cosθ ₁ + a ₂ cos (θ ₁ + θ ₂ ). … … … … … … … … … … … … … … … … … … … (One)

yi = a ₁ sin θ ₁ + a ₂ sin (θ ₁ + θ ₂ ). … … … … … … … … … … … … … … … … … … … … (2)

Where a ₁ is the length of the robot's first arm

a ₂ : length of robot's second arm.

When the positions xi and yi in the XY plane are obtained, the process proceeds to step S3, where the final target values θ ₃ and θ ₄ are read from the joint coordinates of the robot, and in step S4, the final target values θ read in the step S3. ₃ , θ ₄ ), the target position values (xf, yf) in the XY plane are obtained by the following equations (3) and (4).

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

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

Is displayed

Accordingly, as shown in FIG. 2, the positional deviation of each axis in the XY plane is obtained in step S5, and the amount of movement xp in the x-axis direction is obtained by xp = xf-xi- (5), and the amount of movement in the y-axis direction. (yp) is obtained by yp = yf-yi- (6).

When the linear transfer speed is V [mm / sec], when the magnitude of the movement amount Xp in the X-direction and the movement amount Yp in the Y-axis direction is compared, it is assumed that the X axis is large. If the speed is set to V [mm / sec] and the distance to be moved every sampling time Ts in the absence of acceleration / deceleration as shown in FIG. 4 is Pix [mm], The distance Pix to move for each sampling time is Pix = V x Ts [mm].

For example, when the feed rate V of the straight line is 1000 [mm / sec] and the sampling time Ts is 10 msec, the distance Pix to be moved every sampling time without the acceleration / deceleration is Pix = V × Ts = 100 [㎜ / sec] is a ^{× 10 × 10- 3 [sec]} = 10㎜.

Since the positional deviations Xp and Yp of the respective axes in the XY plane are obtained in step S5, the process proceeds to step S6 where the maximum positional deviation Pmax is determined by Pmax = MAX {Xp, Yp} and the maximum positional deviation is determined in step S7. The number N of Pis is obtained from the distance value Pi to be moved at every sampling time.

에 의해서 구해지는 것이다. (여기서 나머지는 Rix라고 한다)That is, when the maximum positional deviation is equal to the positional deviation of the X axis (Pmax = Px), that is, the number N of distances Pix to be moved every mesampling time Ts without acceleration / deceleration is N =

Obtained by (The rest here is called Rix)

Therefore, the distances Pix and Piy to be moved at every sampling time Ts when there is no acceleration in step S8 are obtained by the following equation.

이고, X축 방향으로 가감속이 없는 경우의 매 샘플링 시간마다 이동해야할 거리(Piy)는, Piy=

이다. (여기서 나머지는 Riy로 한다.) 예를들어서That is, as shown in FIG. 4, the distance Pix to be moved every sampling time when there is no acceleration / deceleration in the X-axis direction is Pix =

And the distance Piy to be moved at every sampling time in the absence of acceleration and deceleration in the X-axis direction is Piy =

to be. (The rest here is called Riy.) For example

V = 1000 [mm / sec], Ts = 10 [msec]

Px = (Xi-Xf) = 400 [mm]

In the case of Py = (Yi-Yf) = 200 [mm],

=400/10=40회이며, X축 방향으로 가감속이 없는 경우의 매샘플링 시간마다 이동해야할 거리(Piy)는 Piy=Py/N=200/40=5[㎜]가 된다.Since the maximum positional deviation (Pmax) is the same as the positional deviation (Px) of the X-axis, Pmax = Px = 400 [mm], and the distance (Pix) to be moved at every sampling time when there is no acceleration / deceleration in the X-axis direction is Pix = V Ts = 1000 [mm / sec] x 10 [msec] = 10 [mm], and the number N is N =

= 400/10 = 40 times, and the distance Piy to be moved for every sampling time in the absence of acceleration and deceleration in the X-axis direction is Piy = Py / N = 200/40 = 5 [mm].

That is, in the absence of acceleration and deceleration, the distance to move to the X and Y axes on the XY plane until the 40th sampling is 10 [mm] and 5 [mm], respectively, and the number of pulses to be moved from the 201st sampling is ○ [mm], respectively. .

Subsequently, 0 is substituted into K in step S9, and the positional increment of each axis in the X-Y plane is obtained by the following equation in step S10.

Where Sx (0) = Sy (0) = S'y (0) = S'x (0) = 0 and K = 0, 1, 2, 3,... to be.

As can be seen from the above equations (7) and (8), since there is a division term, it is necessary to process the decimal point or less, but the exact position path can be calculated in the X-Y plane. That is, at step S11, the remaining values of Sx (k) / n, Sy (k) / n, S'x (k) / n, and S'y (k) / n are represented by R (k) at an arbitrary K point. If Pix (k), Piy (k), Vox (k), and Voy (k) are represented by Pi,

Pi (k + 1) ← Pi (k + 1) + R (k)

The position increase amount is calculated by using equations (7) and (8).

According to the above formulas (7) and (8), the S-shaped characteristic curve of the present invention shown in Figs. 6 and 7 is obtained.

On the other hand, when the acceleration / deceleration time constant n is 20, the acceleration time tacc and the deceleration time tdec are tacc = tdec = n × Ts = 20 × 2 msec = 40 [msec], and the acceleration / deceleration time (tacc · tdec) is appropriately selected depending on the characteristics of the machine attached to the motor.

After the decimal point value correction in step S11 is performed, the flow advances to step S12 to obtain the target position of each axis in the X-Y plane for each sampling time using the following equation.

Since the target position of each axis is obtained in step S12, in step S13

Find the target position of each axis at point K in the joint coordinate system using. Accordingly, in step S14, the positional deviation at any K time point is obtained from the following equation.

That is, P? X (k) = P? X (k)-C? X (k)... … … … … … … … … … … … … … … … … … (13)

P? Y (k) = P? Y (k)-C? Y (k)... … … … … … … … … … … … … … … … … … (14)

(Cθx (lk) and Cθy (k) are the current position values of the robot read by the up / down counter 7 for each axis at an arbitrary K point)

Since the positional deviation of each axis is determined in step S14, the process proceeds to step S15 and after performing proportional integral derivative (PID) control on the positional deviation of each axis at the time K at step S14 by the following equation, digital / analog Input to transducer 2. In other words,

Where T is the sampling time, Dx out and Dy out are the values input to the digital-to-analog converter (2) on each axis, Kpx and Kpy are the proportional gains (P) of each axis, and Kix and Kiy are the integral gains of each axis ( I)

In addition, Kdx and Kdy already determine the derivative gain D of each axis, and the proportional integral gain of each axis as in step S15 determines the optimum state of motion of the robot.

Therefore, the values 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. The motor 4 is driven.

Subsequently, at step S16, the value of K + 1 is substituted for K, and the flow proceeds to step S17 to determine whether the value of K is equal to the number N of the moving pulses Pi for every sampling time when there is no acceleration or deceleration. If it is the same (Yes), the number N of moving pulses Pi for every sampling time when there is no acceleration is determined in step S18, and the remaining values Rix and Riy of each axis are obtained for each axis. It is determined by substituting the moving pulses Pix and Piy, and if it is determined in step S19 whether the value of K is greater than the number N of the moving pulses Pi for every sampling time, and if it is large (Yes), step S20 Subsequently, it calculates by substituting 0 to the movement pulses Pix and Piy for each axis, and determines whether it is a final target position in step S21.

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

On the other hand, if the value of K in step S17 is not equal to the number n of moving pulses Pi for every sampling time (when K ≠ N, No), the flow proceeds directly to step S19 to perform subsequent operations. If the value of K in step S19 is not greater than the number N of the moving pulses Pi for each sampling time (No), the process proceeds to step S21 to determine whether the robot is the final target position.

Although the above-described embodiment is directed to a horizontal articulated robot, the present invention is also applicable to an orthogonal robot and a vertical articulated robot.

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 coordinates, and the velocity distribution of the straight line is defined by the discrete time state equation. Exciting second function acceleration and deceleration not only shortens the sampling time and improves linear accuracy, but also has an excellent effect of linear movement at high speed.

Claims (2)

After calculating the current position and target position in the XY plane (World coordinate system) by the kinematic equation from the current position and final target position in the joint coordinate system of the robot, the movement distance of the X axis and the movement distance of the Y axis in the XY plane The number of movements to calculate the number of movements from the amount of movement for every sampling time when there is no acceleration and deceleration corresponding to the feed speed of the straight line with respect to the axis with a large distance, and the number of movements for the axis with a short distance. The travel distance calculation step to find the travel distance for every sampling time in the absence of acceleration and deceleration, After calculating the positional increments for each sampling time, the joint kinematics equations of the robot arms The movement pulse calculation step of calculating the pulse, the current position of each axis is detected, the position deviation is calculated, the proportional integral derivative (PID) control is performed on the position deviation, and the servo motor outputs the drive control signal of the servo motor. A linear interpolation method for a robot, comprising a driving step. The linear interpolation method of claim 1, wherein the proportional integral value is converted into an analog value by a digital / analog converter and then input to a servo control unit to drive a motor.

Images