KR930007781B1 - Position control method of robot - Google Patents

Abstract

The method controls the position of a robot arm based on different state equation and linear acceleration and deceleration profiles so that chattering of a robot arm is reduced and high speed and accurate position control is obtained. The method includes the steps of: (A) calculating maximum number of position deviation of each axis, (B) calculating the distance of each axis to be moved during sampling intervals when no acceleration and deceleration motion occur; (C) calculating the moved distance of each axis (D) calculating position deviation of each axis; and (E) calculating control input of each axis using proportional, integral and differential control scheme.

Description

How to control the position of the robot

1 and 2 are methods for obtaining the positional increments for every sampling time during linear acceleration and deceleration in the conventional robot position control method.

3 is a velocity distribution diagram for plural axes of the robot starting simultaneously and reaching the target position at the same time.

4 is a waveform diagram showing the number of pulses each axis of the robot should move at every sampling time.

5 is a position control system diagram used in the position control method of the robot of the present invention.

6 is a graph showing a target position when performing linear acceleration and deceleration.

7 is a flowchart showing an operation procedure of the robot's position control method of the present invention.

* Explanation of symbols for main parts of the drawings

1: microprocessor 2: digital-to-analog converter

3: servo control unit 4: motor

5: taco generator 6: encoder

7: up / down counter

The present invention suppresses residual vibration and enables high-speed position control during position control of a horizontal articulated robot or an orthogonal robot, and realizes acceleration and deceleration by discrete time state equation to reduce sampling time and to achieve high precision position control. It relates to a robot position control method to be possible.

Conventionally, there have been various methods for controlling the position of a horizontal articulated robot or an orthogonal robot.

That is, there was a linear acceleration / deceleration scheme as shown in FIG. 1, but in this scheme, in order to calculate a target position at every sampling time in the microprocessor, the distance S may be

S = 1/2 at ² ... … … … … … … … … … … … … … … … … … … … (One)

Where a: acceleration

t: time

, And for constant velocity, the distance (S)

S = v · t... … … … … … … … … … … … … … … … … … … … … … (2)

Where v: speed

t: time

Since the acceleration, constant velocity, and deceleration areas must be determined by the microprocessor, and when the axis is extended like a robot, the sampling time for calculating the equation (1) (2) becomes long, and the accuracy of the position path is reduced. The disadvantage is that the position control system becomes unstable and the movement distance during acceleration, deceleration, and constant velocity is constant when the speed pattern in the form of linear acceleration / deceleration is made from the movement distance. there was.

For example, if the movement distance is 2010 pulses, the movement distance at the time of acceleration and deceleration is 200 pulses, and when the movement distance at the constant speed is 1800 pulses, when acceleration and deceleration is completed, only 2000 pulses can be moved. Only if you move 10 pulses again can you accurately reach the target position.

In addition, as shown in FIG. 2, in order to obtain a target position at an arbitrary sampling time, the area of the linear acceleration / deceleration curve must be calculated. As shown in FIG. 2, only the portion A (square area: position incremental amount) is calculated. Since the B portion (triangle area) is not calculated, there is a problem that the target position is not calculated correctly.

Accordingly, the present invention has been made in view of the above-described problems, and an object of the present invention is to calculate the linear function itself for each position during acceleration and deceleration during linear acceleration / deceleration at every sampling time. The present invention provides a robot position control method capable of reducing the sampling time, improving the accuracy of the position path, and enabling high-speed position control by realizing the linear function acceleration / deceleration by the discrete time state equation.

Another object of the present invention is to provide a robot position control method in which each axis starts at the same time and at the same time can reach the target position, thereby suppressing the vibration caused by the motion interference of the axis.

In order to achieve the above object, the method for controlling the position of a robot according to the present invention includes a step of calculating the current position of the robot, the position deviation of each axis, the maximum position deviation, and the acceleration of the servo motor based on the driving current of the robot moving servomotor. In this case, the number of pulses to be moved at every sampling time is calculated, the step of obtaining the number of pulses, the step of obtaining the moving pulses of each axis at each sampling time in the absence of acceleration and deceleration, and the position increment amount of each axis. A step for obtaining the target position of each axis at each sampling time at a predetermined point, and calculating the positional deviation of each axis and controlling the proportional integral derivative, and driving the servo motor based on the proportional integral differential control value. Each horse race according to whether it is equal to the number of moving pulses for every sampling time when there is no acceleration / deceleration in the elapsed state A robot control method comprising: obtaining a plurality of moving pulses; and clearing a moving pulse of each axis or determining a final target position according to whether the K value of the predetermined point is greater than the number of times. Position increment

S (k) = fi (k) -fi (k-n) + S (k-1)

fo (k) = S (k) / n

Where fi is the acceleration pulse of each axis for every sampling time in the absence of acceleration and deceleration, and n is the acceleration and deceleration time constant.

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

4 is a waveform diagram showing the number of pulses that each axis of the robot should move at every sampling time, and FIG. 5 is a diagram of a position control system used in the position control method of the present invention. A block diagram showing the position control system for only one axis among a plurality of axes configured in the robot.

In FIG. 4, the diagonal line shows the number of pulses that each axis (x, y, z, w) of the robot should move at every sampling time Ts, and Vxmax, Vymax, Vzmax and Vwmax are the motors of each axis. Is the highest speed.

In Fig. 5, reference numeral 1 denotes a microprocessor that outputs the speed command of the servomotor 4 and controls the entire system, and reference numeral 2 denotes the speed command of the digital value output from the microprocessor 1. A digital / analog converter that converts an analog value into a digital / analog converter, which inputs the converted analog value to a servo driver (not shown) and drives a command signal and a drive speed to drive the servo motor 4 in the forward and reverse directions. Outputs

Further, (3) indicates that the current feedback signal Ix of the servo motor 4 is fed back and the speed feedback signal Vox detected by the taco generator 5 according to the drive speed of the servo motor 4 is fed back. And a servo control unit for controlling the servo motor 4 based on the feedback signal, and (7) up counting or down counting a pulse signal corresponding to the rotation amount of the servo motor 4 input from the encoder 6. An up / down counter that counts and inputs the current position of the robot to the microprocessor 1, and (8) is a timer for interrupting the microprocessor 1 at regular intervals to determine the driving speed of the robot. .

In the robot position control method according to the present invention configured as described above, when the timer 8 interrupts the microprocessor 1 at regular intervals, the microprocessor 1 is interrupted by the timer 8. Each time the position increment amount of each axis according to the linear acceleration and deceleration is calculated, the target position is calculated, and the current position of the robot is determined based on the signal input from the up / down counter 7 which counted the pulse signal from the encoder 6. After calculating the difference between the final target position and the current position, the control of proportional integral derivative (PID) is performed and input to the digital-to-analog converter 2.

The digital value for the robot speed command input to the digital / analog converter 2 is converted into an analog value and input to the servo controller 3. Accordingly, the servo controller 3 forwards or reverses the servo motor 4. The taco generator 5 is outputted along with the current feedback signal Ix of the servo motor 4 while outputting a command signal to drive the drive signal and a drive speed signal to drive the motor 4 according to each signal. The speed feedback signal Vox from is received by the feedback input to automatically control the servomotor 4 in the same manner as described above.

Next, FIG. 7 is explained.

7 is a flowchart showing the operation procedure of the robot position control method of the present invention, in which S denotes a step (step).

Since the present invention is to control the position of the robot, when the device for controlling the position of the robot of the present invention is operated, first, in step S1, the position deviation of each axis is obtained.

That is, since the position deviation is the target position of the robot minus the current position value of the robot, the position deviation of the 4 axes (x, y, z, w) is Px for the x axis, Py, x for the y axis. By specifying Pz for the axis and Pw for the w-axis, the positional deviations Px, Py, Pz, and Pw for each axis are obtained.

Subsequently, in step S2, the maximum positional deviation Pmax is obtained, and the maximum positional deviation Pmax is obtained by Pmax = MAX_Px, Py, Pz, Pw '.

Accordingly, in the absence of acceleration, it is possible to calculate the number of pulses (fi) to be moved at every sampling time. For example, the maximum speed of the motor is 3,000 rpm, and the number of pulses of the encoder 6 is 1,000 pulses / If the interrupt cycle by the circuit and the timer 8, i.e., the sampling time is assumed to be 1 msec, the number of pulses to be moved every sampling time in the absence of the acceleration is

Where P is the number of pulses of encoder 6 and Ts is the sampling time.

fi = 3000rpm / 60sec × 1000pulse × 1msec

= 3000rpm / 60sec × 1000pulse × 1 × 10 ^-3 sec

= 50 pulses.

Since the number of pulses and the sampling time of the encoder 6 are fixed values, in order to change the value of the number of pulses to be moved every sampling time Ts without the acceleration, the maximum speed of the motor 4 is changed. Can only be changed, and of course can also be changed by the sampling time and the number of pulses P of the encoder 6.

Since the fi value is obtained, in step S3, the number N of pulses fi to be moved at every sampling time in the absence of acceleration is obtained, and this number N is the maximum position deviation value ( Pmax) is obtained by dividing by the number of pulses (fi) to be moved at every sampling time in the absence of the acceleration.

That is, the number of times Obtained by

Subsequently, in step S4, the movement pulses (fix, fiy, fiz, fiw) of each axis for each sampling time in the absence of acceleration and deceleration are obtained, where the maximum position deviation Pmax obtained in step S2 and the position deviation Px of the x axis are obtained. Assuming that P is equal to (Pmax = Px), if there is no acceleration or deceleration for each axis, the moving pulse for every sampling time is fixed to fix = fi for the x-axis, fiy = Py / N for the y-axis, and z-axis. Fiz = Pz / N for the w-axis and fiw = Pw / N for the w-axis.

Specifically, for example, Px = 4321 pulses

Py = 3214 pulses

Pz = 2413 pulse

When Pw = 1234 pulse

The number of times N is N = 4321/50 = 86 + 21 = Qx + Rx, and the number N of moving compasses for every sampling time in the absence of acceleration becomes N = 86.

therefore,

Becomes

That is, in the case where there is no acceleration up to the 86th time, the number of pulses (fix, fiy, fiz, fiw) that the x, y, z, and w axes should move at every sampling time is fixed = 50, fiy = 37, fiz = 28, fiw = 14, and the 87th fix = 21, fiy = 32, fiz = 5, fiw = 30, and the number of pulses to move the x, y, z, and w axes from the 88th (fix , fiy, fiz, fix) are zero.

Therefore, in step S6, the positional increment of each axis | shaft is calculated | required using the following formula.

That is, the position increment of the x axis Sx (k) = fix (k) -fix (k-n) + Sx (k-1)

fox (k) = Sx (k) / n

Position increment of y-axis Sy (k) = fiy (k) -fiy (k-n) + Sy (k-1)

foy (k) = Sy (k) / n

Position increment of z-axis Sz (k) = fiz (k) -fiz (k-n) + Sz (k-1)

foz (k) = Sz (k) / n

Position increment of w-axis Sw (k) = fiw (k) -fiw (k-n) + Sw (k-1)

fow (k) = Sw (k) / n

The position increment of each axis is calculated by (fo (k): position increment for every sampling time during linear acceleration and deceleration).

Where k = 0, 1, 2, 3... Therefore, if 0 is substituted into the K value of the formula for calculating the position increment of each axis in step S5, Sx (0) = Sy (0) = Sz (0) = Sw (0) = 0, and fix (k), fiy (k), fiz (k) and fiw (k) are all zero when the k value is K ≦ 0.

In addition, the constant n is a constant representing the acceleration / deceleration time constant. For example, when the constant n = 40 and the sampling period Ts is 1 msec, the acceleration and deceleration time constant is 40 msec (n × sampling period Ts). = 40 x 1 msec), and if the n value is selected according to the characteristics of the joint attached to the motor, the acceleration / deceleration time constant can be converted.

When the positional increment of each axis is obtained, a speed curve in the form of linear acceleration and deceleration is obtained.

Further, in order to calculate the exact position path of the robot, in S7, a value below the decimal point obtained by the position increment of each axis is corrected, and the correction below the decimal point is processed as follows.

That is, Sx (k), Sy (k), Sz (k), and Sw (k) are represented by S (k), and the remaining cumulative values of S (k) / n at arbitrary K time points are represented. If you say If,

fo (k) ← fo (k) +1

, Otherwise

fo (k) ← fo (k)

to be.

Where fo (k) represents fox (k), foy (k), foz (k), and fow (k) on each axis.

After correcting the values below the decimal point for each axis in the same manner as described above, each sampling time is determined in step S8 using the position increment of each axis (x, y, z, w) for each sampling time obtained in step S6. The target position at any K time point for each axis is obtained.

The target position (Fig. 6) at the K point with respect to each axis is

Obtained by

Since the target position at the time of K with respect to each axis in every sampling time is calculated | required in said step S8, in step S9, the positional deviation in the K time of a robot is calculated | required by the following formula. Further

Position deviation with respect to the x axis = Px (k) -Cx (k)

Position deviation with respect to y axis = Py (k) -Cy (k)

Position deviation with respect to z-axis = Pz (k) -Cz (k)

Position deviation with respect to w axis = Pw (k) -Cw (k)

(Cx (k), Cy (k), Cz (k) and Cw (k) are the current position values of the robot read by the up / down counter 7 for each axis at any K point.)

When the positional deviation of each axis at time K is found in step S9, in step S10, a proportional integral differential (PID) control is performed on the positional deviation of each axis at time K obtained in step S9. Calculate

Here, Dxout, Dyout, Dzout, and Dwout are values inputted to the digital-to-analog converters 2 on each axis, and Kpx, Kpy, Kpz, and Kpw are proportional (P) individuals of each axis, and KIx, KIy, and KIw are integrals of each axis. (I) Gain. Kdx, Kdy, Kdz, and Kdw are derivative (D) gains of each axis.

The proportional integral derivatives of Dxout (k), Dyout (k), Dzout (k), and Dwout (k) are input to the digital / analog converter 2, and the digital / analog converter 2 converts this value to analog. After converting to a value, the value is inputted to the sub-control unit 3 to drive the motor 4.

Subsequently, in step S11, the position point for judging the positional deviation is set to K + 1, which is the point in which the predetermined time has elapsed, and substituted into K. It is determined whether or not the number N of pulses fi is the same. As a result of this determination (Yes), in step S13, the values of Rx, Ry, Rz, and Rw of each axis obtained by obtaining the number N of the moving pulses fi for every sampling time when there is no acceleration are determined for each axis. It is determined by substituting the moving pulses (fix, fiy, fiz, fiw) of?, And in step S14, it is determined whether the value of K is greater than the number N of the moving pulses fi for every sampling time. If the determination result is large (Yes), the process proceeds to step S15, where 0 is substituted by the movement pulses (fix, fiy, fiz, fiw) for each axis. In step S16, it is determined whether the position is the final target position.

If it is determined that this is the final target position (Yes), the driving of the robot is terminated. If it is not the final target position (No), the operation returns to step S5 and subsequent operations are repeated.

On the other hand, when the value of K in step S12 is not equal to the number N of the moving pulses fi for every sampling time (K) N, No), go directly to step S14, and if the value of K in step S14 is not greater than the number N of moving pulses fi for every sampling time (No), go directly to step S16. It is determined whether the robot is in the final target position, and subsequent operation is executed according to the determination result.

Thus, according to the position control method of the robot of the present invention, each axis is driven by linear acceleration and deceleration, and the target position for every sampling time is calculated by discrete time counterpart formula, so that the accuracy of the position path is good and the vibration is high speed. Not only can control be performed, but each axis starts and ends at the same time, so there is an excellent effect of reducing the vibration caused by the motion interference of the axis.

Claims (1)

Steps for calculating the current position of the robot, the position deviation and maximum position deviation of the robot based on the driving position of the robot moving servomotor, and the number of pulses to be moved at every sampling time in the absence of acceleration and deceleration of the servo motor are calculated. A step for obtaining the number of times, a step for obtaining a moving pulse of each axis for every sampling time in the absence of acceleration and deceleration, a step for calculating the position increment amount for each axis, and a target position for each axis for each sampling time at a fixed point To obtain the positional deviation of each axis and control the proportional integral derivative control, and the movement pulse of each sampling time when there is no acceleration and deceleration in the state where a certain time has elapsed while driving the servo motor based on the proportional integral differential control value. Obtaining a moving pulse for each axis depending on whether the number is equal to the number of times; In the robot control method comprising the steps of clearing the movement pulse of each axis or determining the final target position according to whether or not, the position increment amount of each axis is such that a speed curve in the form of a linear function acceleration / deceleration is obtained. S (k) = fi (k) -fi (k-n) + S (k-1) fo (k) = S (k) / n (Where S (k): parameter, fo (k): position increment for every sampling time when linear acceleration and deceleration, fi: movement pulse of each axis for every sampling time without acceleration and deceleration, n: acceleration and deceleration time constant Robot position control method characterized in that obtained by.