KR101713195B1 - Joint angle calculation system of robot and method thereof - Google Patents

Abstract

The present invention relates to a system for calculating a joint angle of a robot and a method thereof for calculating a joint angle of a robot for a CNC lathe having a biaxial / triaxial multi-joint using an industrial PLC ladder program A joint angle calculation system for a robot for a CNC lathe having a first rotary arm (100), a second rotary arm (200), and a third rotary arm (300) And calculates a coordinate value of the third joint C to which the second rotary arm 200 and the third rotary arm 300 are connected, The length 310 between the first joint A and the third joint C is calculated and the angle between the first joint A and the third joint C is calculated and then the first joint A ), The second joint (B), and the third joint (C) (A), the second joint (B), and the third joint (C) by calculating the angles of the first joint (A), the second joint (B) Servo angle is calculated and provided based on angle. Accordingly, the same effect can be provided without using an expensive robot controller, and an expensive CNC lathe robot can be supplied at low cost.

Description

[0001] Joint angle calculation system of robot and method [

[0001] The present invention relates to a system for calculating a joint angle of a robot and a method thereof, and more particularly, to a system and a method for calculating a joint angle of a robot for a CNC lathe having two or three axis joints by using an industrial PLC ladder program And more particularly, to a system and a method for calculating a joint angle of a robot.

CNC machine tools are designed to improve the quality and productivity by automatically processing the materials based on the programs written according to the products. In accordance with the development of the industry, the range of use is in machinery, automobile, aviation, shipbuilding, IT, It is gradually expanding.

The general process is manual work of the worker by supplying workpieces to the CNC lathe and taking out the workpieces after the work. This results in excessive waste of the working time and the quality of the product depending on the skill of the worker, There is a problem of productivity deterioration.

Accordingly, a gantry loader, an industrial robot, a vertical loading system, a horizontal loading system, and the like have been used as devices capable of automatically supplying and extracting workpieces with a CNC lathe in the recent years.

Especially, in the industrial field, the mechanical structure of the arm has a rotating joint, which is a parallel axis, and a scalar robot, which is a robot having compliance in a plane perpendicular to the axis, and joints such as human shoulders, arms, elbows and wrists, Many articulated manipulators (articulated robots) can be used to exercise similar to exercise.

In order to control the scara robot or the articulated robot, the work path, torque, acceleration / deceleration and other parameters of the robot should be set using a PC or a separate robot controller.

However, since such a PC or a separate robot controller is so expensive as to cost more than 30% of the robot system, there is a problem in that it is practically impossible to operate the robot system of small and medium enterprises and small enterprises.

Further, since the robot system is mounted on the front face of the machine tool, there is a problem that it is inconvenient for a worker to perform tool change and other work.

In addition, since the robot system has a high weight of 130 Kg or more, there is a problem that the risk of collision is very high.

Japanese Laid-Open Patent Application No. 1995-155947 [entitled " Miso Pad Dicing Machine] Korean Patent No. 10-0834919 [Title of the Invention: Material Supply and Workpiece Take-Out Device of Machine Tool] Japanese Unexamined Patent Publication No. 2009-202236 [Title of the invention: Spindle unit with lift function, CNC machine tool and machining line] Korean Patent No. 10-1149361 [Title of the invention: Rapid loading and unloading system for machine tools]

In order to satisfy such a necessity, the present invention relates to a method for calculating a joint angle of a robot, which can calculate a joint angle of a robot for a CNC lathe having two or three axis joints by using an industrial PLC ladder program, System and a method thereof.

The joint angle calculation system of the robot according to the embodiment of the present invention includes a first rotary arm 100, a second rotary arm 200, and a third rotary arm 300, and a robot for a CNC lathe A joint angle calculating system for calculating a coordinate value of a third joint C provided in the CNC lathe robot and connected to the second rotary arm 200 and the third rotary arm 300, The first joint A and the third joint C connected to the first rotary arm 100 and the third joint C are connected to form a triangle and then the first joint A and the third joint C C between the first joint A and the third joint C by using an angle between the first joint A and the third joint C calculated after calculating the angle between the first joint A and the third joint C 310), calculates the area S of the triangle using the lengths AB, BC, and AC of the triangle three sides, calculates the area S of the calculated triangle, A second joint B to which the first joint A, the first rotary arm 100 and the second rotary arm 200 are connected based on the length of each side of the triangle, The joint angles are calculated and the joint angles of the first joint (A), the second joint (B) and the third joint (C) are calculated using the calculated joint angles of the first joint (A) B), and a robot controller that calculates and provides a servo angle that is a rotation angle of the third joint (C).

As an embodiment related to the present invention, the length AB of the two sides of the triangle AB may be equal to the length of the first rotary arm 100 and the second rotary arm 200.

As an embodiment related to the present invention, the angle between the first joint A and the third joint C can be calculated by the following equation: ac_angle = atan (Cy / Cx).

As an embodiment related to the present invention, the joint angles of the first joint A, the second joint B and the third joint C are as follows: A = asin (2S / (AB * AC) (2S / (AB * BC)) and C = asin (2S / (AC / BC)), and the servo angle is calculated by the following equations: servo_A = ac_angle + A, Servo_B = B, servo_C = t_angle- Lt; / RTI > Here, S is the area of the triangle, AB, AC, and BC are the lengths of the sides of the triangle, and A, B, and C are the servo angle values of each joint set in advance.

The robot controller 600 calculates the joint angles of the first rotary arm 100, the second rotary arm 200 and the third rotary arm 300 according to the method of calculating the joint angle of the robot according to the embodiment of the present invention In the method of calculating the joint angle of a robot for a CNC lathe including a robot controller 600 for controlling the front and rear transfer unit 400 and the right and left transfer unit 500 and capable of vertical vertical operation, Calculating coordinate values (X, Y) of a third joint (C) connected to the second rotary arm (200) and the third rotary arm (300); A first joint A connecting the first rotary arm 100 and an end of each of the third joint C are connected to form a triangle and then the first joint A and the third joint C, The angle between the joints C is calculated and an angle between the calculated first joint A and the third joint C is used to calculate the angle between the first joint A and the third joint C, Calculating a length (310) of the object; The second joint (B) to which the first joint (A), the first rotary arm 100 and the second rotary arm 200 are connected by using the calculated triangle area and the length of each side of the triangle, Calculating a joint angle of the joint (C); And calculating the servo angle using the calculated joint angles of the first joint (A), the second joint (B), and the third joint (C).

As an embodiment related to the present invention, the length AB of the two sides of the triangle AB may be equal to the length of the first rotary arm 100 and the second rotary arm 200.

As an embodiment related to the present invention, the angle between the first joint A and the third joint C can be calculated by the following equation ac_angle = atan (Cy / Cx).

As an embodiment related to the present invention, the joint angles of the first joint A, the second joint B and the third joint C are as follows: A = asin (2S / (AB * AC) (2S / (AB * BC)) and C = asin (2S / (AC / BC)), and the servo angle is calculated by the following equations: servo_A = ac_angle + A, Servo_B = B, servo_C = t_angle- Lt; / RTI > Here, S is the area of the triangle, AB, AC, and BC are the lengths of the sides of the triangle, and A, B, and C are the servo angle values of each joint set in advance.

The present invention can calculate the joint angles of a robot for a CNC lathe having a biaxial / triaxial multi-joint using an industrial PLC ladder program to drive a manifold, thereby achieving the same effect without using an expensive robot controller It is possible to provide an expensive CNC lathe robot at a low cost.

The present invention can provide a CNC lathe robot at a low cost, thereby making it possible to eliminate the manpower of the enterprise, to create added value, and to improve the competitiveness of companies and nations.

1 is a block diagram for explaining a joint angle calculating system of a robot according to the present invention.
Fig. 2 is a geometric analysis diagram of the joint angle calculation of the CNC lathe robot.
FIG. 3 is a flowchart illustrating a method for calculating a joint angle of a robot according to the present invention.

It is noted that the technical terms used in the present invention are used only to describe specific embodiments and are not intended to limit the present invention. In addition, the technical terms used in the present invention should be construed in a sense generally understood by a person having ordinary skill in the art to which the present invention belongs, unless otherwise defined in the present invention, Should not be construed to mean, or be interpreted in an excessively reduced sense. In addition, when a technical term used in the present invention is an erroneous technical term that does not accurately express the concept of the present invention, it should be understood that technical terms can be understood by those skilled in the art. In addition, the general terms used in the present invention should be interpreted according to a predefined or prior context, and should not be construed as being excessively reduced.

Furthermore, the singular expressions used in the present invention include plural expressions unless the context clearly dictates otherwise. In the present invention, terms such as "comprising" or "comprising" and the like should not be construed as encompassing various elements or stages of the invention, Or may further include additional components or steps.

Hereinafter, exemplary embodiments of the present invention will be described in detail with reference to the accompanying drawings, wherein like reference numerals refer to like or similar elements throughout the several views, and redundant description thereof will be omitted.

In the following description, well-known functions or constructions are not described in detail since they would obscure the invention in unnecessary detail. It is to be noted that the accompanying drawings are only for the purpose of facilitating understanding of the present invention, and should not be construed as limiting the scope of the present invention with reference to the accompanying drawings.

1 is a block diagram for explaining a joint angle calculating system of a robot according to the present invention.

1, the CNC lathe robot includes a first rotary arm 100, a second rotary arm 200, and a third rotary arm 300. The first rotary arm 100, the second rotary arm 200, A front and rear transport unit 400 having front and rear actuators for controlling the forward and backward movements of the rotary arm 200 and the third rotary arm 300; A left and right moving unit 500 having left and right actuators for controlling the left and right motions of the arm 300 and a joint angle And a robot controller 600 for controlling the forward / backward transport unit 400 and the left / right transport unit 500 by calculating the angles. Here, the robot controller 600 is made up of an industrial PLC (programmable logic controller) which can be implemented at a low cost. The robot controller 600 can be realized by using a ladder program of an industrial PLC and calculating a joint angle of a scalar or articulated robot having two degrees of freedom or three degrees of freedom and calculates a joint angle, in particular, a robot controller 600 that calculates a joint angle using a trigonometric function.

The robot controller 600 is provided with a first rotary arm 100, a second rotary arm 200 and a third rotary arm 300. In the joint angle calculation system of a robot for a CNC lathe, The coordinate values of the third joint C provided to the CNC lathe robot and to which the second rotary arm 200 and the third rotary arm 300 are connected are calculated and the first rotary arm 100 is connected The angles between the first joint A and the third joint C are calculated based on the x axis after the ends of the first joint A and the third joint C are connected to form a triangle, The length 310 between the first joint A and the third joint C is calculated using the angle between the first joint A and the third joint C and the lengths AB, BC and AC are used to calculate the area S of the triangle and the first joint A, the first rotary arm 100 and the second rotary arm 200 are connected based on the area S of the triangle There (A), the second joint (B), and the third joint (C) by using the joint angles of the second joint (B) and the third joint (C) 1 Servo angle that is the rotation angle of the joint (A), the second joint (B), and the third joint (C) is calculated and provided.

The length AB of the two sides of the triangle is equal to the length of the first rotary arm 100 and the second rotary arm 200.

 The angle between the first joint A and the third joint C is calculated by the following equation ac_angle = atan (Cy / Cx).

The joint angles of the first joint A, the second joint B and the third joint C are as follows: ) And C = asin (2S / (AC / BC)), and the servo angle is calculated by the equations servo_A = ac_angle + A, Servo_B = B, servo_C = t_angle-servo_B-servo_A. Here, S is the area of the triangle, AB, AC, and BC are the lengths of the sides of the triangle, and A, B, and C are the servo angle values of each joint set in advance.

A method of calculating the joint angle of the robot configured as described above will be described below.

Fig. 2 is a geometric analysis diagram of the joint angle calculation of the CNC lathe robot. FIG. 3 is a flowchart illustrating a method for calculating a joint angle of a robot according to the present invention.

2 and 3, the robot controller 600 calculates coordinate values (X, Y) of the third joint C to which the second rotary arm 200 and the third rotary arm 300 are connected, (S110).

The equation for calculating the coordinate values (X, Y) of the third joint C is C (x) = Tx-CT * cos (t_angle) , The equation for calculating the length 310 between the first joint A and the third joint C is AC = sqr (Cx ² + Cy ² ). The length 310 between the first joint A and the third joint C can be calculated based on the Pythagorean theorem. That is, when a route is applied to a result obtained by summing the square of the X coordinate value of the third joint C and the square of the Y coordinate value of the third joint C calculated by the above equation, the hypotenuse of the right triangle The length 310 between the first joint A and the third joint C is calculated. Here, AC is the length between the first joint (A) and the third joint, and Cx and Cy are the lengths of the two sides having the right angle with the hypotenuse of the AC. CT is a length of the third rotary arm 300 starting from the third joint C and T is a target point of supply, machining and extraction of the workpiece, and coordinates Tx, Ty is predetermined in correspondence with the method of feeding the workpiece to the CNC lathe and taking out the workpiece after the machining operation. The t_angle is the angle of the third rotary arm 300, and the workpiece is also fed to the CNC lathe, Is predetermined.

2, the robot controller 600 forms a triangle by connecting the ends of the first joint A and the third joint C, and then forms a triangle using the equation ac_angle = atan (Cy / Cx The angle between the first joint A and the third joint C is calculated using the angles between the first joint A and the third joint C, ) And the third joint C (step S120). The robot controller 600 has the lengths AB, BC, and AC of the three sides of the triangle having the first joint A, the second joint B, and the third joint C as vertexes. Here, the lengths AB and BC of the two sides of the triangle are known by the lengths of the first rotary arm 100 and the second rotary arm 200, respectively.

delete

The robot controller 600 compares the lengths AB, BC, AC of the triangular three sides obtained through step S130 with the equation S = sqr (s * (s-AB) * (s- (S) = (AB + BC + AC) / 2, where s is the area of the triangle.
That is, the parameters (s) and the area (S) of the triangle mentioned above are calculated by the Heron's formula proof method.
Here, using the sine law, sin (A) / BC = sin (B) / AC = sin (C) / AB = AB * BC * AC / 2S. Therefore, the robot controller 600 computes the above from the following equations for the mathematical formulas A = asin (2S / (AB * AC)), B = asin (2S / (AB * BC) The joint angles of the first joint A, the second joint B, and the third joint C are calculated (S130).
The robot controller 600 calculates the joint angles of the first joint A, the second joint B and the third joint C by the following equations: servo_A = ac_angle + A, Servo_B = B, servo_C = t_angle-servo_B- To calculate the servo angle.
S is the area of the triangle, AB, AC, and BC are the lengths of the sides of the triangle, and A, B, and C are the servo angle values of each joint set in advance.
The robot controller 600 provides the calculated servo angles to the forward and backward transport units 400 and the left and right transport units 500 in step S140 so as to calculate the calculated first and second joints A, The first rotary arm 100, the second rotary arm 200, and the third rotary arm 300 are rotated based on a servo angle that is a rotation angle of the third joint C.

delete

delete

delete

delete

It will be apparent to those skilled in the art that various modifications and variations can be made in the present invention without departing from the spirit or essential characteristics thereof. Therefore, the embodiments disclosed in the present invention are intended to illustrate rather than limit the scope of the present invention, and the scope of the technical idea of the present invention is not limited by these embodiments. The scope of protection of the present invention should be construed according to the following claims, and all technical ideas within the scope of equivalents should be construed as falling within the scope of the present invention.

100: first rotary arm
200: second rotary arm
300: Third rotary arm
A: First joint
B: second joint
C: Third joint

Claims (8)

A joint angle calculating system for a robot for a CNC lathe having a first rotary arm (100), a second rotary arm (200), and a third rotary arm (300)
The coordinate system of the third joint C provided in the CNC lathe robot and connected to the second rotary arm 200 and the third rotary arm 300 is calculated, The angle between the first joint (A) and the third joint (C) is calculated based on the x-axis after forming the triangle by connecting the ends of the connected first joint (A) and the third joint (C) A length 310 between the first joint A and the third joint C is calculated using the angle between the first joint A and the third joint C calculated after the first joint A and the third joint C, The area S of the triangle is calculated by using the lengths AB, BC, and AC of the triangle three sides, and the first joint A, the second joint A, and the second joint A are calculated based on the area S of the calculated triangle and the lengths of the sides of the triangle. A second joint (B) to which the first rotary arm (100) and the second rotary arm (200) are connected, a joint angle of the third joint (C) (A), the second joint (B), the third joint (C), and the third joint (C) by using the joint angles of the first joint (A), the second joint And a robot controller for calculating and providing a servo angle that is a rotation angle of the robot.
The method according to claim 1,
The length AB of the two sides of the triangle is equal to the length of the first rotary arm 100 and the second rotary arm 200.
The method according to claim 1,
Wherein the angle between the first joint (A) and the third joint (C) is calculated by the following equation: ac_angle = atan (Cy / Cx).
The method according to claim 1,
The joint angles of the first joint A, the second joint B and the third joint C are expressed by the following equations: A = asin (2S / (AB * AC)), B = asin (2S / ), And C = asin (2S / (AC / BC)), and the servo angle is calculated by the following equation: servo_A = ac_angle + A, Servo_B = B, servo_C = t_angle-servo_B- Joint Angle Calculation System of Robot.
Here, S is the area of the triangle, AB, AC, and BC are the lengths of the sides of the triangle, and A, B, and C are the servo angle values of each joint set in advance.
A robot controller 600 for calculating the joint angles of the first rotary arm 100, the second rotary arm 200 and the third rotary arm 300 and controlling the front and rear transfer units 400 and the left and right transfer units 500, A method for computing a joint angle of a robot for a CNC lathe including a vertical up and down operation,
The robot controller 600 may calculate the coordinate values X and Y of the third joint C to which the second rotary arm 200 and the third rotary arm 300 are connected.
A first joint A connecting the first rotary arm 100 and an end of each of the third joint C are connected to form a triangle and then the first joint A and the third joint C, The angle between the joints C is calculated and an angle between the calculated first joint A and the third joint C is used to calculate the angle between the first joint A and the third joint C, Calculating a length (310) of the object;
The second joint (B) to which the first joint (A), the first rotary arm 100 and the second rotary arm 200 are connected by using the calculated triangle area and the length of each side of the triangle, Calculating a joint angle of the joint (C); And
And calculating a servo angle using the calculated joint angles of the first joint (A), the second joint (B), and the third joint (C).
6. The method of claim 5,
The triangle is formed such that each of the first joint (A), the second joint (B), and the third joint (C)
The length of either side of the triangle connecting the first joint A and the second joint B is equal to the length of the first rotary arm 100 and the length of the second joint B and the third joint B C) is equal to the length of the second rotary arm (200).
6. The method of claim 5,
Wherein an angle between the first joint (A) and the third joint (C) is calculated by the following equation: ac_angle = atan (Cy / Cx).
6. The method of claim 5,
The joint angles of the first joint A, the second joint B and the third joint C are expressed by the following equations: A = asin (2S / (AB * AC)), B = asin (2S / ), And C = asin (2S / (AC / BC)), and the servo angle is calculated by the following equation: servo_A = ac_angle + A, Servo_B = B, servo_C = t_angle-servo_B- The method of calculating the joint angle of a robot.
Here, S is the area of the triangle, AB, AC, and BC are the lengths of the sides of the triangle, and A, B, and C are the servo angle values of each joint set in advance.

Images