Abstract:
In a robot, an object to be controlled, e.g., a working tool mounted on a robot wrist is purposively moved in accordance with data taught in advance. In case the object is encountered with a shaping or mounting error, a correction is accordingly required. This requirement makes it necessary to generate a desired error vector so that the robot may be moved while correcting the taught data in accordance with that vector. The object can be moved in a manual mode with its location being fixed while changing its position. In case, however, the object is accompanied by the shaping error or the like, not only the position but the location is changed following the position changing operation. The error vector can be arithmetically determined from data concerning such locational change.

Description:
BACKGROUND OF THE INVENTION 
     1. Field of the Invention 
     The present invention relates to a robot control system which is made capable of utilizing data for a robot, which are taught in advance, even if an object to be controlled, such as a working tool which is fixed on a robot wrist, is encountered with shaping or mounting errors. 
     2. Description of the Prior Art 
     A robot controls the location and position of a controlled object, which is fixed on the wrist thereof, such as a welding torch in accordance with data taught in advance. This makes it impossible to use the original taught data if the controlled object has an error in shape or size or if the controlled object is improperly attached to the robot wrist. This makes it necessary to make an adjustment for eliminating the shaping, sizing or mounting error(s) of the controlled object. This adjustment takes a long time and is so physically acted upon the object that a special jig or gauge is required. It is therefore desired to eliminate the physical adjustment and, if possible, the use of such special jig or gauge. 
     SUMMARY OF THE INVENTION 
     The present invention is equipped with means for computing, storing and utilizing an error vector indicating an error in a wrist coordinate system restrained by a robot wrist, i.e., the locational change of an object with respect to the robot wrist so that a robot may be controlled by using original taught data without any physical adjustment of the object. 
     It is, therefore, an object of the present invention to eliminate such a physical adjustment of the object using a gauge as accompanies the locational change of the object. 
     Another object of the present invention is to make it possible to effectively control the robot by using the original taught data while omitting the physical adjustment of the object. 
     Still another object of the present invention is to arithmetically determine an error vector which is required for utilizing the original taught data for the robot control. 
     A further object of the present invention is to make it possible to compute the error vector without using any gauge or the like by noting the locational change of the object, which is caused following the change in the position of the controlled object when the robot is controlled to change the position only. 
     The present invention is premised on a robot control system for controlling the location and position of the controlled object fixed on the robot wrist in accordance with the data taught in advance. These taught data contain at least the locational and positional data but their teaching method may be arbitrary. The location of the controlled object described herein is that which is left unchanged irrespective of any positional change, and the control point is preset as a specific point having a relation to a prescribed object. The so-called &#34;articulated robot&#34; may sometimes adopt the taught data in the robot coordinate system based on the drive quantity (e.g., the angle of rotation of each articulation) of a robot drive unit. In this case, the locational and positional data cannot be formally discriminated, which is still sufficient for the present invention. 
     The present invention is equipped with: means for computing a error vector indicating an error in a wrist coordinate system restrained by the robot wrist, i.e., the locational change of the object with respect to the robot wrist; and means for storing the computed error vector. The wrist coordinate system restrained by the robot wrist has its coordinate axis moving in association with the robot wrist or the object fixed on the former. As a result, the error vector is left unchanged even, if the controlled object moves with the robot wrist, but is changed to correspond to the locational change of the object with respect to the robot wrist. For example, a welding torch is imagined as an controlled object and has its leading end assumed as the control point. If the welding torch is hit, its shape is changed so that the location of the control point at its leading end is also changed. This change is an error vector which provides three-dimensional data having directions and lengths. This error vector is also generated even in case the old object on the robot wrist is replaced by new one. 
     The present invention is equipped with command value computing means for computing a command value for the robot drive unit, from which the locational change concerning the object, i.e., the error, is eliminated in accordance with the taught data of the taught data memory means and the error vector of the error vector memory means. This command value is exemplified in an articulated robot by that of an angle of rotation in each articulation (i.e., the robot drive unit). The taught data are a series of operation programs containing the locational and positional data at each step and will change in accordance with the advance of the steps, but the common error data can be used for computing the command value so long as no error is newly caused. 
    
    
     BRIEF DESCRIPTION OF THE DRAWINGS 
     FIG. 1 is a flow chart for explaining a control system according to the present invention; 
     FIG. 2 is a schematic diagram showing the mechanism of a robot body; 
     FIG. 3 is a detailed view showing the wrist portion of the same; 
     FIG. 4 is a coordinate diagram showing an error vector; 
     FIG. 5 is an explanatory diagram showing the relationship between the robot body and the coordinate system; and 
     FIG. 6 is an explanatory diagram showing the processes of coordinate transformations. 
    
    
     DESCRIPTION OF THE PREFERRED EMBODIMENT 
     An embodiment shown in FIG. 1 will be described in the following with additional reference to other drawings. A block 60 of FIG. 1 is the drive unit of a robot body and constitutes a closed loop servo control circuit together with a deviation computing circuit 50 and a detecting unit 70. This detecting unit 70 such as an encoder detects the operation of the drive unit 60 and has its present output value 72 compared with a command value 45 so that the drive unit 60 is driven to reduce the deviation output 55 inbetween to zero. In accordance with this, angles of rotation corresponding to θ 1 , θ 2 , θ 3 , θ 4  and θ 5  of FIG. 2 change purposely. 
     The above angles θ 1 , θ 2 , θ 3 , θ 4  and θ 5  of FIG. 2 designate the respective articulations or angles of rotation of an articulated robot and correspond to the drive unit 60 of FIG. 1. The robot body in this embodiment is five-articulated to have five degrees of freedom. Reference numeral 510 designates a fixed base fixed on a floor; numeral 520 a swivel base capable of rotating at the angle θ 1  with respect to the fixed base 510; numeral 530 a first arm capable of rotating at the angle θ 2  ; and numeral 540 a second arm capable of rotating at the angle θ 3 . Reference numeral 590 designates a robot wrist which is capable of rotating at the angles θ 4  and θ 5  with respect to the second arm 540 and which corresponds to the output end of the robot having five degrees of freedom. An object 600 to be controlled is fixed on the flange of the robot wrist 590. Designated at reference letter P is a control point which is positioned in a prescribed relationship to the robot wrist 590 or the object 700 fixed on the former. In this embodiment, the control point P is set on the axis of rotation of the articulation θ 5  and is apart by a length θ 4  from the center of rotation of another articulation θ 4 . The controlled object 600 is a widely defined working tool such as a hand or a fastening driver and corresponds to a welding torch in this embodiment. The welding torch 600 has its leading end coming into coincidence with the control point P when its center line is extended by a specified length. The control point P can be visually confirmed, although imaginary, in relation to the controlled object 600 as described above. The control point P represents the locational change of the controlled object 600 so that either the location of the point P of the controlled object 600 or the position of the same around the point P can be controlled by driving the drive unit 60 (i.e., θ 1 , θ 2 , θ 3 , θ 4  and θ 5 ), as described above. These controls can be continuously executed in accordance with taught data which are taught in advance in a taught data memory means 120 of FIG. 1. 
     The teaching operation of the taught data will be described in the following. By manually operating a not-shown robot control board or teaching box key board, the location and position of the control point P are purposively set in relation to a work 700 of FIG. 3, for example. This setting corresponds to the steps of a block 10 of FIG. 1. Following the manual operation 10, a necessary incrementation 20 is conducted. This is for changing the location and position of the control point P, but its details are omitted here. The result of the incrementation 20 is set in a register 30 and is subjected to an inverse coordinate transformation until it is outputted as the command value 45. If the control is made in such a manual mode and if it is judged that the location and position concerning the control point P are proper, output data 78 from the detecting unit 70 at that time are introduced as the taught data or their portion into the memory means 120 by another manual operation according thereto. 
     Here, the error vector will be described with reference to FIG. 4. Reference letter J appearing in FIG. 4 designates the error vector which can be illustrated by an arrow having the point P as a start and a point P&#39; as an end. The point P is an old control point, and the point P&#39; is a new control point. For example, if the welding torch 600 as the controlled object is replaced by new one or hit, the control point is shifted from the old one P to the new one P&#39; in relation to the welding torch 600. This is because the welding torch 600 is accompanied by the shaping, sizing or mounting error. If the taught data 120 are used as they are for the control, the old point P is controlled and has to be corrected to the new point P&#39;. In FIG. 4, the error vector J is designated by a coordinate system XYZ which is restrained by the floor or fixed base 510 of FIG. 2. If this coordinate system XYZ is used, the coordinates of the error vector J can be expressed by [x 0  y 0  z 0  ]. The coordinate [x 0  y 0  z 0  ] of the error vector J thus expressed will change in accordance with the positional change of the robot wrist 590 or the object 600. 
     Here, a variety of coordinate of systems will be described in the following. The coordinate system XYZ is restrained by the fixed base 510, as has been described hereinbefore. This is especially called the general coordinate system which is the most suitable for a person to conceive a three-dimensional space. Likewise, there can be conceived a coordinate system which is restrained by the swivel base 510. There can also be conceived a coordinate system which is restrained by the first arm 530 and the second arm 540. There can be further conceived a coordinate system which is restrained by the output shaft of the articulation θ 4  and a coordinate system which is restrained by the robot wrist 590. The last is especially called here a wrist coordinate system. This wrist coordinate system is designated as X&#39;Y&#39;Z&#39;. There are totally six coordinate systems from the general coordinate system XYZ to the wrist coordinate system X&#39;Y&#39;Z&#39;. This number is one subtracted by 1 from the number of degrees of freedom of the robot, and the location of the control point P or P&#39; and the position therearound can be described by any of those coordinate systems. This similarly applies to the error vector J. 
     Here, transformation between the respective coordinate systems will be thought in the following. Here, these coordinate systems are all orthogonal. The transformation between the wrist coordinate system X&#39;Y&#39;Z&#39; and the general coordinate system XYZ is expressed by the following equation (1): ##EQU1## The letters x 1  &#39;y 1  &#39;z 1  &#39; appearing in the equation (1) designate the coordinates of an arbitrary point represented by the wrist coordinates X&#39;Y&#39;Z&#39;, and the letters x 1  y 1  z 1  designate the coordinates of the same point viewed in the general coordinates XYZ. Letters A 1  to A 5  respectively designate a 4-row, 4-column coordinate transformation matrix, which is expressed by the following equation. Incidentally, letter i appearing in the following equation takes any of 1 to 5 in the present embodiment:  In the above equation (2), the 3-row, 3-column portion (from K 11i  to K 33i ) the coordinate transformation matrix expressing a rotation, and letters K 14i , K 24i  and K 34i  designate the coordinates expressing the displacement of an origin from the old to new coordinate systems. All their coefficients take zero unless the original is not shifted in this meanwhile. 
     The equation (2) will be described more with a supplement. If the 3-row, 3-column portion of the equation (2) is expressed by Ri (θi) and if the letters K 14i , K 24i  and K 34i  are expressed by Si, the above equation (2) can be formally expressed by the following equation: ##EQU2## Now, if the transformation from the old to new coordinate systems is effected by a rotation on the x axis of the old coordinate system, the expresion Ri (θi) takes the form of the following equation (4). On the other hand, the expression Ri (θi) takes the forms of the following equations (5) and (6), respectively, for the rotations on the y and z axes. Here, θi designates the respective quantities of rotation, and the expression Ri (θi) is through as a function having the variable θi: ##EQU3## Here, the coordinate transformation matrix (Ai) of the robot body of FIG. 2 is rearranged, if taken into consideration, as shown in FIG. 6. The old coordinate system for i=1 of FIG. 6 is the aforementioned general one. On the other hand, the new coordinate system for i=5 is the wrist one. In FIG. 6, for example, the phrase &#34;the origin at θ 2  &#34; implies that the origin of the coordinate system under consideration is present at the center of the articulation θ 2 . On the other hand, for example, the phrase &#34;the rotation of θ 1  on the z axis&#34; implies that each reference axis of the new coordinate system rotates in the quantity &#34; 1  on the reference axis z of the old coordinate system&#34;. FIG. 5 shows such one coordinate system x,y,z in relation to the robot body of FIG. 2 as is coincident with the old or general coordinate system for i=1. Moreover, this coordinate system x,y,z is coincident with the new one for i=1 at θ 1  =0 and further with the new one for i=2 at θ 1  =0 and θ 2  =0. In FIG. 6, for example, the coordinate transformation matrix A 3  for effecting transformation from the old to new coordinate systems for i=3 is expressed by the following equation: ##EQU4## 
     In the equation (7), letter θ 2  designates the length of the first arm 530, which can be handled as a constant. Since the variable in the equation (7) is θ 3 , therefore, the coordinate transforming matrix A 3  can be described in the form of A 3  (θ 3 ) by considering it as a function of θ 3 . Likewise, the equation (1) can be expressed by the following equation: ##EQU5## 
     The origin in the new or wrist coordinate system (X&#39;, Y&#39;, Z&#39;) for i=5 in FIG. 6 is coincident with the control point P. If the coordinate values when the coordinates (0, 0, 0) of the point P are transformed into the general coordinate system (X, Y, Z) are designated at x p  y p  z p , the following equation is obtained in accordance with the equation (8): ##EQU6## The righthand side of the equation (9) expresses the location (or position) of the control point P in terms of the angles of rotation (θ 1  to θ 5 ) of the respective articulations (θ 1  to θ 5 ) of the robot body and is a kind of robot coordinate system. Hence, the equation (9) provides one coordinate transformation equation for transformation from the robot coordinate system on its righthand side of the general coordinate system on its lefthand side. This coordinate transformation is called a positive transformation. 
     A block 130 appearing in FIG. 1 is for the positive transformation, and the following equation (10) corrected from the equation (9) is adopted: ##EQU7## The portion (x 0  &#39;y 0  &#39;z 0  &#39;) in the equation (10) expresses the error vector designated by the wrist coordinate system (X&#39;Y&#39;Z&#39;) so that the equation (10) becomes coincident with the equation (9) when the error vector J is zero. The portion (x p  &#39;y p  &#39;z p  &#39;) expresses the location (or location vector) of the new control point P&#39; in terms of the general coordinate system (XYZ). The execution of the computation of the equation (10) requires the data (x 0  &#39;y 0  &#39;z 0  &#39;) of the error vector J, which is stored in error vector memory means 110. The execution also requires the data on θ 1  to θ 5 , which are obtained by reading out the taught data memory means 120. 
     As to the positive transformation containing the error vector J of the foregoing equation (10), the following specific computations are made with reference to FIGS. 2 and 6 and to the following equations (4) to (6): ##EQU8## 
     Incidentally, the values θ 3  &#39; and θ 4  &#39; of the equations (14) and (15) were used in place of the angles of rotation θ 3  and θ 4  in the equations (11) to (13). This is because the respective motors for the drive unit 60 of the articulations θ 3  and θ 4  are fixed on the swivel base 520 so that their rotational outputs are transmitted to the respective articulations θ 3  and θ 4  through not-shown power transmission means (e.g., a chain mechanism). The angles of rotation θ 3  &#39; and θ 4  &#39; of the respective motors in this case conform to the equations (14) and (15). 
     Here, an interpolation shown by a block 140 of FIG. 1 will be described in the following. The taught data (120) designate the location and position of the controlled object 600 in time-series and non-continuous manners. Several points are interpolated between one point to another so that the locus inbetween may be smoothed or specified. Linear or arcuate interpolations are frequently used as such interpolation. This similarly applies to the present embodiment, in which the interpolation (140) is conducted in accordance with data 135 in the general coordinate system (XYZ) after the taught data (120) and the error vector J (110) have been subjected to the positive transformation (130) by the foregoing equation (10). These data according to the general coordinate system (XYZ) are the so-called &#34;data on taught points&#34;, and the data on interpolated points based thereon are also those of the general coordinate system (XYZ). The aforementioned register 30 is set with such respective output data 145 from the interpolation means 140 as contain the taught points and interpolated points thus far described. The subsequent process is similar to the one in the case of the manual operation and is conducted in this case in a mode (i.e., an automatic operation mode) for the robot operation in accordance with the taught data 120. 
     Here, the inverse transformation means in the block 40 will be newly described in the following. The portion x p  &#39;y p  &#39;z p  &#39; of the equation (10) is determined by the positive transformation means 130. That portion expresses the location data on the new control point P&#39;. These data in the general coordinate system (XYZ) contain no data concerning the position of the new controlled object 600 around the new control point P&#39; but are incomplete in this sense. For describing the position in the general coordinate system (XYZ), for example, there is known a method using the angles of rotation on the X and Y axes. The position can also be described by the angle of rotation θ 4  and θ 5  of the articulations θ 4  and θ 5  in the robot coordinate system (θ 1 , θ 2 , θ 3 , θ 4  and θ 5 ). The present embodiment belongs to the latter method. As a result, the output data 145 from the interpolation means 140 are accurately x p  &#39;y p  &#39;z p  &#39;θ 4  θ 5  expressed in the general coordinates. Under the condition of the foregoing data being known, the inverse transformation means 40 determines the angles of rotation (θ 1  θ 2  θ 3 ) of the remaining unknown articulations (θ 1  θ 2  θ 3 ). These specific computations are as follows on the basis of FIGS. 2 and 6: ##EQU9## The double signs appearing in the above equations (16) to (18) are selected to fall within the robot operation range. Moreover, the undefined variables are expressed by the following equations: ##EQU10## 
     In the specific computations described above, the angles of rotation θ 3  &#39; and θ 4  &#39; of the equations (14) and (15) were used in place of the angles of rotation θ 3  and θ 4  by the reasons similar to the aforementioned ones. 
     Next, the error computing means of a block 100 appearing in FIG. 1 will be described in the following. This means determines the error vector J in the wrist coordinate system and corresponds to the portion (x 0  &#39;y 0  &#39;z 0  &#39;) in the equation (10). Here, the equation (10) is interpreted at the level of the equations (4), (5) and (6) or the equation (7), and the equations (14) and (15) are substituted thereinto. As a result, the following equation is obtained: ##EQU11## 
     The letters B 1  (θ 1 ) and B 1  (θ 5 ) appearing in the equation (25) are coordinate transformation matrixes having forms similar to that of the equation (6) so that they take the following form if the values θ 1  and θ 5  are represented by θ: ##EQU12## 
     On the other hand, the letters B 2  (θ 2 ), B 2  (θ 3  &#39;), and B 2  (θ 4  &#39;) appearing in the equation (25) are coordinate transformation matrixes having forms similar to that of the equation (5) so that they take the following form if the values θ 2 , θ 3  &#39; and θ 4  &#39; are represented by θ: ##EQU13## 
     In the block 100, the error vector J (x 0  &#39;y 0  &#39;z 0  &#39;) is determined on the basis of the equation (25). In the practical operation, output data 74 and 76 of multiple positions of one point from the detecting unit 70 are used as parameters. If the operation is branched to the error vector measuring program by the manual operation 10, although not specifically shown in FIG. 1, the operator determines the one-point, multi-position manual operation 10. The operator positions the preceding control point P of the controlled object 600 in a characteristic point (e.g., an apex or corner whose position can be discriminated) on the work 700 and then depresses a not-shown confirmation button after the positioning operation. The operator repeats the positioning and confirming operations described above, upon each of which he changes the position of the controlled object 600. If the confirmation button is depressed, on the other hand, the output data 74 (or 76) from the detecting unit 70 at that time are stored in memory means 80 (or 90). When the program for the error vector measurement is completed, the error computation 100 is executed. 
     In accordance with the one-point, multi-position operation, a plurality of sets of data according to the number of the positions are stored in the memory means 80 and 90. The set number is at least two or more, preferably three or more. Now, it is assumed that the plural sets of data be expressed by the following equations: ##EQU14## The plural sets of data specified above are those when their control points P are all positioned in a common location. Hence, the following equation holds by eliminating (x p  &#39;y p  &#39;z p  &#39;) from the equation (25): ##EQU15## 
     The above equation (28) takes the following form if it is rearranged in respect of (x p  &#39;y p  &#39;z p  &#39;): wherein: ##EQU16## 
     The value (x p  &#39;y p  &#39;z p  &#39;) is determined from the equation (29) in the following form: ##EQU17## 
     Since the equation (30) is a ternary, simultaneous linear equation, it can be solved to determined the value (x p  &#39;y p  &#39;z p  &#39;). This result is stored in the error vector memory means 110. In case the positional change accompanying the one-point, multi-position operation happens to be unique, as in case both the values D and E in the equation (29) are zero, the error vector J cannot be determined. In this particular case, the number of the simultaneous equations may be increased by using three sets or more of data. 
     The error vector J in FIG. 1 is referred to for the position transformation 130 and for the inverse transformation 40. This respect will be a little supplemented in the following. The error vector J to be referred to for the positive transformation 130 is one concerning the controlled object 600 when in the teaching operation. On the other hand, the error vector J to be referred to for the inverse transformation 40 is one concerning the controlled object 600 when in the operation (which is based on the taught data 120). As a result, when those error vectors J are not coincident (as in case the controlled object is hit or replaced by new one after the taught data 120 has been generated), it is necessary to store the two kinds of error vectors J in the memory means 110 so that they may be separately used. If the error vectors J are identical, on the contrary, the same error vectors J are referred to for both the positive transformation 130 and the inverse transformation 40. This may apparently nullify the result by conducting the minus correction after the plus correction, but not in practice. If the interpolation 40 of FIG. 1 is linear, it is not the new control point P&#39; but the old point P to follow the linear locus in the presence of the error vector J. Despite this fact, even for the error vector J common between the positive transformation 130 and the inverse transformation 40, the locus of the new point P&#39; is straight if the common error vector J is referred to. This corresponds to the case of linear interpolation. However, like discussion applies to the case of another arcuate interpolation. In the latter case, the arcuate locus of not the old point P but the new point P&#39; is warranted.