KR102520793B1 - Method for blending motion route of robot and apparatus thereof - Google Patents

Abstract

A robot motion path blending method and apparatus are provided. A method for blending a motion path of a robot according to an embodiment of the present invention includes sequentially storing a path movement command for the robot in a queue, processing a motion of a current node that is currently executing a path movement command, and blending the current node. Determining whether a condition is satisfied, processing the motion of the next node at the same time if the current node satisfies the blending condition, converting the motion of the current node and the motion of the next node into the same coordinate system. , and blending so that parts of the motion path for the current node and the motion path for the next node overlap.

Description

Method for blending motion path of robot and apparatus thereof

The present invention relates to a motion path blending method and apparatus of a robot, and more particularly, to a motion path blending method and apparatus applicable to a motion controller of an industrial robot used in a manufacturing process.

In general, when a robot sequentially performs two or more unit motions, the robot can continuously perform the motions without stopping by overlapping and blending parts of the currently performed motion and the next motion to be performed. As advantages of continuous motion due to blending, motion execution time can be shortened and noise or vibration can be reduced.

However, motion blending of conventional robots may blend unit motions expressed in the same coordinate system, blend joint motions with joint motions, or blend task motions with task motions. If the coordinate system representing the motion is different or the space representing the motion is different, such as joint motion and task motion, there is difficulty in blending.

KR 10-2020-0022043 A

In order to solve the above problems of the prior art, an embodiment of the present invention is a robot motion path blending method capable of blending unit motions expressed in various coordinate systems of the robot or unit motions expressed in a joint space or task space. And to provide the device.

According to one aspect of the present invention for solving the above problems, path movement commands for the robot are sequentially stored in a queue; processing a motion for a current node that is currently executing a path movement command; determining whether the current node satisfies a blending condition; simultaneously processing motion for a next node when the current node satisfies the blending condition; transforming the motion of the current node and the motion of the next node into the same coordinate system; and blending a motion path for the current node and a part of the motion path for the next node to overlap each other.

In an embodiment, the converting may include converting a joint coordinate system of a joint space among the current node and the next node to a base coordinate system of the task space when the current node and the next node have different coordinate systems.

In one embodiment, the transforming step may transform coordinates by forward kinematics of the robot.

In one embodiment, the determining step may include performing motion processing for the current node at least half of a moving distance, a next node in the queue, and the remaining execution time of motion for the current node being the next node. If it is within the blending time range of , it may be determined that the blending condition is satisfied.

In one embodiment, the blending time for the next node may be calculated as an inverse function of a predetermined profile function for obtaining a movement distance with respect to time.

In one embodiment, the blending step may calculate the blended position ( p ) by the following equation:

Here, x ₀ is a vector representing a position of the current node on a motion path, x ₁ is a vector representing a position of the next node on a motion path, and p ₀ is a vector representing the position of the next node.

In one embodiment, the method of blending the motion path of the robot may include, after the blending step, converting a position of a task space into a position of a joint space, generating an angle in the joint space, and transmitting a joint driving command to the robot. may further include.

In one embodiment, each of the step of processing the motion for the current node and the step of simultaneously processing the motion for the next node normalizes the movement amount on the motion path for each node and then calculates the position on the motion path by interpolation. can be calculated

In one embodiment, when the current node does not satisfy the blending condition, motion processing for the current node may be completed by interpolation.

According to another aspect of the present invention, a queue in which path movement commands for the robot are sequentially stored; and a control unit sequentially executing the path moving command stored in the queue. Processes a motion for a current node that is executing a current path movement command, determines whether the current node satisfies a blending condition, and if the current node satisfies the blending condition, the next path received from the queue Based on the move command, motion for the next node is simultaneously processed, the motion for the current node and the motion for the next node are converted into the same coordinate system, and the motion path for the current node and the motion path for the next node are converted. An apparatus for blending a motion path of a robot that blends so that a part of is overlapped is provided.

In an embodiment, when the current node and the next node have different coordinate systems, the controller may convert a joint coordinate system of a joint space among the current node and the next node into a base coordinate system of the task space.

In one embodiment, the control unit may transform the coordinates by the regular kinematics of the robot.

In one embodiment, the control unit performs motion processing for the current node by half or more of the moving distance, a next node exists in the queue, and the remaining execution time of the motion for the current node is determined by blending of the next node. If it is within the time range, it may be determined that the blending condition is satisfied.

In one embodiment, the control unit may calculate the blending time for the next node by an inverse function of a predetermined profile function for obtaining a movement distance with respect to time.

In one embodiment, the control unit may calculate the blended position ( p ) by the following equation:

Here, x ₀ is a vector representing a position of the current node on a motion path, x ₁ is a vector representing a position of the next node on a motion path, and p ₀ is a vector representing the position of the next node.

In one embodiment, the control unit may transmit a joint drive command to the robot by converting a position of the task space into a position of the joint space after the blending and generating an angle in the joint space.

In one embodiment, when processing motion for the current node and simultaneously processing motion for the next node, the control unit normalizes a movement amount on the motion path for each node and then interpolates on the motion path. location can be calculated.

In one embodiment, when the current node does not satisfy the blending condition, the controller may complete motion processing for the current node by interpolation.

The robot motion path blending method and apparatus according to an embodiment of the present invention blend unit motions such as arcs, segments, and joint angular movements according to blending conditions, so that continuous motion of the robot can be quickly implemented in the manufacturing process. The robot's working time can be shortened.

A method and apparatus for blending motion paths of a robot according to an embodiment of the present invention can quickly and stably implement continuous motion of a robot in a manufacturing process by blending unit motions with respect to different coordinate systems or spaces, thus improving the reliability of robot operation. can improve

1 is a block diagram showing a motion path blending device for a robot according to an embodiment of the present invention.
Figure 2 is a diagram showing a robot coordinate system, (a) is a coordinate system for a 6-axis robot, (b) is a diagram showing a coordinate system for the robot.
3 is a diagram illustrating transformation between a task space and a joint space.
4 is a diagram showing a movement path according to a path movement command function, (a) a joint, (b) a circular arc, (c) a straight line, and (d) a movement path for a circle.
5 is a diagram showing a process of sequentially proceeding two route movement commands, wherein (a) is a movement path according to a route movement command function, and (b) is a diagram showing a process of processing a route movement command.
6 is a diagram for explaining an interpolation method of path movement points, (a) is a diagram for explaining the calculation of the rotational position of the arc, and (b) is a diagram for explaining the calculation of the rotation angle of the arc.
7 is a diagram for explaining the operation of a motion path blending device for a robot according to an embodiment of the present invention, (a) is a basic configuration, (b) is interpolation for one node, (c) is two nodes It is a diagram for explaining the operation when blending for .
8 is a diagram for explaining interpolation of an apparatus for blending a motion path of a robot according to an embodiment of the present invention.
9 is a diagram for explaining blending conditions of a robot motion path blending apparatus according to an embodiment of the present invention, where (a) is normalization and (b) is a diagram for explaining blending length and time.
10 is a diagram for explaining a blending operation of a motion path blending device for a robot according to an embodiment of the present invention.
11 is a diagram for explaining transformation for blending of a motion path blending apparatus of a robot according to an embodiment of the present invention, (a) is a joint space for both nodes, and (b) is a joint space for only the current node. , (c) shows a case where both nodes are task spaces, and (d) shows a case where only the current node is a task space.
12 is a diagram for explaining blending position calculation of a motion path blending device for a robot according to an embodiment of the present invention.
13 is a diagram for explaining a process of transmitting a joint driving command to a robot of a motion path blending device of a robot according to an embodiment of the present invention.
14 is a flowchart illustrating a motion path blending method of a robot according to an embodiment of the present invention.

Hereinafter, with reference to the accompanying drawings, embodiments of the present invention will be described in detail so that those skilled in the art can easily carry out the present invention. This invention may be embodied in many different forms and is not limited to the embodiments set forth herein. In order to clearly describe the present invention in the drawings, parts irrelevant to the description are omitted, and the same reference numerals are assigned to the same or similar components throughout the specification.

Hereinafter, a motion path blending device for a robot according to an embodiment of the present invention will be described in more detail with reference to the drawings. 1 is a block diagram showing a motion path blending device for a robot according to an embodiment of the present invention.

Referring to FIG. 1 , an apparatus 100 for blending a motion path of a robot according to an embodiment of the present invention includes a queue 110 and a control unit 120.

The robot motion path blending device 100 is for blending the movement paths generated by each command when one or more robot 10 movement commands are given, and several path movement commands are input to the queue 110. , the control unit 120 may interpret and execute the command to finally provide a joint drive command for the robot 10 .

More specifically, first, in the robot script input to the motion path blending device 100 of the robot, the robot 10 reads an input value from an input port, moves the robot 10, and outputs an output to an output port. It describes subdivided operations to perform a given task. Here, the motion of the robot 10 is given by a path movement command to move the end of the robot 10 continuously.

These path movement commands may be sequentially input to and stored in the queue 110 . At this time, the control unit 120 sequentially interprets and executes commands stored in the queue 110 one by one from the first input, thereby generating joint drive commands that move each joint of the robot 10 and outputting the commands to the robot 10. .

Hereinafter, in order to help understand the structure of the motion path blending device 100 of the robot according to an embodiment of the present invention and to clarify the description of the blending algorithm, a coordinate system used by the robot is referred to with reference to FIGS. 2 and 3 (Coordinate system) and transformation between joint/task spaces are explained.

Figure 2 is a diagram showing a robot coordinate system, (a) is a coordinate system for a 6-axis robot, (b) is a diagram showing a coordinate system for the robot.

Referring to (a) of FIG. 2, the coordinate system used by the robot is largely divided into a joint space and a task space. Here, the task space is divided into a tool coordinate system and a task coordinate system. As shown in (b) of FIG. 2, the tool coordinate system is a coordinate system based on the end, which is the working part of the robot.

The task coordinate system is divided into a base coordinate system, a world coordinate system, and a work coordinate system according to the location where the coordinate system is set. As shown in (b) of FIG. 2, the base coordinate system is a coordinate system based on the base of the robot, the world coordinate system is an international standard coordinate system, and the work coordinate system is a coordinate system based on the work object. Here, points displayed on three coordinate systems can be converted into points on each coordinate system through a transformation matrix.

In this specification, when subscripts attached before or after an unknown in the expression of a mathematical expression are indicated by letters B, O, and W, this means the type of coordinate system assigned to the unknown. That is, B denotes a base coordinate system, 0 denotes a world coordinate system or an origin coordinate system, and W denotes a work coordinate system.

3 is a diagram illustrating transformation between a task space and a joint space.

Referring to FIG. 3 , the position of a point in a joint space and a task space can be mutually converted through forward kinematics and inverse kinematics of a robot.

The joint space is a coordinate space with one coordinate system axis per joint of the robot. For example, in the case of a 6-axis vertical multi-joint robot, since there are 6 joints, it can be considered as a space having 6 axes, and mathematically, it becomes a vector having only 6 elements.

The task space is a coordinate space with x, y, and z axes in a three-dimensional space, and is composed of the robot end effector's position (x, y, z) and rotation direction (Euler angle; phi, theta, psi). It can be expressed as a vector with up to 6 elements.

In order for the end of the robot to reach the target position and orientation in the task space, the joints of the robot must be rotated at a specific angle. When a target position is given in the task space, joint angles of the robot may be calculated by calculating inverse kinematics of the robot. Conversely, when the joint angle of the robot is known, the position and orientation of the robot end can be known by calculating the forward kinematics of the robot 10 .

4 is a diagram showing a movement path according to a path movement command function, (a) a joint, (b) a circular arc, (c) a straight line, and (d) a movement path for a circle.

The path movement command is written in the form of a function in a script language that programs the motion of the robot. As shown in FIG. 4 , a move command in joint space includes a MoveJoint() function, and a move command in task space includes a MoveLine() function, a MoveArc() function, and a MoveCircel() function.

This movement command is generated by setting the target position point b or c based on the current point a. The MoveJoint() command and MoveLine() command require only the target position point b. The MoveArc() command and the MoveCircel() command require three points (a, b, c) to form a circular arc, so they need b as the route location point and c as the target location point.

The target point in the joint space is configured as in Equation 1 below.

Here, joint positions (j _k ) exist as many as n of the joints of the robot.

The target point in the task space is configured as in Equation 2 below.

는 3차원 공간에서의 위치를 나타내고,

는 3차원 공간에서 z-y-x 축 순서의 오일러 각(Euler angle) 회전을 나타낸다. here,

represents a position in a three-dimensional space,

represents the Euler angle rotation of the order of the zyx axis in 3D space.

In (b) of FIG. 4, the point a indicated by the MoveLine() function is a point on the base coordinate system indicated by B. When the user sets the command factor b, the base coordinate system (B), the world coordinate system (O) and the work Specify the position of the point based on one of the coordinate systems (W). However, through coordinate transformation inside the function, all points are stored with the transformed position relative to the base coordinate system.

In addition, angles represented by roll, pitch, and yaw rotations are stored as quaternions, and angles converted based on the rotation angle base coordinate system are stored.

5 is a diagram showing a process of sequentially proceeding two route movement commands, wherein (a) is a movement path according to a route movement command function, and (b) is a diagram showing a process of processing a route movement command.

Referring to FIG. 5 , consider a process in which two route movement commands are issued sequentially.

In (a) of FIG. 5, the MoveLine() command has already been input to the queue 110 through a series of processes, and now, if the MoveArc() command is to be processed, point a is already based on the base coordinate system. , and the b and c points (points on the coordinate system selected by the user) passed as arguments of the MoveArc() command must be converted to the base coordinate system.

In (b) of FIG. 5, a process of processing dots used as arguments of a path movement command is shown. Here, the controller 120 may include a conversion unit 121-1, a path checker 121-2, and a limit checker (121-3).

의 위치와 방위에 대하여 하기의 수학식 3을 이용하여 베이스 좌표계 상의 점으로 변환된다. 즉, 점 b와 c는 베이스 좌표계가 아닌 모르는 좌표계이므로, 이를 베이스 좌표계로 변환된다. The conversion unit 121-1 is an input point

The position and direction of is converted to a point on the base coordinate system using Equation 3 below. That is, since the points b and c are not in the base coordinate system but in an unknown coordinate system, they are converted into the base coordinate system.

Here, the Quat() function is a function that converts scalar and vector values into quaternions. Since point c is the same operation as point b, the formula is omitted. In addition, the transformation matrix T and the quaternion q used for transforming the base coordinate system, the world coordinate system, and the work coordinate system are as shown in Equation 4 below.

The path checking unit 121-2 checks whether forward/inverse transformation is possible in the path connecting the points a, b, and c. Here, if the check result fails, a warning is displayed without further processing.

The limit check unit 121-3 checks whether the joint space/task space is within the limits. Here, if the check result fails, a warning is displayed without further processing.

Meanwhile, since the path movement command is given only to the target position, a continuous path is required for the robot end to move from the current position to the target position. When two points are given, the method of generating this path is called interpolation, and the interpolation method of the path movement points is divided into four types according to the movement type. That is, the interpolation method of moving points of a path may include joint interpolation, line interpolation, arc interpolation, and circle interpolation. Among them, circular interpolation and circular interpolation are described as one item because they are similar.

Joint interpolation is an interpolation method that moves along a line segment connecting two points when two points a and b are given in the joint space, as shown in (a) of FIG. 4 . For example, in the case of a vertical multi-joint robot, it moves along a straight line in joint space, but moves along a curve as shown in FIG. 4(a) in task space.

를 잇는 선분상의 한 점을 연산하는 것으로, 하기의 수학식 5와 같이 간단한 수식으로 표현할 수 있다. 여기서, 선분 상의 한 점 x는 변수 s에 의해 결정된다.These joint interpolations are two points

By calculating a point on the line segment connecting , it can be expressed by a simple formula as shown in Equation 5 below. Here, a point x on the line segment is determined by the variable s.

를 잇는 선분 상의 한 점을 연산한다. As shown in (c) of FIG. 4, line segment interpolation is an interpolation method that moves along a line segment connecting two points when two points a and b are given in the task space. This line segment interpolation method is

Calculate a point on the line segment connecting .

벡터와 쿼터니언 회전을 나타내는

벡터로 구성된다. 이때, 선분 상의 한 점 x는 변수 s에 의해 하기의 수학식 6식으로 결정된다. here. The two points a and b represent position and rotation in 3D space.

Representing vector and quaternion rotations

made up of vectors. At this time, a point x on the line segment is determined by Equation 6 below by a variable s.

Here, the Slerp() function is as shown in Equation 7 below.

Here, p ₀ and p ₁ are positions of points a and b on a circular arc due to rotation, respectively, and t is 0≤t≤1.

6 is a diagram for explaining an interpolation method of path movement points, (a) is a diagram for explaining the calculation of the rotational position of the arc, and (b) is a diagram for explaining the calculation of the rotation angle of the arc.

Circular (or circular) interpolation is an interpolation method that moves along an arc (or circle) connecting three points when three points a , b, and c are given in the task space. For such arc (or circle) interpolation, it is necessary to know the center o of the arc (or circle) and the amount of rotation R of the plane containing the arc.

를 하기의 수학식 9로 연산한다. Referring to (a) of FIG. 6, for circular interpolation, if the radii r, o , and R values of the arc are known, first, a point rotated by θ on the coordinate system based on the origin o

is calculated by Equation 9 below.

Since this point is a point on the coordinate system based on o, in order to convert this point to a point on the base coordinate system, it is multiplied by R and moved by o as shown in Equation 9 below. Interpolation for rotation is performed using the Slerp() function in the same way as for segment interpolation.

가 주어졌을 때, 원호의 중심

를 연산하는 방법은 다음과 같다.As shown in (b) of FIG. 6, three points

Given , the center of the arc

The way to compute is as follows.

A vector n perpendicular to the plane through which the three points p ₀ , p ₁ , and p ₂ pass is expressed in Equation 10 below.

And since the distance r from the three points to the center c of the circle is constant, Equation 11 below holds.

라고 하면, n과 u에 수직인 벡터

를 연산할 수 있고, 중심에서 u, v, n 벡터로 회전 행렬 R을 하기의 수학식 12로 나타낼 수 있다. The vector pointing from the center c of the circle to the point p ₀

, the vector perpendicular to n and u

can be calculated, and the rotation matrix R with vectors u , v , and n at the center can be expressed by Equation 12 below.

이 성립하고, 이 식을 풀면 하기의 수학식 13이 만들어진다.The equation of the plane formed by 3 points is

If this holds and this equation is solved, the following equation (13) is created.

이 성립하고, 수학식 13에 각각의 점을 대입하여 하기의 수학식 14를 만들 수 있다.And since the radius r is the same

When this is established, the following Equation 14 can be made by substituting each point into Equation 13.

When Equations 13 and 14 are combined and solved, the centroid c can be obtained as in Equation 15 below.

As in Equation 15, if the center c is obtained, the rotation matrix R can be obtained by substituting points into Equation 12.

In this way, if you have obtained the center c and the rotation matrix R , you need to know the rotation angles of the points p ₁ and p ₂ with respect to the point p ₀ . To do this, it is necessary to determine the direction of rotation of the circular arc along the points p _0, p _1, p ₂ . That is, it is necessary to know whether the rotation on the circular arc for each point is a clockwise rotation or a counterclockwise rotation. Assuming a coordinate system in which the center c of the arc is the origin and the x-axis is the point p ₀ , the points p ₁ and p ₂ are converted into points on this coordinate system as shown in Equation 16 below.

과

를 하기의 수학식 17과 같이 연산한다.and the angle between the two points

class

is calculated as in Equation 17 below.

As shown in Equation 18 below, the angle between the two points is within the range of 0 to 2π.

과

의 크기를 비교해 보면, 회전 방향을 알 수 있다. 만일

과

보다 크다면, 하기의 수학식 19와 같이

과

에서 2π를 뺌으로 회전 방향을 반대로 바꿀 수 있다. At this time,

class

By comparing the size of , the direction of rotation can be determined. if

class

If greater than, as shown in Equation 19 below

class

By subtracting 2π from , the direction of rotation can be reversed.

7 is a diagram for explaining the operation of a motion path blending device for a robot according to an embodiment of the present invention, (a) is a basic configuration, (b) is interpolation for one node, (c) is two nodes It is a diagram for explaining the operation when blending for .

Referring to FIG. 7 , the controller 120 may include a motion processing unit 122 , an interpolation unit 124 , and a blending interpolation unit 126 . The motion processing unit 122 may process motion for one node according to a path movement command fetched from the queue 110 . At this time, the motion processing unit 122 is activated for each node, and when processing for two nodes is required, such as blending, two modules of the motion processing unit 122-1 and the motion processing unit 122-2 may be activated. there is. In addition, the motion processing unit 122 may include a profile unit 122a and a robot kinematics unit 122b.

The profile unit 122a may generate a point on the trajectory to be followed by the end of the robot 10 from the profile information of the node. Here, the node is information composed of a path movement command, a starting point, and an end point, the profile information is information for determining movement properties such as acceleration, deceleration, and maximum speed, and the end of the robot 10 is the actual control for the robot 10. The target may be a gripper or the tip of a welding rod.

When a point on the trajectory generated by the profile unit 122a is expressed in the task space or the joint space, the robot kinematics unit 122b computes the forward kinematics or inverse kinematics of the robot 10 and maps it between the two spaces. can be computed.

In the robot motion path blending device 100 configured as described above, path movement commands are sequentially stored in the queue 110, and the control unit 120 fetches commands from the queue 110 one by one and executes them. That is, as shown in (a) of FIG. 7 , the control unit 120 repeatedly proceeds with the process of completing execution of one command, fetching the next command from the queue 110 and executing the execution. Accordingly, the control unit 120 may process the motion of the current node that is currently executing the path movement command.

In this case, the controller 120 may determine whether the current node satisfies the blending condition.

If blending is not necessary, that is, if the current node does not satisfy the blending condition, the controller 120 may complete motion processing for the current node by interpolation as shown in (b) of FIG. 7 . In this way, when the motion processing unit 122 processes a single node without blending, only the motion processing unit 122-1 may be activated. That is, only the profile part 1 (122a-2) and the robot kinematics part 1 (122b-2) can be activated.

In this case, when motion processing for one node is completed, motion processing for the next node may be performed. Therefore, since two nodes do not need to be processed simultaneously, only one motion processing unit 122-1 can be activated. Accordingly, a movement path may be generated by the interpolator 124 .

On the other hand, if blending is required, there is a section in which the next command overlaps and is performed before the processing of one command ends. In this way, since two profiles can be matched for blending, two modules of the profile part 122a and the robot kinematics part 122b must be activated.

As shown in (c) of FIG. 7 , when the motion processor 122 simultaneously processes two nodes for blending, both motion processors 122-1 and 122-2 may be activated. That is, both the profile part 1 (122a-2) and the robot kinematics part 1 (122b-2) may be activated together with the profile part 0 (122a-1) and the robot kinematics part 0 (122b-1). Accordingly, two profiles may be simultaneously generated and synthesized by the blending interpolation unit 126 .

As such, when the current node satisfies the blending condition, the controller 120 may simultaneously process motion for the next node based on the next path movement command received from the queue 110 . In this case, the controller 120 may blend a motion path for the current node and a part of the motion path for the next node to overlap each other.

Accordingly, the motion path blending apparatus 100 of the robot according to an embodiment of the present invention can quickly implement the continuous motion of the robot in the manufacturing process, thereby reducing the robot's working time.

8 is a diagram for explaining interpolation of an apparatus for blending a motion path of a robot according to an embodiment of the present invention.

Referring to FIG. 8 , the control unit 120 may further include a normalization unit 123. At this time, the control unit 120 calculates the position on the motion path by interpolation after normalizing the movement amount on the motion path for each node when processing the motion for the current node and the motion for the next node at the same time. can

That is, the profile unit 122a may calculate the movement amount d for the length d _max of a unit path generated by one command. Here, the unit path may be any one of a line segment, an arc, and a circle.

The normalization unit 123 may normalize (d/d _max ) the movement amount d calculated by the profile unit 122a.

The interpolator 124 may calculate a point x on a unit path as shown in Equation 20 below.

9 is a diagram for explaining blending conditions of a robot motion path blending apparatus according to an embodiment of the present invention, where (a) is normalization and (b) is a diagram for explaining blending length and time.

As shown in (a) of FIG. 9, a method of generating a profile and calculating an interpolation point is illustrated by taking a circular arc as an example. If a one-dimensional line segment equal to the length of the circular arc is created and the length of this line segment is denoted by d _max , the profile unit 122a calculates the position d of the moving point by accelerating and decelerating on this line segment over time. The normalization unit 123 normalizes a point moved by d on the line segment to d/d _max .

Finally, the interpolator 124 can convert the point x on the circular arc. This is a value for the factor s in each interpolation equation (Equation 5 and Equation 6) in the interpolation method of three path movement points as described above.

On the other hand, if blending is set, motion processing for the next node (n _next ) must be started before motion processing for the current node (n ₁ ) ends, and the two paths must be blended. To this end, while motion processing for the current node is being performed, motion processing for the next node should be performed if the execution condition (blending condition) is satisfied by checking the execution condition for the motion processing for the next node.

In this case, the control unit 120 may determine conditions for performing motion processing for the next node according to whether the following three conditions are satisfied.

First, motion processing for the current node should be performed at least half of the moving distance. That is, the following Equation 21 must be satisfied.

Second, there must be a next node in the queue 110. That is, the following Equation 22 must be satisfied.

Finally, the remaining execution time of the motion for the current node must be within the blending time range of the next node. That is, the following Equation 23 must be satisfied.

를 알고 있다면, 하기의 수학식 24와 같이, 이 함수의 역함수를 사용하여 blend_time을 산출할 수 있다. At this time, in order to determine Equation 23, the blending length (blend_length) must be known and the blending time (blend_time) must be calculated in advance. blend_length is known because it is a value preset for a node by the user. However, blend_time must be obtained through calculation. If the profile function to find the moving distance s at a specific time t

If is known, blend_time can be calculated using the inverse function of this function, as shown in Equation 24 below.

As such, the control unit 120 may calculate the blending time for the next node by using an inverse function of a predetermined profile function for obtaining a movement distance with respect to time.

And the RemainTime() function for node n can be calculated by Equation 25 below.

If all three conditions are satisfied, the control unit 120 may determine that the next node satisfies the blending condition. In this way, if the above three conditions are satisfied, the control unit 120 may fetch the next node from the queue 110 and allocate it to the profile unit 1 (122a-2) and the robot kinematics unit 1 (122b-2). .

10 is a diagram for explaining a blending operation of a motion path blending device for a robot according to an embodiment of the present invention.

Referring to FIG. 10 , when the blending condition for the next node is satisfied, two profile units (profile unit 0 and profile unit 1) may be simultaneously performed. Because profile unit 0 (122a-1) is already running, it will continue to run and eventually end. However, the profile unit 1 (122a-2) starts to run. Here, a movement point for each node may be calculated through the normalization units 123-1 and 123-2 and the interpolators 124-1 and 124-2.

In this case, the blending interpolator 126 may blend two points x ₀ and x ₁ generated while the two profiles are performed, as shown in (c) of FIG. 7 .

For such blending, all points must be expressed on the same coordinate system. As described above, when the path movement command is stored in the queue 110, all points on the world coordinate system or the work coordinate system in the task space are converted to the base coordinate system. However, points in the joint space are stored in the queue 110 as they are.

Therefore, if the points of x ₀ and x ₁ are expressed in different spaces, the controller 120 may convert these two points by making the task space dominant.

11 is a diagram for explaining transformation for blending of a motion path blending apparatus of a robot according to an embodiment of the present invention, (a) is a joint space for both nodes, and (b) is a joint space for only the current node. , (c) shows a case where both nodes are task spaces, and (d) shows a case where only the current node is a task space.

Referring to FIG. 11 , the controller 120 may transform motions for a current node and motions for a next node into the same coordinate system. That is, when the current node and the next node have different coordinate systems, the controller 120 may convert the joint coordinate system of the joint space between the current node and the next node into the base coordinate system of the task space. Here, the control unit 120 may include a blending unit 125 and a first conversion unit 125-1.

As shown in (a) of FIG. 11 , as described above, when both points x ₀ and x ₁ of each node are joint spaces, the first transform unit 125-1 may be omitted. In this case, the blending unit 125 may output the marked point q in the joint space.

As shown in (b) of FIG. 11, when one point x ₀ is a joint space and the other point x ₁ is a task space, the first transform unit 125-1 transforms x ₀ of the joint space into a task space. can be converted into space. In this case, the blending unit 125 may output the point p displayed in the task space.

As shown in (c) of FIG. 11, when both points x ₀ and x ₁ are task spaces, the first transform unit 125-1 may be omitted. In this case, the blending unit 125 may output the point p displayed in the task space.

As shown in (d) of FIG. 11, when one point x ₀ is a task space and the other point x ₁ is a joint space, the first transform unit 125-1 transforms x ₁ of the joint space into a task space. can be converted to In this case, the blending unit 125 may output the point p displayed in the task space.

In this case, the first conversion unit 125-1 may convert a point expressed in the joint space by the forward kinematics of the robot 10 into a point on the base coordinate system of the task space. Here, a detailed description of the robot kinematics is omitted because the transformation formula varies depending on the type of robot and undergoes a complicated calculation process.

Accordingly, the motion path blending apparatus 100 of the robot according to an embodiment of the present invention can rapidly and stably implement the continuous motion of the robot in the manufacturing process, thereby improving the reliability of the robot operation.

12 is a diagram for explaining blending position calculation of a motion path blending device for a robot according to an embodiment of the present invention.

As described above, when two points expressed in different spaces are converted into points in the same space, the blending unit 125 may blend the two points to create a single point.

Referring to FIG. 12 , it is assumed that there is a point x ₀ on a path connecting n ₋₁ to n ₀ and a point x ₁ on a path connecting n ₀ to n ₁ . Here, point x ₀ is a point on the motion path for the current node, and point x ₁ is a point on the motion path for the next node.

Here, since x ₀ and x ₁ are vectors expressed based on the base coordinate system, two points cannot be directly added. Therefore, the vector moved to x ₁ based on the starting point p ₀ of the next node must be used for calculation.

Accordingly, the blending unit 125 may calculate the blended position ( p ) by Equation 26 below.

Here, x ₀ is a vector representing the position of the current node on the motion path, x ₁ is a vector representing the position of the next node on the motion path, and p ₀ is a vector representing the position of the next node.

13 is a diagram for explaining a process of transmitting a joint driving command to a robot of a motion path blending device of a robot according to an embodiment of the present invention.

As described above, even though the blending has been completed, the points expressed in the joint space and the points expressed in the task space are still mixed. In FIG. 13 , point p denotes a point marked in the task space, and point q denotes a marked point in the joint space.

Therefore, in order to drive the robot 10, all points must be transformed into positions in the joint space. At this time, the control unit 120 may include a second conversion unit 127 and a joint drive command unit 128.

The second conversion unit 127 may convert the position of the task space into the position of the joint space after blending.

The joint drive command unit 128 may generate an angle in the converted joint space and transmit a joint drive command to the robot 10 .

Hereinafter, a motion path blending method of a robot according to an embodiment of the present invention will be described with reference to FIG. 14 .

14 is a flowchart illustrating a motion path blending method of a robot according to an embodiment of the present invention.

The robot motion path blending method 200 includes a step of checking a blending condition (S210), a blending step (S220 to S240), an interpolation step for a current node (S250), and a joint driving command step (S260).

More specifically, as shown in FIG. 14, first, the motion path blending apparatus 100 of the robot determines whether the current node satisfies the blending condition (step S210). Here, node processing is initiated prior to determining blending conditions.

Specifically, the robot motion path blending device 100 sequentially stores path movement commands for the robot 10 in a queue. At this time, the motion path blending device 100 of the robot may process the motion of the current node that is currently executing the path movement command. Here, the motion path blending apparatus 100 of the robot may calculate a position on the motion path by interpolation after normalizing a movement amount on the motion path for the node.

At this time, the blending condition may be determined according to whether the following three conditions are satisfied. Whether motion processing for the current node has been performed for more than half of the moving distance, whether there is a next node in the queue 110, and whether the remaining motion execution time for the current node is within the blending time range of the next node It can be determined that the blending condition is satisfied if all whether or not are satisfied.

Here, the blending time for the next node may be calculated as an inverse function of a predetermined profile function for obtaining a movement distance with respect to time, as described through FIG. 9 and Equations 23 to 25.

As a result of the determination in step S210, if the current node satisfies the blending condition, the robot motion path blending apparatus 100 processes the motion of the next node (step S220). That is, the motion path blending apparatus 100 of the robot may initiate motion processing for the current node and motion processing for the next node at the same time to simultaneously process the two nodes.

In this case, the motion path blending apparatus 100 of the robot may calculate a position on the motion path by interpolation after normalizing a movement amount on the motion path for the next node.

Next, the motion path blending device 100 of the robot transforms the motion for the current node and the motion for the next node into the same coordinate system (step S230). That is, when the current node and the next node have different coordinate systems, the robot motion path blending apparatus 100 may convert the joint coordinate system of the joint space among the current node and the next node to the base coordinate system of the task space. At this time, the motion path blending device 100 of the robot may transform coordinates by forward kinematics of the robot 10 .

Next, the motion path blending apparatus 100 of the robot blends parts of the motion path for the current node and the motion path for the next node so that they overlap (step S240). At this time, the motion path blending apparatus 100 of the robot may calculate the blended position p using Equation 26.

As a result of the determination in step S210, if the current node does not satisfy the blending condition, the motion path blending device 100 of the robot interpolates the current node (step S250). That is, the robot motion path blending apparatus 100 may complete motion processing for the current node through interpolation.

In this way, when the processing of the current node is completed, that is, when blending of the current node and the next node is completed as in step S240 or interpolation of the current node is completed as in step S250, the motion path blending device of the robot ( 100 transmits a joint drive command to the robot 10 (step S260). In this case, the motion path blending apparatus 100 of the robot may convert the position of the task space into a position of the joint space, generate an angle in the joint space, and transmit a joint drive command to the robot.

The above methods may be implemented by the motion path blending device 100 of the robot as shown in FIG. 1, and in particular, may be implemented by a software program that performs these steps. In this case, these programs can be implemented by a computer. It may be stored in a readable recording medium or transmitted by a computer data signal combined with a carrier wave in a transmission medium or communication network.

At this time, the computer-readable recording medium includes all types of recording devices in which data readable by a computer system is stored, and includes, for example, ROM, RAM, CD-ROM, DVD-ROM, DVD-RAM, magnetic tape, It may be a floppy disk, a hard disk, an optical data storage device, and the like.

Although one embodiment of the present invention has been described above, the spirit of the present invention is not limited to the embodiments presented herein, and those skilled in the art who understand the spirit of the present invention may add elements within the scope of the same spirit. However, it will be possible to easily suggest other embodiments by means of changes, deletions, additions, etc., but this will also be said to fall within the scope of the present invention.

100: robot motion path blending device
110: queue 120: control unit
121-1: conversion unit 121-2: path check unit
121-3: limit check unit 122: motion processing unit
122a: profile part 122b: robot kinematics part
123: normalization unit 124: interpolation unit
125: blending unit 125-1: first conversion unit
126: blending interpolation unit 127: second conversion unit
128: joint drive command unit 10: robot

Claims (18)

sequentially storing path movement commands for the robot in a queue;
processing a motion for a current node that is currently executing a path movement command;
determining whether the current node satisfies a blending condition;
simultaneously processing motion for a next node when the current node satisfies the blending condition;
transforming the motion of the current node and the motion of the next node into the same coordinate system; and
blending a motion path for the current node and a part of the motion path for the next node to overlap;
Including,
The determining step is
Motion processing for the current node is performed at least half of the moving distance;
The next node exists in the queue,
If the remaining execution time of the motion for the current node is within a blending time range of the next node, it is determined that a blending condition is satisfied;
The blending time for the next node is calculated as an inverse function to a predetermined profile function for obtaining a moving distance with respect to time. in paragraph 1
In the converting, if the current node and the next node have different coordinate systems, converting a joint coordinate system of a joint space among the current node and the next node into a base coordinate system of a task space. in paragraph 2
The transforming step is a method of blending a motion path of a robot in which coordinates are transformed by forward kinematics of the robot. delete delete According to claim 1,
The blending step is a motion path blending method of a robot that calculates a blended position ( p ) by the following equation.

Here, x ₀ is a vector representing a position on a motion path relative to the current node,
x ₁ is a vector representing a position on the motion path to the next node,
p ₀ is a vector representing the position of the next node According to claim 1,
After the blending step,
The method of blending a motion path of a robot, further comprising converting a position in the task space to a position in the joint space, generating an angle in the joint space, and transmitting a joint drive command to the robot. According to claim 1,
Each of the steps of processing the motion for the current node and the step of simultaneously processing the motion for the next node normalizes the movement amount on the motion path for each node and then calculates the position on the motion path by interpolation. path blending method. According to claim 1,
When the current node does not satisfy the blending condition, motion processing for the current node is completed by interpolation. a queue in which route movement commands for the robot are sequentially stored; and
And a control unit for sequentially performing the route movement command stored in the queue,
The control unit.
Process motion for the current node that is currently executing the path movement command,
determining whether the current node satisfies a blending condition;
When the current node satisfies the blending condition, simultaneously processing motion for a next node based on a next path movement command received from the queue;
Transforming the motion for the current node and the motion for the next node into the same coordinate system;
blending so that a part of the motion path for the current node and the motion path for the next node overlap;
The control unit,
Motion processing for the current node is performed at least half of the moving distance;
The next node exists in the queue,
If the remaining execution time of the motion for the current node is within a blending time range of the next node, it is determined that a blending condition is satisfied;
A motion path blending device for a robot that calculates a blending time for the next node by an inverse function to a predetermined profile function for obtaining a movement distance with respect to time. According to claim 10,
Wherein the control unit transforms a joint coordinate system of a joint space among the current node and the next node into a base coordinate system of a task space when the current node and the next node have different coordinate systems. According to claim 11,
The control unit is a motion path blending device of a robot that converts coordinates by the regular kinematics of the robot. delete delete According to claim 10,
The control unit calculates a blended position ( p ) by the following formula. Motion path blending device of a robot.

Here, x ₀ is a vector representing a position on a motion path relative to the current node,
x ₁ is a vector representing a position on the motion path to the next node,
p ₀ is a vector representing the position of the next node According to claim 10,
The control unit converts a position of the task space to a position of the joint space after the blending, generates an angle in the joint space, and transmits a joint driving command to the robot. According to claim 10,
The controller calculates a position on the motion path by interpolation after normalizing a movement amount on the motion path for each node when processing the motion for the current node and the motion for the next node at the same time. motion path blending device. According to claim 10,
Wherein the control unit completes motion processing for the current node by interpolation when the current node does not satisfy the blending condition.

Images