KR100784734B1 - Error compensation method for the elliptical trajectory of industrial robot - Google Patents

Abstract

An interpolation method for an elliptical trajectory of an industrial robot is provided to achieve improved accuracy of work of the robot by teaching four to six points from among points of the elliptical trajectory of the robot. An interpolation method for an elliptical trajectory of an industrial robot, includes a first step(S10) of permitting a controller to be taught by a user four to six points including a start point from the elliptical trajectory of the robot sensed by a sensing unit and derive coordinates of points on a three dimension; a second step(S20) of determining a plane by using points forming the elliptical trajectory derived in the first step; a third step(S30) of projecting points derived by the first step onto the plane determined in the second step, and deriving two-dimensional coordinates of points; a fourth step(S40) of deriving an elliptical equation by using the coordinates derived in the third step; and a fifth step(S50) of converting the elliptical equation derived in the fourth step into three-dimensional coordinates.

Description

Elliptic interpolation method for industrial robot system {ERROR COMPENSATION METHOD FOR THE ELLIPTICAL TRAJECTORY OF INDUSTRIAL ROBOT}

1 is a view showing a schematic configuration of an industrial robot system according to the present invention

2 is a flowchart of an elliptic interpolation method of the industrial robot system according to the present invention.

Figure 3 is a view showing for explaining the positioning step of the elliptic interpolation method of the industrial robot system according to the present invention

4 is a view illustrating a rotation direction determination step of the elliptic interpolation method of the industrial robot system according to the present invention.

5 is a view illustrating parameters of the rotation direction determination step of the elliptic interpolation method of the industrial robot system according to the present invention.

Figure 6 is a graph according to the function derived in the positioning step of the elliptic interpolation method of the industrial robot system according to the present invention

7 is a view illustrating a three-dimensional coordinate transformation step of the elliptic interpolation method of the industrial robot system according to the present invention

8 is a diagram illustrating the movement of the R axis in a direction perpendicular to the tangent of the ellipse.

The present invention relates to an elliptic interpolation method of an industrial robot system that performs an operation on an elliptical object, and specifically teaches only four to six points among elliptic trajectories of the robot moving along the shape of the object before the operation. By performing an elliptic interpolation method, the present invention relates to an elliptic interpolation method of an industrial robot system, which can achieve an accurate work of an industrial robot and reduce the working time of the robot.

 The motion of an industrial robot used in dispensing, joining and welding operations on an elliptical object forms an elliptical orbit to suit the shape of the object.

Conventionally, as a method of controlling the elliptic motion, after teaching many points in the elliptical orbit and connecting the taught points with an arc-shaped line to draw an ellipse, an industrial robot draws an ellipse on the elliptical orbit. It is designed to work out.

However, the above-described method has a problem in that the ellipse shape is not accurate and a lot of time is required to draw the ellipse.

To solve these problems, we use precision grid tables and microscopes, vision cameras (CCD) and image processing, and use laser sensors or laser tracking systems. There is a way to control the robot, but all require a separate expensive device, since only the way to control the circular motion or linear motion of the robot, the control method of the robot to perform the operation on the elliptical object eventually This results in the multi-point teaching method described above.

The present invention is to solve the above problems, an object of the present invention is to select the four to six points of the elliptical motion trajectory of the industrial robot even without a separate measuring device to facilitate the operation on the elliptical object;

To provide an accurate and time-saving elliptic interpolation method for industrial robot systems.

The present invention provides a robot for performing an operation on an elliptic-shaped object, a sensing unit for capturing the movement of the robot, a controller for performing an elliptic interpolation operation from data received from the sensing unit, and data received from the sensing unit. An elliptic interpolation method of an industrial robot system including a display device for deriving an image. The elliptic interpolation method of the industrial robot system includes a plurality of points including a starting point among elliptic trajectories of the robot motion captured by the sensing unit. A teaching step of deriving coordinates of the plurality of points; A plane determination step of deriving a plane equation using a plurality of points forming an ellipse derived by the teaching step; A two-dimensional transformation step of projecting a plurality of points derived by the teaching step onto a plane derived by the plane determination step to derive two-dimensional coordinates of the plurality of points; An elliptic equation derivation step of deriving an elliptic equation using the coordinates derived by the two-dimensional transformation step; And a three-dimensional coordinate conversion step of converting the elliptic equation of the two-dimensional coordinate system derived by the elliptic equation derivation step into three dimensions.

The three-dimensional coordinate transformation step is to rotate the ellipse on the two-dimensional coordinate system derived by the elliptic equation derivation step, to move in parallel in the two-dimensional coordinate system, convert the coordinate system into a three-dimensional coordinate system, and implement on the two-dimensional coordinate system It is preferable to include a three-dimensional coordinate movement step of deriving an elliptical image on the three-dimensional coordinate system by moving the ellipse in the three-dimensional coordinate system in the normal direction of the ellipse to obtain an elliptic position on the three-dimensional coordinate system.

The elliptic equation derivation step may include: a rotation direction determining step of determining a rotation direction of the ellipse by the position of the coordinates derived in the teaching step after the elliptic equation derivation step; It is preferable to further include a positioning step of calculating the coordinates of the specific position on the ellipse using the rotation direction derived after the rotation direction determination step and the moving distance from the starting point.

In addition, the teaching step may be based on a five-point teaching method in which a plurality of points to be taught are five points, and the elliptic equation applied to the elliptic equation derivation step may include an unknown b ', c', d ', e', f 'on the equation. In terms of coefficients, x2 + b'xy + c'y2 + d'x + e'y + f '= 0, and it is preferable to convert the equation into a matrix to derive a solution, which is the simplest and most accurate.

Hereinafter, an elliptic interpolation method of an industrial robot system according to the present invention will be described in detail with reference to the accompanying drawings.

First, as shown in FIG. 1, an industrial robot system to which an elliptic interpolation method of an industrial robot system according to the present invention is applied includes an industrial robot 10 for applying a predetermined work to an elliptical object A; A sensing unit 20 which receives the position signal from the industrial robot 10 and converts the position signal into three-dimensional coordinates, and gives an operation signal to the industrial robot 10 to operate the sensor 10; A control unit 30 for performing an elliptic interpolation operation from the coordinate data from the sensing unit 20; It includes a display device 40 to implement by deriving the image to the user for viewing the coordinate data received from the sensing unit 20.

Specifically, the industrial robot system according to the present invention performs an operation of drawing an ellipse when the user uses a command for drawing an ellipse corresponding to the shape of the object by using the data received from the sensing unit 20 in the control unit 30. It's about the process.

2 is a flowchart of an elliptic interpolation method of the industrial robot system according to the present invention.

As shown in Figure 2, the elliptic interpolation method of the industrial robot system according to the present invention includes a teaching step (S10), a plane determination step (S20); It includes a two-dimensional transformation step (S30), an elliptic equation derivation step (S40), and a three-dimensional coordinate transformation step (S50).

First, in the teaching step S10, when the robot 10 performs an elliptic motion corresponding to the shape of the object A, the sensing unit 20 captures an elliptical trajectory on a three-dimensional surface, and receives the data of the control unit ( In step 30), a plurality of points are taught from the user.

Next, in the planar decision step S20, a planar equation is derived using a plurality of points selected by the teaching step S10.

In the present invention, the method for determining an ellipse is largely performed in three ways. A four point teaching method using a starting point and three other points (center, long axis, and short axis), a five point teaching method using five points, and a six point teaching method using six points are used. The five- and six-point teaching methods also include a starting point, similar to the four-point teaching method.

Therefore, the ellipse is determined by using at least four points. Since the plane is determined by three points, a point deviating from the plane may occur. In this case, the plane is determined by the least-square method, and the coefficient of the constant term of the plane equation derived in the plane determination step S20 is adjusted so that the plane passes through the starting point.

In the two-dimensional transformation step S30, the plurality of points selected by the teaching step S10 are projected onto the plane derived by the plane determination step S20, and the three-dimensional coordinates are converted into two-dimensional. The purpose of performing the two-dimensional transformation step (S30) is to facilitate the calculation, because the ellipse exists on the plane in the three-dimensional space.

Specifically, the two-dimensional transformation step S30 is performed by projecting all points on the plane determined in the plane determination step S20. Since the projected points are still composed of three-dimensional coordinates, the coordinates of each point are reset by resetting the coordinate system from the starting point to the origin, the direction to the second point in the x-axis direction, and the plane normal vector direction in the z-axis direction. Get it. As a result, since all points exist on the plane derived by the plane determination step S20, the z-axis component becomes zero, and only x and y components are obtained. In addition, each axial direction vector used when converting the three-dimensional coordinate system into a two-dimensional coordinate system is used when performing the three-dimensional coordinate conversion step (S40) described later.

In the elliptic equation derivation step (S40), an ellipse is calculated using one of three elliptic determination methods.

First, the four-point teaching method is performed by teaching the coordinates of the center, short axis, and long axis of the ellipse as well as the starting point. The four-point teaching method can easily obtain an elliptic equation, but there are many difficulties in teaching.

In the case of the six-point method, if the points are exactly on the ellipse, the solution of the ellipse is not obtained. That is, it is a method having the property of approximation. Therefore, if the starting point is not on the ellipse, it promotes the size of the ellipse. Since the six-point method has the same problems as described above, the five-point method is preferably used.

Next, the rotation direction determination step (S41) and the positioning step (S42) performed to derive the coordinates of a specific position on the elliptic orbit formed by the elliptic equation derived in the elliptic equation derivation step (S40). Let's look at it.

The rotation direction determining step S41 and the positioning step S42 are performed to provide a more accurate elliptic interpolation method by determining coordinates for a specific position on an elliptical orbit.

Specifically, as shown in FIG. 3, in the positioning step S42, when a moving distance u from the starting point P0 is given, determining a coordinate of a point separated by the moving distance u. The rotation direction determining step (S41) is a step for setting in which direction the movement distance u is to be moved before performing the positioning step S42.

In the rotation direction determination step S41, the rotation direction of the ellipse is determined in accordance with the position of the taught coordinates. Although the two ellipses shown in FIGS. 4A and 4B have the same shape, the rotation direction of the ellipse is different depending on how to teach a point on the ellipse. Here, in the internal algorithm, the rotation direction of the ellipse can be determined using only P0, P1, and P2, and the rotation direction of the ellipse is determined using the sign of the z-axis component of the external products P1-P0 and P2-P1.

In the positioning step S42, as shown in Fig. 3, when the moving distance u is given from the starting point P0, the position of the black point whose length of the arc on the ellipse from the starting point to the black point becomes u is calculated. Since the ellipse is symmetrical about the x- and y-axes, we can implement a function that connects u, which represents the distance and θ, which represents the position of the black point with respect to the first quadrant.

In the three-dimensional coordinate transformation step (S50), the two-dimensional ellipse position is converted into a position on the original three-dimensional plane.

This is to rotate the ellipse on the two-dimensional coordinate system derived by the elliptic equation derivation step (S40), to move in parallel in the two-dimensional coordinate system, to convert the coordinate system into a three-dimensional coordinate system, and to convert the ellipse implemented on the two-dimensional coordinate system It is performed by deriving an elliptical image on the three-dimensional coordinate system by moving in the normal direction of the ellipse in the three-dimensional coordinate system.

Hereinafter, a preferred embodiment of the present invention having the above configuration will be described in more detail.

Teaching step (S10) and plane determination step (S20)

After teaching 5 or 6 points on an ellipse (S10), a plane equation including the plurality of points is derived (S20). Adjust the z-axis intercept d so that the plane equation derived in this way crosses the starting point.

The planar equation calculates coefficients a, b, and d by linear regression using z = ax + by + d. In the case of teaching an ellipse using a five-point or six-point teaching method, the ellipse may be expressed as follows.

The form above can be represented as Ax = b,

The solution is solved by using, and the coefficient d of the constant term is adjusted so that the plane obtained above passes the starting point x ₀ , y ₀ , and z ₀ .

Two-dimensional transformation step (S30) and elliptic equation derivation step (S40)

Projecting the remaining points except the starting point to the plane to make a two-dimensional coordinate system (S30).

,

는 평면상에 존재하므로, 평면 방정식에 대입하여 정리하면, If the plane equation is ax + by + cz + d = 0 and the coordinates of the points projected on the plane are (Px, Py, Pz), then the normal vector

,

Is on the plane, so substituting it in the plane equation,

이므로,

이고,

Because of,

ego,

이다.The coordinates of the points projected on the plane

to be.

로 두었을 때,

을 평면상에서의 x방향으로 설정한다. 평면의 normal벡터를 z방향으로 설정하면, y방향은

와 같이 외적을 사용하여 계산할 수 있다. 다음으로, 평면상에서 Po를 원점으로 설정한 후에, 나머지 점들의 위치를 평면상에서의 x, y위치로 변환한다.Now, because all the points are on the plane, we convert them to a plane coordinate system.

When put in

Is set in the x direction on the plane. If the normal vector of the plane is set in the z direction, the y direction is

It can be calculated using the cross product as Next, after setting Po as the origin on the plane, the positions of the remaining points are converted into x and y positions on the plane.

When the two-dimensional transformation step (S30) is finished, the ellipse is calculated using one of the following three elliptic determination methods (S40).

The equation applied in the four-point teaching method is as follows.

The 5 or 6 point teaching method is an ellipse with 5 or 6 point coordinate data including the starting point. The equation applied here is as follows.

, 양변을 a로 나누면,

, Dividing both sides by a,

Here, a to f, b 'to f' are unknown, and the former equation is applied to the six-point teaching method because six unknowns, and the latter equation is applied to the five-point teaching method of five unknowns.

The matrix following the 5-point teaching method is as follows.

Further, the condition for the above solution to be an ellipse is the discriminant b ² -4ac <0.

If the condition is not satisfied, an error occurs and a new operation is performed again and again, and finally, the coefficient of the equation can be obtained.

Rotation direction determination step (S41) and positioning step (S42)

Using an equation derived in the elliptic equation derivation step (S40), a function for obtaining an angle when an arbitrary moving distance from a starting point on an ellipse is given is derived.

For example, when θ is defined as shown in FIG. 5, the present invention may utilize an elliptic integral equation as follows. In this case, the following negative integral integrates it through a numerical integration method.

a is the length of the major axis of the ellipse, b is the length of the minor axis,

이다.

to be.

In addition to the elliptic integration method, a Gaussian quadrature method may be used to obtain an accurate integration value with fewer calculations.

The graph according to the function derived by the calculation is shown in FIG.

3D coordinate transformation step (S50)

The position in the 3D is calculated by applying the calculated ellipse position in the order of reverse rotation, translation, 3D rotation transformation, and 3D parallel movement. 7A to 7E sequentially illustrate the above process.

The graph shown in FIG. 7A is an elliptic graph configured by calculating a specific position on an ellipse using the moving distance of the ellipse and the function derived in the positioning step (S42). The elliptic graph of FIG. 7A is rotated as shown in FIG. 7B, and parallelly moved as shown in FIG. 7C, and then the 2D coordinate system is converted into a 3D coordinate system as shown in FIG. 7D, and the ellipse is shown as shown in FIG. 7E. The coordinates of the three-dimensional position on the ellipse are calculated by parallel translation in the three-dimensional coordinate system.

   In addition, when supporting operations such as sealing, the R axis may be required to move perpendicular to the elliptical orbit.

8A shows the movement of the R axis in a direction perpendicular to the tangent of the ellipse.

In order to move as perpendicularly as possible to the elliptic trajectory, the tool axis with the R-axis property is rotated 360 degrees regardless of the teaching value of the R-axis. The R-axis value is not used, but the X, Y, and Z values (small circle) are used to calculate the elliptic trajectory (dotted line), so the robot must be taught to be perpendicular to the R axis.

In addition, FIG. 8B illustrates a case in which the Tool axis does not have an R-axis property, and the coordinate value of the taught Tool axis is used for this.

When teaching as shown in the two points shown in FIG. 8B, the non-R-axis interpolates and moves the teaching two coordinate values.

An example of calculating the angle of the R axis to be perpendicular to the tangent of the ellipse is as follows.

로 정의되는 속도벡터에서,

In the velocity vector defined by

A vector perpendicular to the velocity vector can be found as follows.

Here, the angle of the R axis is

However, the R-axis at the starting point may be taught differently from the normal direction, so it is offset and added while moving. Therefore, the offset should be taught as close to zero as possible to maintain the normal direction when moving the entire elliptic trajectory.

Here, θ _RO is the R-axis coordinate of the starting point.

함수는 -pi에서 pi까지의 값을 변환하기 때문에 -pi에서 시계방향으로 더 회전할 경우에 pi가 나오므로 각도 값이 불연속이 된다. 이를 해결하기 위해서 이동거리(u)를 통해 얻은 각도값인 θ로 N값을 얻은 후에, atan2함수를 사용한다. 여기서, 각도는 θ'=2*pi*N+θ 로 나타낸다.(-pi<θ<pi)

Since the function converts the value from -pi to pi, the angle value is discontinuous because pi appears when rotated further clockwise from -pi. To solve this problem, use the atan2 function after obtaining the N value with the angle value θ obtained from the moving distance u. Here, the angle is represented by θ '= 2 * pi * N + θ. (-Pi <θ <pi)

The scope of the present invention is not limited to the above embodiments, and may be embodied in various forms of embodiments within the scope of the appended claims. It will be understood that those skilled in the art to which the invention pertains may fall within the scope of the claims without departing from the gist of the invention as claimed in the claims.

As described above, according to the elliptic interpolation method of the industrial robot system according to the present invention, the operation on the elliptical object can be easily performed by selecting only 4 to 6 points of the elliptical motion trajectory of the industrial robot without a separate measuring equipment. To ensure that;

It has the advantage of providing elliptic interpolation method of industrial robot system which is accurate and saves time.

Claims (4)

A robot performing an operation on an elliptic-shaped object, a sensing unit for capturing the movement of the robot, a controller for performing an elliptic interpolation operation from data received from the sensing unit, and data received from the sensing unit In the elliptic interpolation method of an industrial robot system comprising a display device, A teaching step of deriving coordinates of the plurality of points in three dimensions by teaching four to six points including a starting point among elliptic trajectories of the robot motion captured by the sensing unit in the control unit; A plane determination step of deriving a plane equation using a plurality of points forming an ellipse derived by the teaching step; A two-dimensional transformation step of projecting a plurality of points derived by the teaching step onto a plane derived by the plane determination step to derive two-dimensional coordinates of the plurality of points; An elliptic equation derivation step of deriving an elliptic equation using the coordinates derived by the two-dimensional transformation step; And a three-dimensional coordinate transformation step of converting the elliptic equation of the two-dimensional coordinate system derived by the elliptic equation derivation step into three-dimensional. The method of claim 1, The three-dimensional coordinate transformation step is to place the ellipse on the two-dimensional coordinate system derived by the elliptic equation derivation step on the original three-dimensional plane including the plurality of points taught in the teaching step. After rotating about the origin, and in parallel to at least one of the x-axis or y-axis of the two-dimensional coordinate system, the coordinate system is converted into a three-dimensional coordinate system, and the ellipse implemented on the two-dimensional coordinate system in the three-dimensional coordinate system Elliptic interpolation method of the industrial robot system comprising a three-dimensional coordinate movement step of deriving the elliptical image on the three-dimensional coordinate system by moving in the normal direction of the ellipse. The method of claim 2, The elliptic equation derivation step may include: a rotation direction determining step of determining a rotation direction of the ellipse by the position of the coordinates derived in the teaching step after the elliptic equation derivation step; And an positioning step of calculating coordinates of a specific position on an ellipse using the rotation direction derived after the rotation direction determination step and the moving distance from the starting point. The method according to any one of claims 1 to 3, The teaching step is a five point teaching method in which a plurality of points to be taught are five points, and the elliptic equation applied to the elliptic equation derivation step is If the unknowns b ', c', d ', e', f 'are coefficients in the equation, and x ² + b'xy + c'y ² + d'x + e'y + f '= 0, and convert the equation into a matrix to derive a solution.

Images