KR930000128B1 - Position detecting method of rectangular coordinate - Google Patents

Abstract

The method simplifies the calculation of the X,Y coordinates by using the rotation of the coordinate axis. The method comprises: (a) reading conditions of X1 for starting point, X2 for passing point, X3 for arriving point, and Δl for arc length against the fine displacement; (b) calculating the radius (r) against the circular arc by using the conditions; (c) calculating the shift angle (ΔO) by using the r and Δl; (d) determining the rotating direction; (e) calculating the position coordinates against the coordinate rotation; and (f) storing the coordinate value of X3 when the present calculation value reaches the approximate point.

Description

Method for detecting plane moving Cartesian coordinate position in circular interpolation.

1 is a hardware block diagram for carrying out the present invention.

2 is a flow chart in accordance with the present invention.

3 is a coordinate state diagram illustrating the present invention.

* Explanation of symbols for main parts of the drawings

10: CPU 12: Pendent

14: Program loader 16: Interface

18: W-axis motor 20: memory

22: DAC 24, 26, 28: 1st, 2nd, 3rd motor

42: taco generator section 44: encoder section

46: counter

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to circular interpolation of industrial equipment. In particular, the present invention relates to a method for detecting a positive and reverse planar moving coordinate position using coordinate side rotation in circular interpolation of industrial equipment.

In the case of industrial equipment which normally operates along the arc, for example, numerical control equipment such as CNC, etc., and circular interpolation such as robots, the position of the next X and Y coordinates to be moved from the computer to the main controller at every sampling time according to the given arc. Should be calculated.

In the conventional industrial equipment, the method of calculating the position of the X and Y coordinates as described above is calculated by using the method of calculating the moving coordinate position (ΔX, ΔY) from the trigonometric law from the length of the chord for the micro displacement, and using the equation of the circle The moving coordinate position was calculated.

However, the calculation method as described above is very complicated and the microprocessor requires a very large amount of calculation time, which causes a problem of high speed operation. In particular, the robot has a large amount of calculations, but the fast calculation of each part is urgently required, but the calculation time is long.

Accordingly, an object of the present invention is to provide a method for high-speed calculation of a plane moving rectangular coordinate position using coordinate side rotation in an industrial device using a microprocessor.

Another object of the present invention is to provide a method for simplifying calculation of forward and backward coordinates using X and Y coordinates.

Hereinafter, the present invention will be described in detail with reference to the accompanying drawings.

1 is a hardware block diagram for carrying out the present invention, which reads at least three or more arc coordinate values to determine a rotation direction, compares the target position by the arc coordinate value with the current position of the control target, and compares the deviation. A CPU 10 for calculating speed command data for calculating and reaching the neck position, a pendent 12 for setting a desired target position value to an arc coordinate value, and a program for the CPU (10). A program loader 14 for loading into the 10, an interface 16 for interfacing outputs of the pendant 12 and the program loader 14 to the CPU 10, and the interface ( 16, a W-axis motor 18 that rotates under the control of the CPU 10, and a predetermined program data, and a memory that reads / writes predetermined data under the control of the CPU 40 ( 20) and outputted from the CPU 10 A digital to analog converter (DAC) 22 for converting speed command data into an analog voltage and outputting the same, and the analog voltage output from the DAC 22 is inputted as a reference voltage and a tacho generator 42. First, second, and third motors 24, 26, 28 having a servo pack for controlling the motor speed as a secondary by inputting a feed vaccine from the feeder; and the first and second And a taco generator connected to the shafts of the third motors 24, 26, 28 to feed back the current speed of the motor to the SERVOPACKs of the first, second, and third motors 24, 26, 28. (42) and encoder portions (44) connected to the axes of the first, second, and third motors (24, 26, 28) to generate pulse trains of a period proportional to the rotational speed of the motors, respectively; And a count 46 which up / down counts each pulse train of the encoder unit 44 and inputs a current position value to the CPU 10.

2 is a flow chart according to the present invention, a conditional lead process for leading a given starting point X1, passing point X2, arrival point X3 and the length of the string Δℓ for the microdisplacement, and the starting point X1, passing point X2, arrival point X3 and circle A radius calculation process for an arc for calculating a radius r from a center coordinate, a movement angle calculation process for calculating and storing a movement angle Δθ as a radius r of the radius calculation process and a string length Δℓ of a condition lead process, and the condition lead As the Cartesian coordinates of the starting point X1, the passing point X2, and the arrival point X3 of the process, a rotation direction discrimination process for discriminating the forward and reverse rotation directions, and the coordinates of the travel angle calculated in the movement angle calculation process and the starting point X1 of the lead process Position coordinate calculation process of calculating and storing position coordinate by rotation of coordinate axis, and the position coordinate system until the currently calculated position coordinate reaches the coordinate of arrival point X3 It performs a process and consists of a storage process of storing the coordinates of the end points, X3 upon reaching the approximate point.

3 is a diagram illustrating a state of coordinate movement for explaining the present invention, wherein 3A shows an example in which the coordinate axis is moved by a movement distance Δℓ for the small displacement angle Δθ, and 3B is an arc passing through a starting point X1, an intermediate point X2, and an arrival point X3. The coordinate axes X 'and Y' are shifted by Δθ to obtain the X and Y coordinates.

3C shows the coordinates calculated by the rotation of the coordinate axes at every sampling period.

An operation example of FIG. 2 according to the present invention will now be described with reference to FIGS. 1 and 3.

First, a method of calculating X and Y coordinates using coordinate axis rotation will be described. Given the movement length Δℓ as shown in FIG. 3A and the radius r, the Δθ is obtained by the following equation (A) by the formula of the movement length Δℓ = rΔθ.

Δθ = ΔL / r... … … … … … … … … … … … … … … … … … (Eq. 1)

Therefore, it can be seen that the small displacement Δθ can be obtained by the above equation (A) given the moving length Δℓ and the radius r of the coordinate axis.

As shown in FIG. 3B, in order to find the x, y coordinate of the arc passing through the starting point X1 (x1, y1), the passing point X2 (x2, y2) and the arrival point X3 (x3, y3) on the x, y plane coordinates, Assuming the moved coordinate axes x ', y', the moving point B of the main point A in the x, y axes can be obtained as shown in Equations (2a) and (2b).

x = x'cos Δθ-y'sinΔθ... … … … … … … … … … … … (2a)

y = x'sin Δθ-y'sinΔθ... … … … … … … … … … … … (2b)

That is, the substitution value can be obtained from the x and y coordinates in which A point is moved to B point.

Since cos Δθ is α and sin Δθ is β, α and β also become constants.

In addition, x 'and y' are the same as x and y in terms of another movement Δθ, so that the above formulas (2a, 2b) are represented by the following formulas (3a, 3b).

x (K + 1) = alpha x (K)-beta y (K)... … … … … … … … … … … (3a)

y (K + 1) = βx (K) -αy (K)... … … … … … … … … … … (3b)

In Formulas 3a and 3b, kK is a constant and is a moving coordinate of x 'and y'.

Therefore, calculation of cos and sin functions that takes a long time is done only once, and calculations of products and +,-, etc. can be performed sequentially so that high-speed operation can be achieved by applying a microcomputer.

On the other hand, in the general case where the origin of the circle is (a, b) as shown in FIG. 3B, the following equation (4a, 4b) is moved to the origin (0,0) of the circle before calculating (Equations 3a, 3b). Let's do it.

x (K) = x (x) -a... … … … … … … … … … … … … … … … … (4a)

y (K) = y (x) -b... … … … … … … … … … … … … … … … … (4b)

Calculate Equation (4a, 4b) and return to the origin (a, b) with x (K + 1) = x (K + 1) + a, y (K + 1) = y (K + 1) + b). By moving to the coordinate position of x, y by the coordinate axis rotation can be calculated. That is, x (K) = x1-a, y (K) = y1-b

However, if X1 = (x1, y1), the above-described formulas (4a, 4b) are as shown in the following formulas (5a, 5b).

x (K + 1) = alpha x (K)-beta y (K)... … … … … … … … … … … (5a)

y (K + 1) = βx (K) -αy (K)... … … … … … … … … … … (5b)

Therefore, when the origin of the circle is (a, b), the x.y coordinate by the rotation of the coordinate axis can be obtained by the above expressions (5a, 5b).

Now, for the desired arc control, the first rectangular coordinate x1 having (X1, Y1), the second rectangular coordinate x2 having (X2, Y2), and (x3) on the X, Y plane rectangular coordinates as shown in FIG. When the third rectangular coordinate X3 having the coordinate of y3) is set using the pendant 12, the rectangular coordinate values X1, X2, and X3 are input to the CPU 10 through an interface.

The CPU 10 reads the Cartesian coordinate values X1 and X3 input in the process of FIG. 2A and reads the length of the chord, that is, the moving length Δℓ, for the microdisplacement stored in the memory 20. At this time, the movement length Δℓ is set by the user so that the CPU 10 can access it from the memory 20.

In the process of FIG. 2A, the CPU 10 having read the coordinate values X1, X2, and X3 in FIG. 2A calculates the radius r ² by Equation (6) in the process of FIG. 2B and stores it in the internal memory area.

r ² = (xa) ² + (yb) ² . … … … … … … … … … … … … … (6)

In Formula 6, a is the x-coordinate of the center of the circle, b is the y-coordinate of the center of the circle.

Therefore, the CPU 10, which recognizes the movement length Δℓ and the radius r of the strings in FIG. 2A and 2B, obtains the small displacement angle Δθ as shown in Equation 7 below in the process of FIG. 2C by using Equation 7 below. .

DELTA L = r DELTA? … … … … … … … … … … … … … … … … … (7)

Δθ = ΔL / r... … … … … … … … … … … … … … … … … … (8)

Therefore, the CPU 10 recognizes the small displacement angle Δθ of the small moving distance Δℓ with respect to the predetermined radius r, and sets the values of the angle Δθ of the small displacement calculated by the above equation (8) as the values of cos Δθ and sinΔθ. Store in internal memory.

The CPU 10 having obtained Δθ by the above processes determines whether the rotation direction is the forward or reverse direction from the data of the rectangular coordinates of the starting point X1, the passing point X2, and the arrival point X3 given in the process of FIG. 2D.

When the rotation direction is reverse in the search result of the 2D process, after converting and setting Δθ calculated in the process of FIG. 2C from 2C to -Δθ in the 2E process, the position is calculated by the rotation of the coordinate axis in the 2F process (9a, 9b) Execute as

If the rotation direction discrimination in the process of FIG. 2D is the radial direction, the CPU 10 detects the position due to the coordinate axis rotation by directly performing the calculation as shown in Equations 9a and 9b in process 2F of FIG. .

x = x (K + 1) + a... … … … … … … … … … … … … … … … (9a)

y = y (K + 1) + y... … … … … … … … … … … … … … … … (9b)

In FIG. 2F, the coordinate values of x and y in which the coordinate axis is rotated by the small displacement Δθ are calculated in every sampling cycle from the starting point X1 in the same manner as the above formulas (5a and 5b), and x is determined according to the rotation direction. It will calculate the coordinate value of, y.

The position calculation value of the coordinate axis rotation calculated in FIG. 2F by FIG. 2F is stored in the memory 20 by the CPU 10 in 2G, and whether the current calculated position in step 2H is close to the arrival point X3 input by the user. To judge.

If the current position is not close to the arrival point X3, the coordinate axis is rotated by Δθ and the above-described process is performed in the next sampling period to calculate the next position coordinate value.

If the current calculated coordinate value coincides with the coordinate value of the arrival point X3, the coordinate value of the arrival point X3 is stored in the memory 20 in step 2I and the coordinate position calculation is completed.

Therefore, when the x, y coordinate position by the coordinate axis rotation calculated in every sampling period in FIG.

As described above, the present invention can calculate the coordinate position at high speed in circular interpolation by performing the coordinate position calculation by the rotation of the coordinate axis with respect to the small displacement Δθ, thereby enabling high speed operation.

Claims (1)

In the planar moving Cartesian coordinate position detection method of the system which performs circular interpolation, the condition lead process which leads the given starting point X1, the passing point X2, the arrival point X3, and the string length (DELTA) with respect to a small displacement, and said starting point X1, the passing point X2 , A radius calculation process for a circular arc that calculates a radius r from the arrival point X3 and the center coordinates of the circle, and a travel angle calculation that calculates and stores the movement angle Δθ as the radius r of the radius calculation process and the string length Δℓ of the conditional lead process. And a rotation direction discrimination process for discriminating forward and reverse rotation directions as an orthogonal coordinate of a starting point X1, a passing point X2, and an arrival point X3 of the condition lead process, and the movement angle calculated in the moving angle calculation process and the lead process. The position coordinate calculation process of calculating and storing the position coordinate by the rotation of the coordinate axis as the coordinate value of the starting point X1, and the currently calculated position coordinate is the left of the arrival point X3. Performing a process of calculating the position coordinate until reaching a table and storing a coordinate value of the arrival point X3 when the approximate point is reached.

Images