KR930007780B1 - Position control method of robot - Google Patents

Abstract

The method controls position of a robot arm based on differential state equation and S-shape acceleration and deceleration profile so that chattering of a robot arm is reduced and high speed and precise position control is obtained. The method includes the steps of: (A) calculating the maximum number of position deviation of each axis; (B) calculating the distance of each axis to be moved during sampling intervals when no accerleration and deceleration motion occurs; (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 (PID).

Description

How to control the position of the robot

1 is a graph showing a linear acceleration / deceleration method in a conventional robot position control method.

2 is a graph showing an exponential function acceleration / deceleration method in a 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 waveform diagram of a straight line and an exponential acceleration and deceleration used in the present invention.

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

8 is a graph showing target positions during acceleration and deceleration of straight lines and exponential functions.

* 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 equations to reduce sampling time and 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 method as shown in FIG. 1, but this method has A, B, C, and D points (robot driving dog) centered on the 0 axis in the relationship between speed (v) and time (t). Since the speed is discontinuous at the starting point, the end point, and the turning point), it causes vibrations in the robot without rigidity, which leads to a long time for determining the position of the robot.

In addition, in the exponential function acceleration / deceleration method as shown in FIG. 2, the speed V (t) at the time of acceleration E is V (t) = Vmax {1-exp (t / τ} (τ; exponent). Time constant of the function), and the motor speed V (Ts) for each sampling time Ts becomes V (Ts) = Vmax {1-exp (Ts / τ}, and the position P (Ts) )

P (Ts) = V (Ts) · Ts = Vmax {1-exp (Ts / τ) · Ts …………………… (1)

The motor speed V (t) at the time of deceleration F becomes V (t) = Vmax · exp (-t / τ), and the motor speed V (Ts) for each sampling time. Is

V (Ts) = Vmax · exp (-Ts = Vmax · exp (-Ts / τ), and the position (P (Ts)) at this time is

P (Ts) = V (Ts) · Ts = Vmax · exp (−Ts /?) Ts... … … … … … … … (2)

Since the acceleration or deceleration is determined at every sampling time by the microprocessor and the calculation is performed for each axis by using the equation (1) or (2), the sampling time becomes long and the position path accuracy is increased. There was a problem that the position control system became unstable.

In addition, in the case of having a plurality of (four axes) axes, such as a horizontal articulated robot, the movement amount of each axis as shown in FIG. The velocity distribution of each axis should be determined using the (Px (K), Py (K)) axes, which involved considerable difficulty.

Accordingly, the present invention has been made in view of the above-described various problems, and an object of the present invention is to provide a straight line and an exponential function for a position during acceleration and a deceleration during acceleration and deceleration of a straight line and an exponential function. Provides a robot position control method that can reduce the sampling time, improve the accuracy of the position path, and at the same time high-speed position control by realizing linear and exponential function acceleration / deceleration by discrete time state equation without calculating each sampling time. have.

It is another object of the present invention to provide a robot position control method in which each axis starts at the same time and can reach the target position at the same time, 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. The number of pulses to be moved at every sampling time in the case of calculating the number of pulses, the step of calculating the pulses of movement of each axis at every sampling time in the absence of acceleration and deceleration, and the position increment of each axis. A step of obtaining the step, the target position of each axis at every sampling time at a fixed point, and calculating the positional deviation of each axis and controlling the proportional integral derivative, and driving the servo motor based on the proportional differential control value Each axis depends on whether it is equal to the number of moving pulses for every sampling time when there is no acceleration or deceleration in the elapsed state. A robot control method comprising the steps of: obtaining a plurality of moving pulses and a step of clearing the moving pulses of each axis or determining the final target position according to whether the value of the fixed point is greater than the number of times. The quantity is calculated so that the speed curve in the form of straight line and exponential function

fo (k) = fo (k-1) + [fi (k) -fo (k-1) / A

S (k) = fo (k) -fo (K-n) + s (k-1)

f'o (K) = s (k) / n

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

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 position control system diagram 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 (s, 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 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.

In addition, (3) when the current limit signal Ix of the servo motor 4 is fed back and the speed feedback speed 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 robot driving speed.

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

The digital value for the robot speed command input to the digital-to-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. Outputs a command signal and a drive speed signal to drive the motor to drive the motor 4 according to each signal, and at the same time the tacho generator 5 together with the current limiting speed Ix of the servo motor 4. The servo motor 4 is automatically controlled in the same manner as described above by receiving the feedback signal of the speed feedback signal Vox from

Next, Fig. 7 will be described.

FIG. 7 is a flowchart showing the operation procedure of the robot's position control method of the present invention. In FIG. 7, S means a 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 driven by the robot, the position deviation of 4 axes (x, y, z, w) is Px for the x axis, Py, z for the y axis. PZ for the axis and Pw for the W axis are defined, and 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 pulse number of the encoder 6 is 1,000 pulses / revolution. It is assumed that the interrupt period by the timer 8, that is, the sampling time is 1 msec, and in the absence of the acceleration, the number of pulses to be moved every sampling time is

(Where p is the number of pulses of encoder 6, T is time)

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

-3

= 3000rpm / 60sec × 1000pulse × 1 × 10sec

= 50 pulses.

Since the number of pulses and the sampling time of the encoder 6 are fixed values, the maximum speed of the motor 4 must be changed in order to change the number of pulses (fi) to be moved at every sampling time in the absence of the acceleration. This can be done, of course, by the sampling time and the pulse number 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 moving pulses (fix, fiy fiz, fiw) of each axis for each sampling time in the case of no acceleration / 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 movement 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, in the case of Px = 4321 pulse Py = 3214 pulse Pz = 2413 pulse Pw = 1234 pulse, when there is no acceleration, the number N of moving pulses fi for every sampling time is N = 4321/50 = 86 + 21 = Qx + Rx, where 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, the 87th fix = 21, fiy = 32, fiz = 5, fiw = 30, and the number of pulses to which the x, y, z and w axes from the 88th should move , fiy, fiz, fiw) are zero.

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

Position increment of the x-axis fox (k) = fox (k-1) + [fix (k) -fox (k-1)] / A

Sx (k) = fox (k) -fox (k-n) + Sx (k-1)

f'ox (k) = Sx (k) / n

Position increment of y-axis fox (k) = foy (k-1) + [fiy (k) -foy (k-1)] / A

Sy (k) = foy (k) -foy (k-n) + Sy (k-1)

foy (k) = Sy (k) / n

Position increment of the z axis foz (k) = foz (k-1) + [fiz (k) -foz (k-1)] / A

Sz (k) = foz (k) -foz (k-n) + Sz (k-1)

f'oz (k) = Sz (k) / n

Position increment of the w-axis fow (k) = fow (k-1) + [fiw (k) -fow (k-1)] / A

Sw (k) = fow (k) -fow (k-n) + Sw (k-1)

f'ow (k) = Sw (k) / n

(S (k): Parameter for calculating the equation .fo (k): Position weight per sampling time during exponential acceleration and deceleration .fo '(k): Every sampling time during exponential function and linear acceleration and deceleration The position increment of each axis is calculated by the position increment of.

Where k = 0, 1, 2, 3,... Therefore, in step S5, if 0 is substituted into the K value of the formula for calculating the position weight of each axis, Sx (0) = Sy (0) = Sz (0) = Sw (0) = 0, and fox (k), foy (k), foz (k), and fow (k) are all zero when k is 0.

A is a time constant at the time of exponential function acceleration / deceleration, and n is a time constant at the time of linear acceleration / deceleration in determining the amount of position in each axis. Therefore, the time constant at the time of linear and exponential function acceleration / deceleration is determined by appropriately selecting the A and n values according to the characteristics of the joint attached to the motor. That is, the acceleration / deceleration curve can be changed by the values A and n.

When the positional increments of the respective axes are obtained, a speed curve in the form of a straight line and an exponential function acceleration / deceleration as shown in Fig. 6 is obtained.

Further, in order to be able to calculate the accurate position path of the robot, in step 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, fox (k-1), foy (k-1), foz (k-2) and fow (k-1) are represented by fo (k-1), and fix (k) and fiz (k) If fiy (k) fiw (k) is represented by fi (k), at any k time point,

The remaining cumulative value of [fi (k) -fo (k-1)] / A ( When R (i) is greater than zero,

[｛Fi (k) -fo (k-1)｝ / A> 0 and R [｛fi (i) -fo (i-1)｝ / A] ＞ A

If, then fo (k) ← fo (k) +1

R ← RA otherwise

fo (k) ← fo (k)

← R is

Conversely, at any K time, ｛fi (k (-fo (k-1)｝ / A cumulative value of the rest of A) R (i) is less than zero

That is, [｛fi (k) -fo (k-1)｝ / A] <0 and R [｛fi (i) -fo (i-1)｝] / A｝] <A

If fo (k) ← fo (k) -1

R ← , Otherwise

fo (k) ← fo (k)

R ← R is.

In addition, Sx (k) and Sy (k) are represented by S (K), and the remaining cumulative values of S (k) / n are represented. If R (i),

If R (i)> n,

f'o (k) ← fo '(k) +1

R (i) ← R (i) -n, otherwise,

fo '(k) ← fo' (k)

R (i) ← R (i).

Here, f'o (k) represents f'ox (k), f'oy (k), f'oz (k), and f'ow (k) of each axis.

After correcting the values below the decimal point for each axis in the same manner as described above, every sampling time is determined in step S8 by using the position increment amount 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. 8) at the K point with respect to each axis is

Px (k) = f'ox (i)

Py (k) = f'oy (i)

Pz (k) = f'oz (i)

Pw (k) = It is obtained by f'ow (i).

Since the target position at the time of K with respect to each axis at every sampling time is always obtained in step S8, in step S9, the positional deviation at the time of K of the robot is obtained by the following equation.

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 time point.)

If the positional deviation of each axis at any K time point is found in step S9, then, in step S10, control is performed on the proportional integral differential (PID) control to the positional deviation of each axis at the time K point obtained in step S9. Calculate as

Here, Dxout, Dyout, Dzout, and Dwout are values inputted to the digital-to-analog converters 2 on each axis, and K _Pz , K _Py , K _Pz , K _Pw are proportional gains of each axis and K _Ix , K _Iy , K _Iz and K _Iw are gains of the integral (I) of each axis. K _Dx , K _Dy K _Dz , and K _Dw are derivative (D) gains of each axis.

The proportional integral derivatives 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 into an analog value. After converting the signal to the servo control unit 3, the motor 4 is driven.

Subsequently, in step S11, a position point for determining the positional deviation is inserted into K by setting the K + 1 value after a predetermined time has elapsed, and then in step S12, when the value of K has no acceleration or deceleration, the moving pulse for every sampling time It is determined whether or not the number N of (fi) is the same. If this determination result is the same (Yes), in step S13, the Rx, Ry, Rz, and Rw values of each axis are obtained for each axis obtained by obtaining the number N of moving pulses fi for every sampling time when there is no acceleration. It is determined by substituting the moving pulses of (fix, fiy, fiz, fiw), and in step S14, it is determined whether the value of k is larger than the number N of the moving pulses fi for every sampling time. If the result of the determination is large (Yes), the process proceeds to step S15, where 0 is substituted for the moving pulses (fix, fiy, fiz, fiw) for each axis, and 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, if the value of K in step S12 is not equal to the number N of the moving pulses fi for every sampling time (when K = N, No), the process proceeds to step S14 and the value of K in step S14. If the value is not greater than the number N of the moving pulses fi for every sampling time (No), go directly to step S16 to determine whether or not the robot is the final target position. Run

As described above, according to the robot position control method, each axis is driven by a straight line and an exponential function acceleration and deceleration, and the target position for every sampling time is calculated by the discrete time state equation, so that the accuracy of the position path is improved. Not only high speed control is possible, 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 of each axis and the maximum position deviation 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 / deceleration of the servomotor 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 weight of the position of 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, and the movement pulse for every sampling time when there is no acceleration or 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 according to whether the number is equal to the number of times, and whether the value of the fixed point is greater than the number of times In the robot control method comprising the steps of clearing the moving pulse of each axis or determining the final target position according to whether or not the position increment amount of each axis is obtained so that the speed curve of the linear and exponential function acceleration / deceleration type is obtained. fo (k) = fo (k-1) + [fi (k) -fo (k-1)] / A S (k) = fo (k)-fo (k-n) + s (k-1) f'o (k) = s (k) / n Where S (k) is a parameter for calculating the equation, fo (k) is the positional increment for every sampling time during acceleration and deceleration, and fo '(k) is every sampling time during exponential and linear acceleration and deceleration. The positional weight of each position fi: Movement pulse of each axis for every sampling time when there is no acceleration / deceleration A: Time constant at exponential function acceleration / deceleration n: Time constant at linear acceleration / deceleration) Robot position control method.