Vertical Revolute Joint Robot

1. Field of the Invention

The present invention relates to a vertical revolute joint robot having an offset wrist, and more particularly, to a robot of this kind which is capable of rapidly calculating joint angles and excellent in operation accuracy.

2. Description of the Related Art

A typical six-axis vertical revolute joint robot comprises a robot body having an arm which consists of three links and a wrist consisting of three links. In this robot, a base fixed in a robot operation space is coupled to a link disposed on the side facing the base and adjacent ones of the links are coupled to each other by means of revolute joints, respectively, so as to permit an end effector mounted to the wrist to assume its arbitrary position and orientation. In order to control the position and orientation of the end effector to perform various operations, the robot comprises a computer for calculating target values of respective joint angles of first to sixth revolute joints on the basis of target position and orientation of the end effector so as to attain the target position and orientation, and a servo system for controlling the respective joint angles to the target values (here, the numbers specifiying the links and the revolute joints increase in sequential order outward from the base-side).

In the case of a so-called in-line wrist having three axes thereof crossing each other at a single point, the respective joint angles are derived from the position and orientation of the end effector in such a manner that the joint angle of the first revolute joint (hereinafter, the joint angle of the i-th (i=1-6) revolute joint is referred to as i-th joint angle) is first calculated, which angle is represented as a function solely of the position and orientation of the end effector, and then the respective i-th joint angle, represented as a function of the joint angle(s) previously calculated, such as the first joint angle, is calculated in sequence.

On the contrary, in the case of an offset wrist having three axes thereof not crossing each other at a single point and being offset relative to the arm, the first joint angle is not given at a function solely of the position and orientation of the end effector, and hence changes in dependence even on the fourth joint angle. Thus, the first joint angle is first calculated based upon a temporal initial value of the fourth joint angle. Then, a procedure of calculating the second to fourth joint angles and the first joint angle based on the first to fourth joint angles is repeatedly carried out until the thus calculated fourth joint angle converges.

In order to move the end effector along a predetermined path, with its orientation controlled to a target orientation, the robot periodically calculates the position and orientation of the end effector. In this respect, a calculation interval (interpolation period) for calculating the position and orientation of the end effector must be shortened. However, in the robot having the offset wrist, tremendous arithmetic operations must be made for calculation of the joint angles, as mentioned above. Accordingly, a required arithmetic time period is prolonged, resulting in a prolonged interpolation period. This makes it difficult to improve operation accuracy.

The object of the present invention is to provide a vertical revolute joint robot having an offset wrist, which is capable of rapidly calculating respective joint angles on the basis of a target position and orientation of an end effector, and hence is excellent in operation accuracy.

In order to achieve the above-mentioned object, according to the present invention, there is provided a vertical revolute joint robot which comprises an arm having a first link coupled through a first revolute joint to a base fixedly disposed in an operation space, a second link coupled through a second revolute joint to the first link, and a third link coupled through a third revolute joint to the second link. The robot further comprises an offset wrist mounted to the arm and having plural revolute joints whose respective joint axes do not cross each other at a single point and arranged to be mounted with an end effector, and a control section for calculating joint angles of the respective revolute joints on the basis of a target position and orientation of the end effector. The first and second revolute joints have their joint axes which respectively extend perpendicular to an axis of the base and along an axis of the first link. The control section operates to calculate the joint angle of the first revolute joint which is given as a function solely of the target position and orientation of the end effector and the calculate the joint angle of each of the revolute joints other than the first revolute joint on the basis of previously calculated joint angles.

As mentioned above, according to the present invention, the first and second revolute joints of the vertical revolute joint robot having the offset wrist are so disposed that the joint axes of these joints respectively extend along the axial direction of the base and the first link, and the joint angle of the first revolute joint given as a function solely of the target position and orientation of the end effector is calculated, and further the other joint angles are calculated on the basis of the previously calculated joint angles. Accordingly, repetitive calculation for calculating the joint angles, heretofore inevitably required in robots of this kind, can be eliminated. This makes is possible to reduce an arithmetic quantity for calculation of the joint angles and a required arithmetic time period therefor. Thus, a calculation interval (interpolation period) for calculation of the position and orientation of the end effector can be shortened, to thereby improve the robot operation accuracy.

FIG. 1 is a principle view showing a six-axis vertical revolute joint robot having an offset wrist according to an embodiment of the present invention;

FIG. 2 is a view similar to FIG. 1 showing a conventional robot;

FIG. 3 is a view showing a relationship established between coordinate transformation matrices;

FIG. 4 is a flowchart showing a process for calculation of joint angles in the robot of FIG. 1; and

FIG. 5 is a schematic view showing the whole arrangement of the robot of FIG. 1.

Prior to an explanation of the present invention, a conventional six-axis vertical revolute joint robot having an offset wrist will be first explained.

In position and orientation control of an end effector, respective joint angles are controlled to target joint angles calculated on the basis of target values of the position and orientation of the end effector. In order to calculate the target joint angles, a coordinate transformation matrix is employed by which an operational coordinate system, set in a robot operation space for indicating the position and orientation of the end effector, is related to coordinate systems set for individual links (joints).

Referring to FIG. 2, the robot comprises first to third revolute joints (hereinafter referred to as joints) 10-12 provided in an arm, and fourth to sixth joints 13-15 provided in a wrist. A base is coupled to the link (first link) disposed at the side facing the base by the first joint 10, and adjacent ones of the first to sixth links are coupled to each other by the second to sixth joints 11-15, respectively. The sixth link is mounted with the end effector. Further, zeroth coordinate system (O0, X0, Y0, Z0), as application oriented coordinate system, and first to sixth coordinate systems (O1, X1, Y1, Z1)-(06, X6, Y6, Z6) are respectively set for the base and the first to sixth links (the first to sixth joints and the center of a surface on which the end effector is mounted). The joint axis (Z0 axis) of the first joint 10 extends in the axial direction of the base fixed in the robot operation space, and the joint axis (Y1 axis) of the second joint 11 extends perpendicularly to the axis of the first link. Reference symbols Y2, Z3-Z5 denote the joint axes of the third to sixth joints 12-15, respectively.

The following are 4.times.4 homogeneous transformation matrices Ai from the (i-1)th coordinate system to the i-th coordinate system, which matrices are utilized to calculate the position and orientation, in the application oriented coordinate system, of the end effector mounting face center on the basis of the joint angles (hereinafter referred to as first to sixth joint angles ) .theta.1-.theta.6 of the first to sixth joints 10-15, when such joint angles are given. ##EQU1## where c1 and s1 respectively represent cos.theta.1, sin.theta.2 (hereinafter, ci and si represent cos.theta.i and sin.theta.i, respectively). The first joint angle .theta.1 assumes a positive value when the first joint 10 rotates in the direction shown by the arrow in FIG. 2 (hereinbelow, the i-th joint angle .theta.1 also assumes a positive value when the i-th joint rotates in the direction specified by the arrow). ##EQU2## where d2 represents the axial length of the second link, i.e., the distance between the coordinate origins O1, O2 of the first and second coordinate systems. Hereinbelow, di also denotes the length of the i-th link (the distance between the coordinate origins of the (i-1) and i-th coordinate systems), and d4 denotes the offset distance of the wrist relative to the arm). In the meantime, the coordinate origins O0, 01 of the zeroth and first coordinate systems are set at the same position to each other, and the origins 04, 05 of the fourth and fifth coordinate systems are set at the same position (di=d5=0 (d1 and d5 are not illustrated). ##EQU3## where the components associated with an angle of .pi./2 represent that attention is given to the fact that the third coordinate system is rotated by an angle of .pi./2 around the Y2 axis of the second coordinate system (the following description is the same in this respect). ##EQU4##

In the following, transformation matrices .sup.i-1 T.sub.i from the (i-1)th coordinate system to the i-th coordinate system will be shown, which are employed for calculation of the first to sixth joint angles .theta.1- .theta.6 based on the position and orientation of the end effector mounting face center, if such position and orientation of the mounting face center are given. ##EQU5## where n(nx, ny, nz), o(ox, oy, oz) and a(ax, ay, az) represent normal vector, orientation vector and approach vector, respectively, and respective vector components are represented as follows:

nx=c1{s3(s4c5s6-c4c6)-c3s5s6}-s1(c4c5s6+s4c6)

ny=s1{s3(s4c5s6-c4c6)-c3s5s6}+c1(c4c5s6+s4c6)

nz=c3(s4c5s6-c4c6)+s3s5s6

ox=c1{s3(s4c5c6+c4s6)-c3s5s6}-s1(c4c5c6-s4c6)

oy=s1{s3(s4c5c6+c4s6)-c3s5s6}+c1(c4c5c6-s4c6)

oz=c3(s4c5c6+c4s6)+s3s5c6

ax=c1(s3s4s5+c3c5)-s1c4s5

ay=s1(s3s4s5+c3c5)+c1c4s5

az=c3s4s5-s3c5

px=c1(s2d2+c3d3+s3c4d4)+s1s4d4

py=s1(s2d2+c3d3+s3c4d4)-c1s4d4

pz=c2d2-s3d3+c3cd4d

Conventionally, the joint angles .theta.1-.theta.6 are determined by utilizing the transformation matrices .sup.i-1 T.sub.i obtained in the above-mentioned manner. First, it is noted that the following equation is fulfilled among the inverse matrix [A1].sup.-1 of the aforesaid transformation matrix A1 and the aforesaid transformation matrices .sup.0 T.sub.6, .sup.1 T.sub.6.

Left-hand side= ##EQU6##

If matrix elements in second row, fourth column on the both sides of the aforesaid equation (3) are equal to each other, the following equation is fulfilled (see, equation (2) for the right-hand side).

As apparent from equation (4), the first joint angle .theta.1 varies in dependence on the fourth joint angle .theta.. Thus, even if the aforesaid various vectors n, o, a indicative of the orientation of the end effector mounting face center and the location vector l(lx, ly, lz) indicative of the position of the end effector mounting face center are given, it is impossible to instantly calculate the first joint angle .theta.1 based on these parameters. Therefore, conventionally, the first joint angle .theta.1 represented by the following equation is determined by giving a temporal initial value of the fourth joint angle .theta.4.

where atan2(y, x) is the expanded function or arc tangent function o tan.sup.-1 y/x, which assumes values varying from -.pi. to .pi. in dependence on signs of variables x and y.

Then, the joint angle .theta.1 represented by the aforementioned equation is determined by using the relationship P=1-d6a (px=lx-axd6, py-ly-ayd6, pz=lz-azd6) established among the vector p(px, py, pz) indicative of the position of the origin of the sixth coordinate system in the zeroth coordinate system, the aforesaid vectors l, a and the length d6 of the sixth link.

Further, the following equations are fulfilled between the aforementioned inverse matrix [Ai].sup.-1 of the transformation matrix Ai and the transformation matrix .sup.i-1 T.sub.i.

Thus, temporal values of the second to fourth joint angles .theta.2-.theta.4 are calculated on the basis of the relationships established among matrix elements in appropriate rows and columns of the both sides of the above-mentioned equation and the temporally determined first joint angle .theta.1. Whereupon, the joint angle .theta.1 given by the expanded function is determined again on the basis of the calculated value of the joint angle .theta.4. Then, this procedure is repeated until the joint angle .theta.4 converges, to thereby determine the joint angle .theta.1-.theta.4. Thereafter, the fifth and sixth joint angles .theta.5 and .theta.6 are determined.

In this manner, according to the prior art vertical revolute joint robot, tremendous arithmetical operations are required for calculation of the joint angles, resulting in inconvenience. Meanwhile, in vertical revolute joint robots having an in-line wrist, equation -slpx+clpy=0 corresponding to the aforesaid equation (4) is fulfilled. Thus, the first joint angle .theta.1 is represented by a function solely of the position and orientation of the end effector, and no inconvenience in calculating the joint angles is found.

In the following, a six-axis vertical revolute joint robot having an offset wrist according to the present invention will be explained.

Referring to FIGS. 1 and 5, the robot comprises a robot body 100 arranged to be drivingly controlled by a servo system 120, which is operated under the control of a computer 110 to which a teaching pendant 111 and a manual data input device 112 are connected through an input circuit (not shown). The robot body 100 comprises a base 101 fixedly disposed in an operating space, an arm having first to third links 102-104, and an offset wrist having fourth to sixth links 105-107. Adjacent ones of the bases 101 and the links 102-107 are coupled to each other through a corresponding one of revolute joints (hereinafter referred to as joints) 1-6 and an end effector 108 is mounted on the sixth link 107. Further, servomotors (not shown) for rotatively driving the respective joints are individually connected to these joints, and detectors (not shown) for detecting the rotational positions of the respective servomotors are mounted on these motors, respectively. The servomotors and the detectors are connected to the servo system 120.

As compared with the conventional robot of FIG. 2, the robot body 100 according to the present embodiment is different therefrom in that the joint axis (Y0 axis) of the first joint 1 extends perpendicularly to the axial direction of the base 101 (extends horizontally), and in that the joint axis (Z1 axis) of the second joint 2 extends in the axial direction of the first link 102 (extends vertically). Due to these differences, the following transformation matrices A1, A2, .sup.1 T.sub.6, and .sup.0 T.sub.6, among the transformation matrices Ai and .sup.i-1 T.sub.i in the robot of this embodiment, are different from conventional ones. ##EQU7## where nx=c1[c2{s3(s4c5s6-c4c6)-c3s5s6}-s2(c4c5s6+s4c6)]+s1{c3(s4c5s6-c4c6)+s3s5s 6}

ny=s2{s3(s4c5s6-c4c6)-c3s5s6}+c2(c4c5s6+s4c6)

nz=-s1[c2{s3(s4c5s6-c4c6)-c3s5s6}-s2(c4c5c6-s4c6)]+c1{c3(s4c5s6-c4c6)+s3s5s 6}

ox=c1[c2{s3(s4c5c6+c4s6)-c3s5c6}-s2(c4c5c6-s4s6)]+s1{c3(s4c5c6+c4s6)+s3s5c6 }

oy=s2{s3(s4c5c6+c4s6)-c3s5c6}+c2(c4c5c6-s4s6)

oz=s1[c2{s3(s4c5s6+c4s6)-c3s5c6}-s2(c4c5c6-s4s6)]+c1{c3(s4c5c6+c4s6)+s3s5c6 }

ax=c1{c2(s3s4s5+c3c5)-s2c4s5}+s1(c3s4s5-s3c5)

ay=s2(s3c4s5+c3c5)+c2c4s5

az=-s1{c2(s3s4s5+c3c5)-s2c4s5}+c1(c3s4s5-s3c5)

px=c1{c2(c3d3+s3c4d4)+s2s4d4}+s1{d2-s3d3+c3c4d4)

py=s2(c3d3+s3c4d4)-c2s4d4

pz=-s1{c2(c3d3+s3c4d4)+s2s4d4}+c1(d2-s3d3+c3c4d4)

Further, the following relationship is fulfilled between the inverse matrix [A].sup.-1 of the transformation matrix A and the transformation matrix T (see, FIG. 3).

In the following, the operation of the six-axis vertical revolute joint robot constructed as above will be explained with reference to FIG. 4.

Prior to robot working, an operator operates the manual data input device 112 to input various parameters, including the link lengths, etc., employed for the below-mentioned joint angle calculation process, and causes the end effector 108 to be located at required positions in sequence, and further effects conventionally known teaching through the teaching pendant 111, with the end effector caused to assume required orientation at its respective position. As a consequence, teaching data is stored in a memory (not shown) accommodated in the computer 110, which data includes the normal vector n, orientation vector o, approach vector a and location vector 1 indicative of target position and orientation of the end effector 108 at the respective teaching position. Furthermore, a teaching program including the teaching data is prepared, if necessary.

Upon start of the robot operation, a central processing unit (hereinafter referred to as CPU), not shown, accommodated in the computer 110 reads out the various vectors n, o, a, l associated with the start point of a first block of a control program from the memory or from the teaching program (step S1). Then, at step S2, the CPU calculates the px, py and pz components of the position vector p indicative of the position of the origin of the sixth coordinate system with respect to the zeroth coordinate system in accordance with equation (7), shown hereinbelow, which has been already explained with reference to FIG. 2: ##EQU9##

Then, the CPU calculates a target value of the first joint angle .theta.1 in accordance with the following equation (8) (step S3).

Equation (8) is obtained by rearranging the following equation, which is obtained by equating the sum of the squares of the respective elements in first row, fourth column, in second row, fourth column and in third row, fourth column on the left-hand side of equation (6) with that on the right-hand side thereof (see, equation (1) for the right-hand side). Equation (8) is represented as a function solely of the position vector components px, py, pz calculated at step S2 and the link lengths d2, d3 and the offset distance d4. Hence, the first joint angle .theta.1 can be easily calculated in accordance with equation (8).

Then, target values of the second to sixth joint angles .theta.2-.theta.6 are calculated, at step S4, on the basis of the target value of the first joint angle .theta.1 determined at step S3 and the relationship established below the reverse matrix [Ai].sup.-1 and the transformation matrix .sup.i-1 T.sub.i, which relationship, shown below, has been already explained with reference to FIG. 2:

When control outputs indicative of the thus calculated target values of the joint angles .theta.1-.theta.6 are delivered from the computer 110 to the servo system 120 through an output circuit, not shown (step S5), the servo system 120 drivingly controls the servomotors associated with the respective axes of the robot body 100 so as to control the actual joint angles to these target values.

Although illustrative is omitted here, the CPU repetitively executes the aforesaid joint angle calculation process at predetermined intervals of cycle. Namely, in stead of executing step S1, the CPU performs conventionally known interpolation process on the basis of teaching data indicative of the start and end points associated with a block which is read at step S1, so as to calculate the target position and orientation of the end effector at a corresponding one of interpolation points on the moving path of the end effector, and further executes the aforesaid step S2-S5 based upon the calculated target position and orientation.