Method for Automatically Setting a Tool Tip Point

1. Field of the Invention

The present invention relates to a method for automatically setting the point of operation of an end effector, i.e., a tool tip point (TCP), in an industrial robot.

2. Description of the Related Art

In mounting a tool on a tool mounting surface of a wrist portion, i.e., a faceplate, of a robot, the coordinate position of a tool tip point, in a faceplate coordinate system having its origin on the center point of the faceplate, must be set. When using a tool whose tip point position is unknown, therefore, such setting is conventionally effected by determining the coordinate position of the tool tip point on the basis of design drawings of the tool, and manually inputting numerical values indicative of the position into a control device of the robot.

However, the design drawings of the tool are often unavailable. In such a case, the coordinate position of the tool is conventionally set in the aforesaid manner after determining the position from actual measurements of various parts of the tool obtained by means of a measuring instrument. Actually measuring the tool, however, is troublesome and subject to substantial errors.

Accordingly, the object of the present invention is to provide a method for automatically setting a tool tip point, in which the position of the tool tip point in a faceplate coordinate system of a robot can be set easily and accurately, without requiring design drawings or a tool measurement by means of a measuring instrument.

In order to achieve the above object, a method according to the present invention comprises steps of: (a) causing a robot to assume at least four postures in a state such that the tip point of a tool is located on one point within a space in which the robot is installed, and instructing the robot of coordinate positions of the center point of a faceplate of the robot, in a base coordinate system set in the space, when the robot assumes the individual postures; (b) calculating the position of the center of curvature of a spherical surface passing through or near the individual coordinate positions thus given for instruction, on the basis of the coordinate positions; and (c) determining and setting the coordinate position of the tool tip point, in a faceplate coordinate system set for the faceplate, on the basis of posture vectors of the tool, the position of the center of curvature of the spherical surface thus calculated, and at least one coordinate position of the faceplate center point given for instruction in the step (a).

According to the present invention, as described above, the tool tip point is automatically set by only giving four or more points for instruction while changing the posture of the robot, in a state such that the tool tip point is located on the certain point within the robot installation space, so that the tool tip point can be set easily and with high accuracy, without requiring a manual input operation based on design drawings of the tool or actual measurements of the tool obtained by means of a measuring instrument.

FIG. 1 is a diagram of a tool on a robot arm for illustrating the principle of the present invention;

FIG. 2 is a diagram of a robot arm for illustrating the relationship between a base coordinate system and a faceplate coordinate system;

FIG. 3 is a block diagram of a robot to which is applied a method for automatically setting a tool tip point according to one embodiment of the present invention, and the principal part of a control circuit for controlling the robot;

FIG. 4 is an operational flow chart of an embodiment of the invention;

FIG. 5 is a diagram illustrating a method for approximating a sphere in accordance with various instruction points; and

FIG. 6 is a diagram illustrating a method for obtaining the vector of the center point of the sphere in the faceplate coordinate system at each instruction point.

Referring first to FIGS. 1 and 2, the principle of the present invention will be described. In FIG. 1, reference numerals 1, 2 and 3 denote an arm of a robot, a tool attached to a faceplate of the arm 1, and the center point of the faceplate, respectively.

In order to determine the coordinate position of the tip point of tool 2, in a faceplate coordinate system centered on the faceplate 3, the robot is caused to assume four postures in succession, with the tool tip point located on one point 4 in a robot installation space. When the robot assumes each posture, the coordinate position of the faceplate center point 3 in each posture is given in a base coordinate system for the aforesaid space.

Here let it be supposed that a coordinate position P1 of the faceplate center point 3 at the time of a first instruction is given by (x1, y1, z1), a coordinate position P2 of the faceplate center point 3 at the time of a second instruction is given by (x2, y2, z2), and likewise, third and fourth instruction points P3 and P4 are given by (x3, y3, z3) and (x4, y4, z4), respectively. Since the tool tip point of the tool 2 is always situated on the one point 4, these instruction points P1 to P4 are on a spherical surface (hereinafter referred to as a sphere) having its center on the one point 4. Thus, if the coordinate position of the point 4 in the base coordinate system and the radius of the sphere (or tool length) are (a, b, c) and r, respectively, the following equations hold for the aforesaid instruction points P1 to P4.

In the equations (1) to (4), x1 to x4, y1 to y4, and z1 to z4 are known values obtained at the time of the individual instructions, and the unknowns are a, b, c and r. Thus, the center (a, b, c) of curvature of the sphere (hereinafter referred to as the center point of the sphere) and the radius r of the sphere can be obtained by solving the four simultaneous equations (1) to (4).

With reference to FIG. 3, let us suppose, that vectors directed from the origin 0 of the base coordinate system 6 to the point 4 and from the center point 3 of the faceplate to the point 4 are designated by numerals 7 8, respectively, numerals 10, 11 and 12 denote tool posture vectors, the components of the vector in the base coordinate system 6 8 and 7 are given by (X.sub.H, Y.sub.H, Z.sub.H) and (X.sub.B, Y.sub.B, Z.sub.B), respectively, the components of the posture vectors 10, 11 and 12 in the base coordinate system 6 are given by (1.sub.X, 1.sub.Y, 1.sub.Z), (m.sub.X, m.sub.Y, m.sub.Z), and (n.sub.X, n.sub.Y, n.sub.Z), respectively, and the center point 3 of the faceplate is given by (O.sub.X, O.sub.Y, O.sub.Z). Thereupon, the following equation holds. ##EQU1##

From the equation (5), we obtain ##EQU2##

Since (X.sub.B, Y.sub.B, Z.sub.B) of the vector 7 is the value (a, b, c) obtained by solving the simultaneous equations (1) to (4), the above equation can be rearranged as follows: ##EQU3##

In equation (6), the posture vectors 10 (1.sub.X, 1.sub.Y, 1.sub.Z), 11 (m.sub.X, m.sub.Y, m.sub.Z), and 12 (n.sub.X, n.sub.Y, n.sub.Z) are values obtained from the values of various joint angles of the robot detectable by a conventional method, and the center point (O.sub.X, O.sub.Y, O.sub.Z) of the faceplate is an instruction point. Thus, by substituting these values into the equation (6), the components (X.sub.H, Y.sub.H, Z.sub.H) of the vector 8, that is, the coordinate position (x.sub.H, y.sub.H, z.sub.H) of the tool tip point in the faceplate coordinate system, is obtained, and this position may be set as the tool tip point.

Referring now to FIGS. 3 to 6, a method for automatically setting a tool tip point according to one embodiment of the present invention will be described. FIG. 3 is a block diagram of a robot to which the method of the invention is applied and peripheral elements. The robot is provided with a control section 51 which includes a central processing unit (hereinafter referred to as a CPU). The control section 51 is connected with a memory 52 formed as a ROM, a memory 53 formed as a RAM, an instruction operation panel 54, an operation panel 55 with CRT display, and a tape reader 56.

The memory 52 is stored with various control programs to be executed by means of the control section 51. The memory 53 is stored with instruction data, inputted through the instruction operation panel 54, the operation panel 55, the tape reader 56, etc., data inputted through the operation panel 55, and results and data of calculations executed by the control section 51. The instruction operation panel 54 has a numerical indicator, lamps, and operation buttons, which are necessary for the operation of the robot. The data input device (hereinafter referred to as a CRT/MDI) 55 with CRT display includes various keys, such as ten-key numeric keypad and function keys, as well as a display unit having a CRT. An operator can input various data through the CRT/MDI 55. Numeral 60 denotes an axis controller including an interpolator and adapted for axis control of a plurality of axes, numeral 59 denotes a servo circuit for controlling a drive source of the robot 58, and numeral 64 denotes an input/output circuit, which performs input/output operations for signals delivered to and from an object 61 of operation, through the medium of a relay unit 63. Numeral 57 denotes an input/output circuit through which various detection signals from the robot 58 are applied to the input of the control section 51. Numeral 62 denotes an interface for interfacial connection with the object 61 of operation, and numeral 66 denotes a bus line. Since these devices can be constructed by a conventional method, a detailed description thereof is omitted.

Referring now to FIG. 4, the method according to the present invention will be described. If the CRT/MDI 55 is first operated to establish an automatic TCP setting mode (automatic tool tip point setting mode), the CPU in the control section 51 sets an index K to "0" (Step S1), and then determines whether input is received due to the operation of an instruction end key or an instruction key of the instruction operation panel 54 (Steps S2 and S3). If the operator depresses the instruction key after locating the tool tip at point 4 (see FIG. 1), the CPU causes the memory 53 to store the instruction position Pi (=P1), i.e., the coordinate position Pi (xi, yi, zi) of the center point of the faceplate, and the current tool posture vectors (1.sub.X, 1.sub.Y, 1.sub.Z), (m.sub.X, m.sub.Y, m.sub.Z), and (n.sub.X, n.sub.Y, n.sub.Z) (Step S4). The subscript i indicates the order of instruction, and is given by i=K+1. After the index K is incremented by "1" (Step S5), the program returns to Step S2, and waits for the input of the next instruction command (Step S3). Thereafter, each time the operator inputs an instruction command after changing the posture of the robot 58 while the tool tip point remains located on the predetermined point 4, the CPU causes the memory to store the instruction position Pi (xi, yi, zi). These steps S2-S5 are repeated to instruct the robot of at least four points.

Theoretically, the tool tip point can be satisfactorily set by instructing the robot of the coordinate positions of the faceplate center point for the four postures, as mentioned before with reference to FIG. 1. In the present embodiment, however, four or more points are given for instruction so that the sphere is obtained in an approximate manner on the basis of these four or more instruction points, as mentioned later, assuming a case that the tool tip point cannot be accurately located on the predetermined point 4.

When the operator has given instruction for four or more points and then inputs an instruction end command, the program of FIG. 4 proceeds to Step S6, and the CPU approximates the sphere 5 as shown in FIG. 5, on the basis of the individual positions P1 (x1, y1, z1) to Pk (xk, yk, zk) given for instruction, and then obtains the center point (a, b, c) of the sphere 5. The index K, which is incremented by "1" in Step S5 with every instruction, indicates the number of instruction points.

In the present embodiment, the sphere 5 is approximated by means of K instruction points, P1 to Pk, and the method of least squares is used to obtain the center point (a, b, c) and the radius r of the sphere (Step S6).

More specifically, an equation representative of the sphere having the radius r and its center on the position (a, b, c) is given by

If an error at each instruction position Pi (xi, yi, zi) (here i=1, 2, 3 . . . k) and the square sum of the error are .epsilon.i and E, respectively, the following relations hold. ##EQU4##

If .delta.E/.delta.r.fwdarw.0 is given, in order to obtain the value of r corresponding to the minimum value of the square sum E of the error, the following equation holds. ##EQU5##

In order to obtain the point (a, b, c) corresponding to the minimum value of the square sum of the error, moreover, a condition such that the partial differential of the sum E for each of the variables a, b and c is infinitesimal is calculated. First, the partial differential of the sum E for the variable a is obtained as follows: ##EQU6##

Here if .delta.E/.delta.r.fwdarw.0 is given, we obtain ##EQU7## (where j=1, 2 . . . k).

Removing the parentheses of the above equation, we obtain ##EQU8##

Rearranging the above equation, we obtain ##EQU9##

If the coefficients of the variables a, b and c of the equation (9) are A1, A2 and A3, respectively, and if the constant term is A4, we obtain

Here the coefficients A1 to A3 and the constant term A4 are given by ##EQU10##

Likewise, the square sum E of the error is subjected to a partial differentiation for the variable b (.delta.E/.delta.b). If .delta.E/.delta.b=0 is given, the equation and coefficients B1, B2 and B3 of the variables a, b and c and a constant term B4 are given as follows: ##EQU11##

The square sum E of the error is subjected to a partial differentiation for the variable c. If .delta.E/.delta.c=0 is given, the equation and coefficients C1, C2 and C3 of the variables a, b and c and a constant term C4 are given as follows: ##EQU12##

Here .SIGMA. represents ##EQU13## For example, .SIGMA.xi and .SIGMA.xj represent ##EQU14## respectively.

Since the values xi, yi and zi are obtained and stored in response to the instruction of Step S3, the coefficients and constant terms A1 to A4, B1 to B4, and C1 to C4 are obtained from these stored values. In consequence, the unknowns a, b, c can be obtained from the tenary linear simultaneous equations (10), (11) and (12) (Step S6).

Subsequently, the equation (8) is solved to obtain the radius r of the sphere 5 in accordance with the center point (a, b, c) obtained in Step S6 and the individual stored positions (xi, yi, zi) (Step S7).

In other words, ##EQU15## is solved to obtain the radius r.

Based on the center point (a, b, c) of the sphere and the radius r of the sphere thus obtained, the vector (X.sub.H, Y.sub.H, Z.sub.H), that is, the coordinate position (X.sub.H, Y.sub.H, Z.sub.H) of the tool tip point (center point of the sphere), in the faceplate coordinate system with its origin on the center point of the faceplate, is obtained. More specifically, the vector (X.sub.Hi, Y.sub.Hi, Z.sub.Hi) is obtained for each instruction position Pi in the processes of the following Steps S8 to S16, and the obtained values are averaged to obtain the aforesaid vector (X.sub.H, Y.sub.H, Z.sub.H).

First, the index i is set to "1" in Step S8, and a transformation matrix Gi at the instruction position Pi (=P1) is made on the basis of the posture vectors (1.sub.X, 1.sub.Y, 1.sub.Z), (m.sub.X, m.sub.Y, m.sub.Z), and (n.sub.X, n.sub.Y, n.sub.Z) at the instruction position Pi and the center point (o.sub.X, o.sub.Y, o.sub.Z) of the faceplate. Since the center point of the faceplate is at the instruction position, (o.sub.X, o.sub.Y, o.sub.Z)=(xi, yi, zi) holds. Since the posture vector depends on the value of the joint angle(s) of the robot, moreover, the transformation matrix Gi is obtained as follows: ##EQU16##

Then, the calculation of the equation (6) is effected on the basis of the transformation matrix Gi thus obtained and the center point (a, b, c), and a vector (Xoi, Yoi, Zoi) directed from the center point 3 of the faceplate at the instruction position Pi to the center point (a, b, c) is obtained (Step S10). Based on the vector (Xoi, Yoi, Zoi) thus obtained, a radius roi is obtained according to the following equation (Step S11). ##EQU17##

Thus, the vector (Xoi, Yoi, Zoi) and the size (radius) roi are obtained in steps S10 and S11 as shown in FIG. 6. However, the vector (X.sub.Hi, Y.sub.Hi, Z.sub.Hi) to be obtained is a vector (whose size is equal to the radius r) directed from a point 3' on the approximated sphere 5 to the point 4.

Hereupon, the following equation (13) holds between the vector (Xoi, Yoi, Zoi) and the vector (X.sub.Hi, Y.sub.Hi, Z.sub.Hi).

From the equation (13), we obtain

The equations (14) to (16) are calculated in accordance with (Xoi, Yoi, Zoi) obtained in Step S10 and roi obtained in Step S11, to obtain the vector (X.sub.Hi, Y.sub.Hi, Z.sub.Hi) (Step S12). Then, the vector values X.sub.H, Y.sub.H and Z.sub.H obtained in Step S12 are added to the values of registers R(X.sub.H), R(Y.sub.H) and R(Z.sub.H), respectively, and stored (Step S13). The registers R(X.sub.H) to R(Z.sub.H) are previously initialized and reset to "0" when the automatic tool tip point setting mode is established.

Subsequently, whether the index i is equal to the value of the index K, which is indicative of a predetermined frequency of instruction, is determined (Step S14). If the two values are not equal, the index i is incremented by "1" (Step S15), and the processes of Step S9 and the subsequent steps are repeated. As a result, a vector directed from the faceplate center point 3 at each instruction position to the center point (a, b, c) of the sphere 5, that is, the coordinate position of the tool tip point in the faceplate coordinate system, is added to each of the registers R(X.sub.H), R(Y.sub.H) and R(Z.sub.H). When the index i attains the instruction frequency K, the respective values of the registers R(X.sub.H), R(Y.sub.H) and R(Z.sub.H) are divided by the instruction frequency K. Thus, the aforesaid averaged vector (X.sub.H, Y.sub.H, Z.sub.H), that is, the coordinate position of the tool tip point in the faceplate coordinate system, is obtained (Step S16).

If the value (X.sub.H, Y.sub.H, Z.sub.H) is stored as the tool tip coordinate position in the memory 53, then the tool tip point is automatically set.

In the embodiment described above, the vectors (X.sub.Hi, Y.sub.Hi, Z.sub.Hi) at the individual instruction points are obtained, and the vector (X.sub.H, Y.sub.H, Z.sub.H) is determined by the average of these values. If the required accuracy is not very high, however, the vector, that is, the tool tip point in the coordinate system with its origin on the center point of the faceplate, may be obtained by effecting the calculation of the equation (6) on the basis of the center point (a, b, c) of the sphere obtained in Step S6 and the coordinate position of any one of the instruction points.