Controller for robot and inverse transforming method for robot

Abstract

A robot has a vertical 6-axis articulated arm having an offset arm having a fifth axis and length link, and mutually linking fourth and sixth axes. The fourth and sixth axes shaft centers are parallel. The articulated arm has a head portion designated as a control point. A position and an orientation targeted to the control point is processed by an inverse transform to calculate angles of the axes. A provisional target position of the sixth axis is obtained by subtracting the link length from a target position of the sixth axis. The link length to the provisional target position is given zero to perform the inverse transform process. Processed results are evaluated. Until a difference between a calculated sixth-axis angles and provisionally decided sixth-axis angles becomes equal to or less than a predetermined value, processes started from the angle provisional decision of the sixth axis are repeatedly performed.

Claims

1. A controller, applied to a robot provided with a vertical 6-axis articulated arm having an offset arm, the offset arm being provided with a fifth axis, linking a fourth axis and a sixth axis with each other, and having a link length d5, the fourth axis having a shaft center and the sixth axis having a shaft center, both of the shaft centers being parallel with each other, the arm having a head portion designated as a control point, a position and an orientation targeted to the control point being processed by an inverse transform in order to calculate an angle of each of the axes, the controller comprising: an angular provisional deciding unit configured to provisionally decide an angle of a sixth angle; a direction calculating unit configured to calculate a direction of the offset arm based on the provisionally decided angle of the sixth axis; a provisional target position calculating unit configured to calculate a provisional target position which is subtracted from a target position of the sixth axis by the link length d5; an inverse transform processing unit configured to apply an inverse transform process to the provisional target position in a state where the link length d5 is set to 0; and an estimation unit configured to repeat the processes starting from the provisional decision performed by the angular provisional deciding unit until a difference between an angle of the sixth axis, which is obtained by estimating a result from the inverse transform process based on an estimation function, and the provisionally decided angle of the sixth axis becomes equal to or less than a predetermined value.

2. The controller for the robot according to claim 1, wherein a third axis and the fourth axis have approach vectors which are described as a3 and a4, and the estimation unit is configured to set, as the estimation function, an inner product of a3a4.

3. The controller for the robot according to claim 1, comprising a form deciding unit configured to decide a form of a wrist resulting from the inverse transform process by previously employing any of angles of the fourth to sixth axes to the same end effector position.

4. The controller for the robot according to claim 1, comprising a form deciding unit configured to decide a form of a wrist based on results of the inverse transform process, by calculating a determinant on a Jacobian matrix with the results of the inverse transform process and determining whether or not signs of the determinant are in accord with a previously designated wrist form.

5. An inverse transform processing method, applied to a robot provided with a vertical 6-axis articulated arm having an offset arm, the offset arm being provided with a fifth axis, linking a fourth axis and a sixth axis with each other, and having a link length d5, the fourth axis having a shaft center and the sixth axis having a shaft center, both of the shaft centers being parallel with each other, the arm having a head portion designated as a control point, a position and an orientation targeted to the control point being processed by an inverse transform in order to calculate an angle of each of the axes, the method comprising: a step configured to provisionally decide an angle of a sixth angle; a step configured to calculate a direction of the offset arm based on the provisionally decided angle of the sixth axis; a step configured to calculate a provisional target position which is subtracted from a target position of the sixth axis by the link length d5; a step configured to apply an inverse transform process to the provisional target position in a state where the link length d5 is set to 0; and a step configured to repeat the processes starting from the provisional decision until a difference between an angle of the sixth axis, which is obtained by estimating a result from the inverse transform process based on an estimation function, and the provisionally decided angle of the sixth axis becomes equal to or less than a predetermined value.

6. The inverse transform processing method of claim 5, wherein a third axis and the fourth axis have approach vectors which are described as a3 and a4, and the estimation function is set as an inner product of a3a4.

7. The inverse transform processing method of claim 5, comprising deciding a form of a wrist resulting from the inverse transform process by previously employing any of angles of the fourth to sixth axes to the same end effector position.

8. The inverse transform processing method of claim 5, comprising deciding a form of a wrist based on results of the inverse transform process, by calculating a determinant on a Jacobian matrix with the results of the inverse transform process and determining whether or not signs of the determinant are in accord with a previously designated wrist form.

9. A controller, applied to a robot provided with a vertical 6-axis articulated arm having an offset arm, the offset arm being provided with a fifth axis, linking a fourth axis and a sixth axis with each other, and having a link length d5, the fourth axis having a shaft center and the sixth axis having a shaft center, both of the shaft centers being parallel with each other, the arm having a head portion designated as a control point, a position and an orientation targeted to the control point being processed by an inverse transform in order to calculate an angle of each of the axes, the controller comprising: a position provisional deciding unit configured to provisionally decide a position P6 of the sixth axis and provisionally decide a position P5 of the fifth axis based on the provisionally decided position P6; a motion range determining unit configured to determine whether or not the provisionally decided positions P5 and P6 are within a motion range defined based on link parameters of the robot; an inverse transform processing unit configured to perform an inverse transform process of a homogeneous transformation matrix based on the positions P5 and P6 in a case where the provisionally decided positions P5 and P6 are within the motion range, the inverse transform process being performed under a condition that the link length d5=0; a forward transform processing unit configured to perform a forward transform process based on angles of the respective axes obtained by the inverse transform process; an evaluation value calculating unit configured to calculate a position matrix p.sub.E of a difference between a target position of the control point and a position resulting from the forward transform process, and calculate a rotation matrix R.sub.E by multiplying a rotation matrix corresponding to the target position, by an inverse matrix of the rotation matrix resulting from the forward transform process; and a calculation repeating unit configured to repeatedly calculate processes starting from the inverse transform process, based on a homogeneous transformation matrix in which the position matrix p.sub.E and the rotation matrix R.sub.E are reflected, when the position matrix p.sub.E has a norm exceeding a threshold thereof or the rotation matrix R.sub.E has an angle exceeding a threshold thereof.

10. The controller for the robot according to claim 9, wherein the inverse transform processing unit is configured to positionally correct the positions P5 and P6 so to be within the motion range when the positions P5 and P6 are not within the motion range.

11. The controller for the robot according to claim 9, wherein the calculation repeating unit is configured to determine whether or not the position of the control point decided based on the homogeneous transformation matrix is within the motion range, and correct the homogeneous transformation matrix such that the position of the control point is located within the motion range when the position of the control point is not within the motion range.

12. The controller for the robot according to claim 9, comprising: a form deciding unit configured to calculate a determinant based on a Jacobian matrix depending on results of the inverse transform process, and decide a form of a wrist which results from the inverse transform process, by determining whether or not signs of the determinant are in accord with a previously designated form of the wrist.

13. An inverse transform processing method, applied to a robot provided with a vertical 6-axis articulated arm having an offset arm, the offset arm being provided with a fifth axis, linking a fourth axis and a sixth axis with each other, and having a link length d5, the fourth axis having a shaft center and the sixth axis having a shaft center, both of the shaft centers being parallel with each other, the arm having a head portion designated as a control point, a position and an orientation targeted to the control point being processed by an inverse transform in order to calculate an angle of each of the axes, the method comprising: a step configured to provisionally decide a position P6 of the sixth axis and provisionally decide a position P5 of the fifth axis based on the provisionally decided position P6; a step configured to determine whether or not the provisionally decided positions P5 and P6 are within a motion range defined based on link parameters of the robot; a step configured to perform an inverse transform process of a homogeneous transformation matrix based on the positions P5 and P6 in a case where the provisionally decided positions P5 and P6 are within the motion range, the inverse transform process being performed under a condition of the link length d5=0; a step configured to perform a forward transform process based on angles of the respective axes obtained by the inverse transform process; a step configured to calculate a position matrix p.sub.E of a difference between a target position of the control point and a position resulting from the forward transform process, and calculate a rotation matrix RE by multiplying a rotation matrix corresponding to the target position, by an inverse matrix of the rotation matrix resulting from the forward transform process; and a step configured to repeatedly calculate processes starting from the inverse transform process, based on a homogeneous transformation matrix in which the position matrix p.sub.E and the rotation matrix R.sub.E are reflected, when the position matrix p.sub.E has a norm exceeding a threshold thereof or the rotation matrix R.sub.E has an angle exceeding a threshold thereof.

14. The inverse transform processing method according to claim 13, wherein the inverse transform processing step positionally corrects the positions P5 and P6 so to be within the motion range when the positions P5 and P6 are not within the motion range.

15. The inverse transform processing method according to claim 13, wherein the calculation repeating step determines whether or not the position of the control point decided based on the homogeneous transformation matrix is within the motion range, and corrects the homogeneous transformation matrix such that the position of the control point is located within the motion range when the position of the control point is not within the motion range.

16. The inverse transform processing method according to claim 13, comprising a step configured to calculate a determinant based on a Jacobian matrix depending on results of the inverse transform process, and decide a form of a wrist which results from the inverse transform process, by determining whether or not signs of the determinant are in accord with a previously designated form of the wrist.

17. A controller, applied to a robot provided with a vertical 6-axis articulated arm having an offset arm, the offset arm being provided with a fifth axis, linking a fourth axis and a sixth axis with each other, and having a link length d5, the fourth axis having a shaft center and the sixth axis having a shaft center, both of the shaft centers being parallel with each other, the arm having a head portion designated as a control point, a position and an orientation targeted to the control point being processed by an inverse transform in order to calculate an angle of each of the axes, the controller comprising: a position provisional deciding unit configured to provisionally decide a position P6 of the sixth axis and provisionally decide a position P5 of the fifth axis based on the provisionally decided position P6; a motion range determining unit configured to determine whether or not the provisionally decided positions P5 and P6 are within a motion range defined based on link parameters of the robot; an inverse transform processing unit configured to perform an inverse transform process of a homogeneous transformation matrix based on the positions P5 and P6 in a case where the provisionally decided positions P5 and P6 are within the motion range, the inverse transform process being performed under a condition of the link length d5=0; a forward transform processing unit configured to perform a forward transform process based on angles of the respective axes obtained by the inverse transform process; an evaluation value calculating unit configured to calculate a position matrix p.sub.E of a difference between a target position of the control point and a position resulting from the forward transform process, and calculate a rotation matrix R.sub.E by multiplying a rotation matrix corresponding to the target position, by an inverse matrix of the rotation matrix resulting from the forward transform process; and a calculation repeating unit configured to: calculate a rotation axis vector q.sub.E of the rotation matrix R.sub.E when the position matrix p.sub.E has a norm p.sub.E exceeding a threshold thereof or the rotation matrix R.sub.E provides an angle R.sub.E exceeding a threshold thereof, calculate a rotation vector r.sub.E defined as a product between the angle R.sub.E and a unit rotation axis vector q.sub.E/q.sub.E, and update the angles of the respective axes based on results obtained by multiplying a position matrix P.sub.E defined by the position matrix p.sub.E and the rotation vector r.sub.E, by an inverse matrix of the Jacobian matrix obtained from the angels of the respective axes calculated by the inverse transform process, processes starting from the forward transform process being repeated.

18. The controller for the robot according to claim 17, wherein the inverse transform processing unit is configured to positionally correct the positions P5 and P6 so to be within the motion range when the positions P5 and P6 are not within the motion range.

19. The controller for the robot according to claim 17, comprising: a form deciding unit configured to calculate a determinant based on a Jacobian matrix depending on results of the inverse transform process, and decide a form of a wrist which results from the inverse transform process, by determining whether or not signs of the determinant are in accord with a previously designated form of the wrist.

20. An inverse transform processing method, applied to a robot provided with a vertical 6-axis articulated arm having an offset arm, the offset arm being provided with a fifth axis, linking a fourth axis and a sixth axis with each other, and having a link length d5, the fourth axis having a shaft center and the sixth axis having a shaft center, both of the shaft centers being parallel with each other, the arm having a head portion designated as a control point, a position and an orientation targeted to the control point being processed by an inverse transform in order to calculate an angle of each of the axes, the method comprising: a step configured to provisionally decide a position P6 of the sixth axis and provisionally decide a position P5 of the fifth axis based on the provisionally decided position P6; a step configured to determine whether or not the provisionally decided positions P5 and P6 are within a motion range defined based on link parameters of the robot; a step configured to perform an inverse transform process of a homogeneous transformation matrix based on the positions P5 and P6 in a case where the provisionally decided positions P5 and P6 are within the motion range, the inverse transform process being performed under a condition of the link length d5=0; a step configured to perform a forward transform process based on angles of the respective axes obtained by the inverse transform process; a step configured to calculate a position matrix p.sub.E of a difference between a target position of the control point and a position resulting from the forward transform process, and calculate a rotation matrix R.sub.E by multiplying a rotation matrix corresponding to the target position, by an inverse matrix of the rotation matrix resulting from the forward transform process; and a step configured to: calculate a rotation axis vector q.sub.E of the rotation matrix R.sub.E when the position matrix p.sub.E has a norm p.sub.E exceeding a threshold thereof or the rotation matrix RE provides an angle R.sub.E exceeding a threshold thereof, calculate a rotation vector r.sub.E defined as a product between the angle R.sub.E and a unit rotation axis vector q.sub.E/q.sub.E, and update the angles of the respective axes based on results obtained by multiplying a position matrix P.sub.E defined by the position matrix p.sub.E and the rotation vector r.sub.E, by an inverse matrix of the Jacobian matrix obtained from the angels of the respective axes calculated by the inverse transform process, processes starting from the forward transform process being repeated.

21. The inverse transform processing method according to claim 20, wherein the inverse transform processing step positionally corrects the positions P5 and P6 so to be within the motion range when the positions P5 and P6 are not within the motion range.

22. The inverse transform processing method according to claim 20 comprising a step configured to calculate a determinant based on a Jacobian matrix depending on results of the inverse transform process, and decide a form of a wrist which results from the inverse transform process, by determining whether or not signs of the determinant are in accord with a previously designated form of the wrist.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) In the accompanying drawings:

(2) FIG. 1 is a diagram showing the configuration of a robot system according to a first embodiment;

(3) FIG. 2 is a diagram showing a robot coordinate system;

(4) FIG. 3 is a diagram showing a robot configuration projected on an xz plane and a yz plane;

(5) FIG. 4 shows both an xy coordinate and an r coordinate on both of which a robot arm form is depicted;

(6) FIG. 5 shows an z coordinate which on which a robot arm form is depicted;

(7) FIG. 6 shows the z coordinate on which various forms of the robot arm are defined;

(8) FIG. 7 is a flowchart explaining an inverse transform process performed by a controller in a first embodiment;

(9) FIG. 8 is a view explain a motion range in a case of d2=d3;

(10) FIG. 9 is an illustration which explains four wrist forms of a robot arm at the same end-effector position;

(11) FIG. 10 is a flowchart explaining an inverse transform process performed by a controller in a second embodiment;

(12) FIG. 11 is a flowchart explaining an inverse transform process performed by a controller in a third embodiment;

(13) FIG. 12 is a flowchart explaining an inverse transform process performed by a controller in a fourth embodiment;

(14) FIG. 13 is an illustration explaining a motion range provided when d2=d3 in the fourth embodiment; and

(15) FIG. 14 is a flowchart explaining an inverse transform process performed by a controller in a fifth embodiment.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

(16) Hereinafter, with reference to the accompanied drawings, robot control apparatuses and inverse transform processes for robot control will now be explained according to various types of embodiments.

First Embodiment

(17) With reference to FIGS. 1 to 9, a first embodiment will now be described.

(18) As shown in FIG. 1, a robot system 1 is provided with a vertical articulated robot 2, a controller for controlling the robot 2, and a base 4 in which the controller 3 is incorporated. This robot system 1 is provided for general industrial use. The robot 2 is a 6-axis vertical articulated robot.

(19) On the base 4, there is provided a first axis J1 having a z-directional shaft center, via which a shoulder 5 is rotatably coupled with the base 4 in a horizontal direction. The shoulder 5 has a second axis J2 having a y-directional shaft center, and is coupled with a second offset arm 6 extending in the y direction. Via this second offset arm 6, a lower part of a first arm 7 extending upward is coupled with the shoulder 5 and rotatable in the vertical direction. The first arm 7 has a head portion provided with a third axis J3 having a y-directional shaft center, and a second arm 9 is coupled with the head portion of the first arm 7 via a third offset arm 8 extending in the y direction and is and rotatable in the vertical direction. The second arm 9 is composed of a base portion 9a and a head portion 9b

(20) The second arm 9 has a fourth axis J4 having an x-directional shaft center, in which the head portion 9b is twistably coupled with the base portion 9a. The head portion 9b of the second arm 9 is provided with a fifth axis J5 having a y-directional shaft center, and a wrist 11 is coupled with the head portion 9b via a fifth offset arm 10 extending in the y direction and is rotatable in the vertical direction. The wrist 11 is twistably coupled with a flange and hand 12, as shown in FIG. 12, via a sixth axis J6 having the x-directional shaft center. At the respective axes J1 to J6 of the robot 2, an electric motor (not shown) is provided which serves as a drive source.

(21) The controller 3 is a device for controlling the motions of the robot 2 and is provided with a computer including a CPU, a ROM and a RAM. The computer thus acts as control means which perform computer programs to control the motions of the robot 2. Practically, the controller 3 is also provided with drive units including inverter circuits. By this configuration, an encoder, which is arranged for each of the motors located at the axes J1 to J6, detects a motor rotating position, and based on this detection, drive of the motor at each axis is controlled by, for example, feedback control.

(22) Practically, the controller includes the CPU (central processing unit) 3A, a ROM (read-only memory) 3B, a RAM (random access memory) 3C, drive circuits 3D, position detecting circuits 3E, and other circuit components, all of which are necessary for conducing motion control of the robot and an inverse transform process method for the robot control. These computer components are communicably connected to each other via an internal bus 3F and connected to the drive circuits 3D and the position detecting circuits 3E via an interface 3G. In the present embodiment, a display unit 3G and an input unit 3H are connected to the internal bus 3F. These components are known, except for an inverse transform process performed by the CPU 3A.

(23) The ROM 3B is provided to previously store therein system programs and operation programs for the robot 2. The RAM 3B is provided to temporarily store therein parameter values and other information data which are necessary for performance of the various programs. In the controller 4, the CPU performs programs stored in the ROM 3B, which enables the controller 3 to functionally act as and correspond to an angle provisional deciding unit, a direction calculating unit, a provisional target position calculating unit, an inverse transform process unit, an estimation unit, and a form deciding unit. The position detecting circuits 3E receive signals detected by the encoders (not shown) arranged at the joints of the robot 2 and, based on the signals detected the encoders, detects rotational angular positions of the motors arranged at the joints. The drive circuits 3D drive the motors at the respective axes J1 to J6, under the control of the controller 3.

(24) The controller 3 (precisely, the CPU 3A) executes an operating program which is previously set, whereby the controller 3 controls the positions and orientations of control points given at the arm head portions based on position information provided from the position detecting circuits 3E.

(25) In the present embodiment, the controller 3 performs CP (Continuous Path) control. During the CP control, positions and orientations which are to be targeted of control points at the arm head portions, which correspond to motion trajectories, are set as functions of time when making the control points move to their targets. The positions and orientations to be targeted includes not only positions and orientations which are taught but also positions and orientations interpolated based on the taught positions and orientations. Via performance of the CP control, the controller 3 controls angles of the respective joints of the arms such that the positions and orientations of the control points change along the motion trajectories. In the control of the positions and orientations, the controller 3 is configured to perform an inverse transform process which calculates the angles of the first to sixth axes in order to realize positions and orientations which are now commanded as their targets.

(26) As shown in FIG. 2, the respective joints of the robot 2 have first to sixth coordinate systems .sub.1 to .sub.6 which are defined as three-dimensional orthogonal coordinate systems. Each of the coordinate systems .sub.1 to .sub.6 has an origin which is set at a predetermined position on each of the first to sixth axial lines J1 to J6. The respective coordinate systems .sub.1 to .sub.6 have z axes consisting of z1 to z6 axes, which are spatially in accord with the first to sixth axial lines J1 to J6.

(27) At the base 4, there is defined a robot coordinate system which is the 0-th coordinate system .sub.0. This 0-th coordinate system .sub.0 does not change when the first to sixth axes rotate. In the present embodiment, the origin of the coordinate system .sub.0 is placed on the first axial line J1. The coordinate system .sub.0 has a z axis which is indicted as a z0 axis, which is specially in accord with the first axial line J1.

(28) The reference symbols d1 to d6 and a2, a3 shown in FIG. 2 are defined as follows:

(29) d1: a link length from the origin of the 0-th coordinate system .sub.0 to the origin of the first coordinate system .sub.1,

(30) d2: a link length from the origin of the first coordinate system .sub.1 to the base portion of the first arm 7,

(31) a2: a link length from the base portion of the first arm 7 to the head portion thereof, i.e., the origin of the second coordinate system .sub.2,

(32) d3: a link length from the origin of the second coordinate system .sub.2 to the head portion of the third offset arm 8,

(33) a3: a distance between the shaft centers of the third and fourth axes J3 and J4,

(34) d4: a link length from the origin of the third coordinate system .sub.3 to the origin of the fourth coordinate system .sub.4,

(35) d5: a link length from the origin of the fourth coordinate system .sub.4 to the origin of the fifth coordinate system .sub.5, and

(36) d6: a link length from the origin of the fifth coordinate system .sub.5 to the origin of the sixth coordinate system .sub.6.

(37) The distance a3 is illustrated in FIG. 3 for reference. The link length d5 corresponds to the link length of the fifth offset arm 10.

(38) First of all, on the premise of explaining the inverse transform process carried out in the present embodiment, a forward transform process will now be described.

(39) <Forward Transform Process>

(40) DH parameters are decided as shown in table 1, by performing coordinate conversions in the order of z-axis rotation, z-axis translation, x-axis translation, and x-axis rotation. .sub.i indicates a rotation angle of each of the joints which rotates from an angular state shown in FIG. 2.

(41) TABLE-US-00001 TABLE 1 DH parameters .sub.i d.sub.i (S) a.sub.i .sub.i 1 .sub.1 d.sub.1 a.sub.1 = 0 90 2 .sub.2 90 d.sub.2 a.sub.2 0 3 .sub.3 + 90 d.sub.3 a.sub.3 +90 4 .sub.4 d.sub.4 0 90 5 .sub.5 d.sub.5 0 +90 6 .sub.6 d.sub.6 0 0

(42) Homogeneous transformation matrixes expressing a base coordinate system .sub.0 to a mechanical interface coordinate system .sub.6 are as follows. In the following matrixes, n, o and a denote a normal vector, an orient vector, and an approach vector, respectively. For simplifying denotations, sin .sub.i and cos .sub.i are denoted as s.sub.i and c.sub.i, respectively. In addition, for example, S.sub.23 denotes sin(.sub.2+.sub.3).

(43) $\begin{matrix} {}^{i - 1}T_{i} = [\begin{matrix} \cos_{i} & - \sin_{i} & 0 & 0 \\ \sin_{i} & \cos_{i} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_{i} \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & a_{i} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & \cos_{i} & - \sin_{i} & 0 \\ 0 & \sin_{i} & \cos_{i} & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] & (1) \\ {}^{0}T_{1} = [\begin{matrix} c_{1} & - s_{1} & 0 & 0 \\ s_{1} & c_{1} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & a_{1} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_{1} \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & - 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} c_{1} & 0 & - s_{1} & a_{1} c_{1} \\ s_{1} & 0 & c_{1} & a_{1} s_{1} \\ 0 & - 1 & 0 & d_{1} \\ 0 & 0 & 0 & 1 \end{matrix}] & (2) \\ {}^{1}T_{2} = [\begin{matrix} s_{2} & c_{2} & 0 & 0 \\ - c_{2} & s_{2} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & a_{2} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_{2} \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} s_{2} & c_{2} & 0 & a_{2} c_{2} \\ - c_{2} & s_{2} & 0 & - a_{2} s_{2} \\ 0 & 0 & 1 & d_{2} \\ 0 & 0 & 0 & 1 \end{matrix}] & (3) \\ {}^{2}T_{3} = [\begin{matrix} - s_{3} & - c_{3} & 0 & 0 \\ c_{3} & - s_{3} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & - a_{3} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & - d_{3} \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 0 & - 1 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} - s_{3} & 0 & c_{3} & a_{3} s_{3} \\ c_{3} & 0 & s_{3} & - a_{3} c_{3} \\ 0 & 1 & 0 & - d_{3} \\ 0 & 0 & 0 & 1 \end{matrix}] & (4) \\ {}^{3}T_{4} = [\begin{matrix} c_{4} & - s_{4} & 0 & 0 \\ s_{4} & c_{4} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_{4} \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & - 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} c_{4} & 0 & - s_{4} & 0 \\ s_{4} & 0 & c_{4} & 0 \\ 0 & - 1 & 0 & d_{4} \\ 0 & 0 & 0 & 1 \end{matrix}] & (5) \\ {}^{4}T_{5} = [\begin{matrix} c_{5} & - s_{5} & 0 & 0 \\ s_{5} & c_{5} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & - d_{5} \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 0 & - 1 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} c_{5} & 0 & s_{5} & 0 \\ s_{5} & 0 & - c_{5} & 0 \\ 0 & 1 & 0 & - d_{5} \\ 0 & 0 & 0 & 1 \end{matrix}] & (6) \\ {}^{5}T_{6} = [\begin{matrix} c_{6} & - s_{6} & 0 & 0 \\ s_{6} & c_{6} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & d_{6} \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} c_{6} & - s_{6} & 0 & 0 \\ s_{6} & c_{6} & 0 & 0 \\ 0 & 0 & 1 & d_{6} \\ 0 & 0 & 0 & 1 \end{matrix}] & (7) \\ ^{0} T_{2} = [\begin{matrix} c_{1} s_{2} & c_{1} s_{2} & - s_{1} & x_{1} + a_{2} c_{1} s_{2} - d_{2} s_{1} \\ s_{1} s_{2} & s_{1} c_{2} & c_{1} & y_{1} + a_{2} s_{1} s_{2} + d_{2} c_{1} \\ c_{2} & - s_{2} & 0 & z_{1} + a_{2} c_{2} \\ 0 & 0 & 0 & 1 \end{matrix}] & (8) \\ {}^{0}T_{3} = [\begin{matrix} c_{1} (c_{2} c_{3} - s_{2} s_{1}) & - s_{1} & c_{1} (\begin{matrix} s_{2} c_{3} + \\ c_{2} s_{3} \end{matrix}) & \begin{matrix} x_{2} - a_{3} c_{1} (c_{2} c_{3} - s_{2} s_{3}) + \\ d_{3} s_{1} \end{matrix} \\ s_{1} (c_{2} c_{3} - s_{2} s_{3}) & c_{1} & s_{1} (\begin{matrix} s_{2} c_{3} + \\ c_{2} s_{3} \end{matrix}) & \begin{matrix} y_{2} - a_{3} c_{1} (c_{2} c_{3} - s_{2} s_{3}) - \\ d_{3} s_{1} \end{matrix} \\ - s_{2} c_{3} - c_{2} s_{3} & 0 & c_{2} c_{3} - s_{2} s_{3} & z_{2} + a_{3} (c_{2} s_{3} + s_{2} s_{3}) \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} c_{1} c_{23} & - s_{1} & c_{1} s_{23} & x_{2} - a_{3} c_{1} c_{23} + d_{3} s_{1} \\ s_{1} c_{23} & c_{1} & s_{1} s_{23} & y_{2} - a_{3} s_{1} c_{23} - d_{3} c_{1} \\ - s_{23} & 0 & c_{23} & z_{2} + a_{3} s_{23} \\ 0 & 0 & 0 & 1 \end{matrix}] & (9) \\ {}^{0}T_{4} = [\begin{matrix} c_{1} c_{23} c_{4} - s_{1} s_{4} & - c_{1} s_{23} & - c_{1} c_{23} s_{4} - s_{1} c_{4} & x_{3} + d_{4} c_{1} s_{23} \\ c_{1} s_{4} + s_{1} c_{23} c_{4} & - s_{1} s_{23} & c_{1} c_{4} - s_{1} c_{23} s_{4} & y_{3} + d_{4} s_{1} s_{23} \\ - s_{23} c_{4} & - c_{23} & s_{23} s_{4} & z_{3} + d_{4} c_{23} \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} n_{x 4} & o_{x 4} & a_{x 4} & x_{4} \\ n_{y 4} & o_{y 4} & a_{y 4} & y_{4} \\ n_{z 4} & o_{z 4} & a_{z 4} & z_{4} \\ 0 & 0 & 0 & 1 \end{matrix}] & (10) \\ {}^{0}T_{5} = [\begin{matrix} n_{x 4} c_{5} + o_{x 4} s_{5} & a_{x 4} & n_{x 4} s_{5} - o_{x 4} c_{5} & x_{4} - a_{x 4} d_{5} \\ n_{y 4} c_{5} + o_{y 4} s_{5} & a_{y 4} & n_{y 4} s_{5} - o_{y 4} c_{5} & y_{4} - a_{y 4} d_{5} \\ n_{z 4} c_{5} + o_{z 4} s_{5} & a_{z 4} & n_{z 4} s_{5} - o_{z 4} c_{5} & z_{4} - a_{z 4} d_{5} \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} n_{x 5} & o_{x 5} & a_{x 5} & x_{5} \\ n_{y 5} & o_{y 5} & a_{y 5} & y_{5} \\ n_{z 5} & o_{z 5} & a_{z 5} & z_{5} \\ 0 & 0 & 0 & 1 \end{matrix}] & (11) \\ {}^{0}T_{6} = [\begin{matrix} n_{x 5} c_{6} + o_{x 5} s_{6} & - n_{x 5} s_{6} + o_{x 5} c_{6} & a_{x 5} & x_{5} + a_{x 5} d_{6} \\ n_{y 5} c_{6} + o_{y 5} s_{6} & - n_{y 5} s_{6} + o_{y 5} c_{6} & a_{y 5} & y_{5} + a_{y 5} d_{6} \\ n_{z 5} c_{6} + o_{z 5} s_{6} & - n_{s 5} s_{6} + o_{z 5} c_{6} & a_{z 5} & z_{5} + a_{z 5} d_{6} \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} n_{x 6} & o_{x 6} & a_{x 6} & x_{6} \\ n_{y 6} & o_{y 6} & a_{y 6} & y_{6} \\ n_{z 6} & o_{z 6} & a_{z 6} & z_{6} \\ 0 & 0 & 0 & 1 \end{matrix}] & (12) \end{matrix}$

(44) When expanding formulae (8) to (10), formulae (13) to (15) can be obtained.

(45) $\begin{matrix} {}^{0}T_{2} = [\begin{matrix} c_{1} s_{2} & c_{1} c_{2} & - s_{1} & c_{1} (a_{1} + a_{2} s_{2}) - d_{2} s_{1} \\ s_{1} s_{2} & s_{1} c_{2} & c_{1} & s_{1} (a_{1} + a_{3} s_{2}) + d_{2} c_{1} \\ c_{2} & - s_{2} & 0 & d_{1} + a_{2} c_{2} \\ 0 & 0 & 0 & 1 \end{matrix}] & (13) \\ {}^{0}T_{3} = [\begin{matrix} c_{1} c_{23} & - s_{1} & c_{1} s_{23} & c_{1} (a_{1} + a_{2} s_{2} - a_{3} c_{23}) - s_{1} (d_{2} - d_{3}) \\ s_{1} c_{23} & c_{1} & s_{1} c_{23} & s_{1} (a_{1} + a_{2} s_{2} - a_{3} c_{23}) + c_{1} (d_{2} - d_{3}) \\ - s_{23} & 0 & c_{23} & d_{1} + a_{2} c_{2} + a_{3} s_{23} \\ 0 & 0 & 0 & 1 \end{matrix}] & (14) \\ {}^{0}T_{4} = [\begin{matrix} \begin{matrix} c_{1} c_{23} c_{4} - \\ s_{1} s_{4} \end{matrix} & - c_{1} s_{23} & \begin{matrix} - c_{1} c_{23} s_{4} - \\ s_{1} c_{4} \end{matrix} & \begin{matrix} c_{1} (\begin{matrix} a_{1} + a_{2} s_{2} - \\ a_{3} c_{23} + d_{4} s_{23} \end{matrix}) - \\ s_{1} (d_{2} - d_{3}) \end{matrix} \\ \begin{matrix} c_{1} s_{4} + \\ s_{1} c_{23} c_{4} \end{matrix} & - s_{1} s_{23} & \begin{matrix} c_{1} c_{4} - \\ s_{1} c_{23} s_{4} \end{matrix} & \begin{matrix} s_{1} (\begin{matrix} a_{1} + a_{2} s_{2} - \\ a_{3} c_{23} + d_{4} s_{23} \end{matrix}) + \\ c_{1} (d_{2} - d_{3}) \end{matrix} \\ - s_{23} c_{4} & - c_{23} & s_{23} s_{4} & d_{1} + a_{2} c_{2} + a_{3} s_{23} + d_{4} c_{23} \\ 0 & 0 & 0 & 1 \end{matrix}] & (15) \end{matrix}$

(46) Positional coordinates and normal, orientation, and approach vectors for each of the fifth and sixth axes are provided as follows:

(47) $\begin{matrix} [\begin{matrix} n_{x 5} \\ n_{y 5} \\ n_{z 5} \end{matrix}] = [\begin{matrix} c_{1} (c_{23} c_{4} c_{5} - s_{23} s_{5}) - s_{1} s_{4} c_{5} \\ s_{1} (c_{23} c_{4} c_{5} - s_{23} s_{5}) + c_{1} s_{4} c_{5} \\ - s_{23} c_{4} c_{5} - c_{23} s_{5} \end{matrix}] [\begin{matrix} o_{x 5} \\ o_{y 5} \\ o_{z 5} \end{matrix}] = [\begin{matrix} - c_{1} c_{23} s_{4} - s_{1} s_{4} \\ - s_{1} c_{23} s_{4} + c_{1} c_{4} \\ s_{23} s_{4} \end{matrix}] [\begin{matrix} a_{x 5} \\ a_{y 5} \\ a_{z 5} \end{matrix}] = [\begin{matrix} c_{1} (c_{23} c_{4} s_{5} + s_{23} s_{5}) - s_{1} s_{4} s_{5} \\ s_{1} (c_{23} c_{4} c_{5} + s_{23} c_{5}) + c_{1} s_{4} s_{5} \\ - s_{23} c_{4} c_{5} + c_{23} c_{5} \end{matrix}] & (16) \\ [\begin{matrix} x_{5} \\ y_{5} \\ z_{5} \end{matrix}] = [\begin{matrix} \begin{matrix} c_{1} (a_{1} + a_{2} s_{2} - (a_{3} - s_{4} d_{5}) c_{23} + d_{4} s_{23}) - \\ s_{1} (d_{2} - d_{3} - c_{4} d_{5}) \end{matrix} \\ \begin{matrix} s_{1} (a_{1} + a_{2} s_{2} - (a_{3} - s_{4} d_{5}) c_{23} + d_{4} s_{23}) + \\ c_{1} (d_{2} - d_{3} - c_{4} d_{5}) \end{matrix} \\ d_{1} + a_{2} c_{2} + (a_{3} - s_{4} d_{5}) s_{23} + d_{4} c_{23} \end{matrix}] [\begin{matrix} n_{x 6} \\ n_{y 6} \\ n_{z 6} \end{matrix}] = [\begin{matrix} c_{1} (c_{23} (c_{4} c_{5} c_{6} - s_{4} s_{6}) - s_{23} s_{5} c_{6}) - s_{1} (s_{4} c_{5} c_{6} + c_{4} s_{6}) \\ s_{1} (c_{23} (c_{4} c_{5} c_{6} - s_{4} s_{6}) - s_{23} s_{5} c_{6}) + c_{1} (s_{4} c_{5} c_{6} + c_{4} s_{6}) \\ s_{23} (- c_{4} c_{5} c_{6} + s_{4} s_{6}) - c_{23} s_{5} s_{6} \end{matrix}] & (17) \\ [\begin{matrix} o_{x 6} \\ o_{y 6} \\ o_{z 6} \end{matrix}] = [\begin{matrix} c_{1} (- c_{23} (c_{4} c_{5} s_{6} + s_{4} s_{6}) + s_{23} s_{5} s_{6}) + s_{1} (s_{4} c_{5} s_{6} - c_{4} c_{6}) \\ s_{1} (- c_{23} (c_{4} c_{5} s_{6} + s_{4} c_{6}) + s_{23} s_{5} s_{6}) - c_{1} (s_{4} c_{5} s_{6} - c_{4} c_{6}) \\ (c_{4} c_{5} s_{6} + s_{4} c_{6}) s_{23} + s_{5} s_{6} c_{23} \end{matrix}] [\begin{matrix} a_{x 6} \\ a_{y 6} \\ a_{z 6} \end{matrix}] = [\begin{matrix} c_{1} (c_{23} c_{4} s_{5} + s_{23} c_{5}) - s_{1} s_{4} c_{5} \\ s_{1} (c_{23} c_{4} s_{5} + s_{23} c_{5}) + c_{1} s_{4} s_{5} \\ - s_{23} c_{4} c_{5} + c_{23} c_{5} \end{matrix}] & (18) \\ [\begin{matrix} x_{6} \\ y_{6} \\ z_{6} \end{matrix}] = [\begin{matrix} \begin{matrix} \begin{matrix} c_{1} (a_{1} + a_{2} s_{2} - (a_{3} - s_{4} d_{5} - c_{4} s_{5} d_{6}) c_{23} + (d_{4} + c_{5} d_{6}) s_{23}) - \\ s_{1} (d_{2} - d_{3} - c_{4} d_{5} + s_{4} s_{5} d_{6}) \end{matrix} \\ \begin{matrix} s_{1} (a_{1} + a_{2} s_{2} - (a_{3} - s_{4} d_{5} - c_{4} s_{5} d_{6}) c_{23} + (d_{4} + c_{5} d_{6}) s_{23}) + \\ c_{1} (d_{2} - d_{3} - c_{4} d_{5} + s_{4} s_{5} d_{6}) \end{matrix} \end{matrix} \\ d_{1} + a_{2} c_{2} + (a_{3} - s_{4} d_{5} - c_{4} s_{5} d_{6}) s_{23} + (d_{4} + c_{5} d_{6}) c_{23} \end{matrix}] & (19) \end{matrix}$

(48) These matrixes are homogeneous transformation matrixes, so that these matrixes can be converted to a position vector in the base coordinate system .sub.0 by multiplying position vectors in the tool coordinate system by such matrixes from the left for the product. That is, based on the joint angles and the DH parameters, a tip position of the position vector in the base coordinate system can be obtained. The orientation angle of the position vector is expressed by normal, orientation, and approach vectors n6, o6 and a6 of the sixth axis.

(49) <Inverse Transform Process>

(50) An inverse transform process will now be explained. It is now assumed that the position and orientation of the end effector of the robot arm are given by a homogeneous transformation matrix. A position vector in the base coordinate system .sub.0 shown in FIG. 2 is denoted by p.sub.i. From an approach vector a.sub.6, a wrist position p.sub.5 is obtained based on:
p.sub.5=p.sub.6d.sub.6a.sub.6(20)

(51) A conventional 6-axis vertical articulated robot is structured such that rotation axes of the fourth, fifth, and sixth axes are mutually perpendicular at the wrist position. Hence, in this structure, the first, second, and third axes are first analyzed in relation to a wrist position, which enables all the joint angles to be obtained. Specifically, because the wrist position is on the respective rotation axes of the fourth, fifth, and sixth axes, the wrist position is not changed even if angles of one or more axes among the fourth, fifth, and sixth axes are changed. In contrast, in the 6-axis vertical articulated robot according to the present embodiment, a position p.sub.4 is changed due to changes in one or more axes among the fourth, fifth, and sixth axes, thus making the conventional approach impossible to obtain all the joint angles.

(52) With consideration of this situation, how to solve this problem will now be described based on an assumption that an angle of the sixth axis is provisionally given. If a rotation range of the sixth axis is for instance 180 degree to +180 degree, the joint angles are searched at angular step sizes properly set during the rotation range which is also called SINGLE. When the angle of the sixth axis is once decided, the position p.sub.4 can be obtained by using formulae (11) and (12).

(53) $\begin{matrix} \begin{matrix} p_{4} = a_{4} d_{6} + p_{6} \\ = o_{6} d_{6} + p_{6} \\ = (o_{6} s_{6} + o_{6} c_{6}) d_{6} + p_{6} \end{matrix} & (21) \end{matrix}$

(54) In this case, as shown in FIG. 4, a r coordinate system is set by rotating the xy axes by .sub.1, in which, however, since a link parameter a.sub.1 is a.sub.1=0, a formula for a.sub.10 becomes different for that for a.sub.1=0. The position p.sub.4 is decided, a formula which do not use the one-axis angle can express an coordinate. In addition, an arm form for .sub.140 is defined as LEFTY, while that for .sub.14<0 is defined as being RIGHTY. In a case .sub.14 shows an imaginary, the arm does not reach a target, thus providing no solution.
l.sub.14={square root over (x.sub.4.sup.2+y.sub.4.sup.2)}(22)
r.sub.14=d.sub.2d.sub.3(23)
.sub.14={square root over (l.sub.14.sup.2r.sub.14.sup.2)}(24)

(55) In formula (24), 114 shows a distance from the origin of the r coordinate system to p.sub.4. Accordingly, the trigonometric functions of the axis angles can be obtained as formulae (25) to (28).

(56) $\begin{matrix} x_{4} = - r_{14} s_{1} +_{14} c_{1} & (25) \\ y_{4} = r_{14} c_{1} +_{14} s_{1} & (26) \\ s_{1} = - \frac{r_{14} x_{4} -_{14} y_{4}}{r_{14}^{2} +_{14}^{2}} = - \frac{r_{14} x_{4} -_{14} y_{4}}{l_{14}^{2}} & (27) \\ c_{1} = \frac{r_{14} y_{4} +_{14} x_{4}}{r_{14}^{2} +_{14}^{2}} = \frac{r_{14} y_{4} +_{14} x_{4}}{l_{14}^{2}} & (28) \end{matrix}$

(57) As shown in FIG. 5, an z plane which views a side of the robot 2 is introduced. There are two sets of the second and third axis angles which meets the position p.sub.4. Formulae (29) and (30) are established based on an arm length from p.sub.1 to p.sub.2 and an arm length from p.sub.2 to p.sub.4.
.sub.12.sup.2+z.sub.12.sup.2=.sub.2.sup.2(29)
.sub.24.sup.2+z.sub.24.sup.2=.sub.3.sup.2+d.sub.4.sup.2(30)

(58) The following amounts can be decided in advance.
z.sub.14=z.sub.4d.sub.1(31)
m.sub.14={square root over (.sub.14.sup.2+z.sub.14.sup.2)}(32)

(59) By expanding formula (30) and being combined with formula (29), the position p.sub.2 can be obtained.

(60) $\begin{matrix} {(_{14} -_{12})}^{2} + {(z_{14} - z_{12})}^{2} = a_{3}^{2} + d_{4}^{2} & (33) \\ _{14}^{2} - 2_{12}_{14} + z_{14}^{2} - 2 z_{12} z_{14} = a_{3}^{2} + d_{4}^{2} - a_{2}^{2} & (34) \\ z_{12} = \frac{_{14}^{2} - 2_{12}_{14} + z_{14}^{2} - a_{3}^{2} - d_{4}^{2} + a_{2}^{2}}{2 z_{14}} & (35) \\ _{12}^{2} + {(\frac{_{14}^{2} - 2_{12}_{14} + z_{14}^{2} - a_{3}^{2} - d_{4}^{2} + a_{2}^{2}}{2 z_{14}})}^{2} = a_{2}^{2} & (36) \\ 4 z_{14}^{2}_{12}^{2} + {(- 2_{14}_{12} +_{14}^{2} + z_{14}^{2} - a_{3}^{2} - d_{4}^{2} + a_{2}^{2})}^{2} = 4 z_{14}^{2} a_{2}^{2} & (37) \\ {\begin{matrix} 4 (z_{14}^{2} +_{14}^{2})_{12}^{2} - 4_{14} (_{14}^{2} + z_{14}^{2} - a_{3}^{2} - d_{4}^{2} + a_{2}^{2})_{12} + \\ {(_{14}^{2} + z_{14}^{2} - a_{3}^{2} - d_{4}^{2} + a_{2}^{2})}^{2} - 4 z_{14}^{2} a_{2}^{2} \end{matrix}} = 0 & (38) \\ k_{1} = z_{14}^{2} - a_{3}^{2} - d_{4}^{2} + a_{2}^{2} & (39) \\ 4 m_{14}^{2}_{12}^{2} - 4_{14} k_{1}_{12} + k_{1}^{2} - 4 z_{14}^{2} a_{2}^{2} = 0 & (40) \\ _{12} = \frac{2_{14} k_{1} \sqrt{4_{14}^{2} k_{1}^{2} - 4 m_{14}^{2} (k_{1}^{2} - 4 z_{14}^{2} a_{2}^{2})}}{4 m_{14}^{2}} & (41) \\ 4_{14}^{2} k_{1}^{2} - 4 m_{14}^{2} (k_{1}^{2} - 4 z_{14}^{2} a_{2}^{2}) = 4 {(_{14}^{2} - z_{14}^{2}) k_{1}^{2} + 4 m_{14}^{2} z_{14}^{2} a_{2}^{2}} = 4 z_{14}^{2} (- k_{1}^{2} + 4 m_{14}^{2} a_{2}^{2}) & (42) \\ k_{2} = 4 m_{14}^{2} a_{2}^{2} - k_{1}^{2} & (43) \\ _{12} = \frac{_{14} k_{1} .Math. z_{14} .Math. \sqrt{k_{2}}}{2 m_{14}^{2}} & (44) \end{matrix}$

(61) For k2<0, the arm cannot reach a target, thus being no solution. Z.sub.12 can be obtained in the same manner as the foregoing.

(62) $\begin{matrix} _{12} = \frac{_{14}^{2} + z_{14}^{2} - 2 z_{12} z_{14} - a_{3}^{2} - d_{4}^{2} + a_{2}^{2}}{2_{14}} & (45) \\ {(\frac{_{14}^{2} + z_{14}^{2} - 2 z_{12} z_{14} - a_{3}^{2} - d_{4}^{2} + a_{2}^{2}}{2_{14}})}^{2} + z_{12}^{2} = a_{2}^{2} & (46) \\ {(- 2 z_{12} z_{14} +_{14}^{2} + z_{14}^{2} - a_{3}^{2} - d_{4}^{2} + a_{2}^{2})}^{2} + 4_{14}^{2} z_{12}^{2} = 4_{14}^{2} a_{2}^{2} & (47) \\ 4 m_{14}^{2} z_{12}^{2} - 4 z_{14} k_{1} z_{12} + k_{1}^{2} - 4_{1}^{2} a_{2}^{2} = 0 & (48) \\ z_{12} = \frac{2 z_{14} k_{1} \sqrt{4 z_{14}^{2} k_{1}^{2} - 4 m_{14}^{2} (k_{1}^{2} - 4_{14}^{2} a_{2}^{2})}}{4 m_{14}^{2}} & (49) \\ 4 z_{14}^{2} k_{1}^{2} - 4 m_{14}^{2} (k_{1}^{2} - 4_{14}^{2} a_{2}^{2}) = 4 {(z_{14}^{2} - z_{14}^{2}) k_{1}^{2} + 4 m_{14}^{2}_{14}^{2} a_{2}^{2}} = 4_{14}^{2} (- k_{1}^{2} + 4 m_{14}^{2} a_{2}^{2}) & (50) \\ z_{12} = \frac{z_{14} k_{1} .Math._{14} .Math. \sqrt{k_{2}}}{2 m_{14}^{2}} & (51) \end{matrix}$

(63) Inserting formulae (44) and (45) into formula (29) causes a condition indicating whether the equality sign is met in formula (52), whereby the following combinations of double-sign corresponds are provided with the signs and absolute value signs removed.

(64) $\begin{matrix} 2_{14} k_{1} .Math. z_{14} .Math. \sqrt{k_{2}} 2 z_{14} k_{1} .Math._{14} .Math. \sqrt{k_{2}} = 0 & (52) \\ _{12} = \frac{_{14} k_{1} z_{14} \sqrt{k_{2}}}{2 m_{14}^{2}} & (53) \\ z_{12} = \frac{z_{14} k_{1}_{14} \sqrt{k_{2}}}{2 m_{14}^{2}} & (54) \end{matrix}$

(65) Because the following formulas are realized based on FIG. 5, the trigonometric functions of the second and third axes can be obtained. When using a tan 2(y, x), the angles of the first, second and third axes can be obtained.

(66) $\begin{matrix} _{12} = a_{2} s_{2} & (55) \\ s_{2} = \frac{_{12}}{a_{2}} & (56) \\ z_{12} = a_{2} c_{2} & (57) \\ c_{2} = \frac{z_{12}}{a_{2}} & (58) \\ _{24} = - a_{3} c_{23} + d_{4} s_{23} & (59) \\ z_{24} = a_{3} s_{23} + d_{4} c_{23} & (60) \\ s_{23} = \frac{d_{4}_{24} + a_{3} z_{24}}{a_{3}^{2} + d_{4}^{2}} & (61) \\ c_{23} = \frac{- a_{3}_{24} + d_{4} z_{24}}{a_{3}^{2} + d_{4}^{2}} & (62) \\ _{i} = atan 2 (s_{i}, c_{i}) & (63) \end{matrix}$

(67) Next, forms of the arm, BELOW and ABOVE, are defined as shown in FIG. 6, the LEFTY provides ABOVE for .sub.3>3 and BELOW for .sub.3<3. Meanwhile, the RIGHTY provides BELOW for .sub.3>3 and ABOVE for .sub.3<3. The signs of z.sub.12 can be determined, and 3 can be expressed by
3=a tan 2(a.sub.3,d.sub.4)(64).

(68) As described, the angles of the first, second and third axes are decided, the head portion of the arm has provided p.sub.4, p.sub.5 and p.sub.6 which converge at p.sub.4. Hence, a.sub.3 and a.sub.4 should be perpendicular to each other and their inner product should be zero. As a result, when the sixth angle has an angle which gives zero to the following evaluation formula, such an angle meets the position and orientation of the arm head portion.

(69) 0 $\begin{matrix} \begin{matrix} e = a_{3} .Math. a_{4} \\ = [\begin{matrix} c_{1} s_{23} \\ s_{1} s_{23} \\ c_{23} \end{matrix}] .Math. (\frac{p_{4} - p_{5}}{d_{5}}) \end{matrix} & (65) \end{matrix}$

(70) This evaluation formula seldom shows zero due to the fact that the step size for searching the angle of the sixth axis is set discretely, so that interpolation is used to obtain a zero crossing point. Using the angle of the sixth axis which has been obtained at the zero crossing point, the angles of the first, second and third axes are re-calculated, and then, the remaining angles, i.e., the angles of the fourth and fifth axes are calculated. From the formula (18), s5 and c5 are calculated during which the denominator should not be zero.

(71) $\begin{matrix} n_{6} = [\begin{matrix} c_{1} (c_{23} (c_{4} c_{5} c_{6} - s_{4} s_{6}) - s_{23} s_{5} c_{5}) - s_{1} (s_{4} c_{5} c_{6} + c_{4} s_{6}) \\ s_{1} (c_{23} (c_{4} c_{5} c_{6} - s_{4} s_{6}) - s_{23} s_{5} c_{6}) + c_{1} (s_{4} c_{5} c_{6} + c_{4} s_{6}) \\ s_{23} (- c_{4} c_{5} c_{6} + s_{4} s_{6}) - c_{23} s_{5} c_{6} \end{matrix}] & (66) \\ c_{1} n_{x 6} + s_{1} n_{y 6} = c_{23} (c_{4} c_{5} c_{6} - s_{4} s_{6}) - s_{23} s_{5} c_{6} & (67) \\ s_{23} (c_{1} n_{x 6} + s_{1} n_{y 6}) + c_{23} n_{z 6} = - s_{5} c_{6} & (68) \\ s_{5} = - \frac{s_{23} (c_{1} n_{x 6} + s_{1} n_{y 6}) + c_{23} n_{z 6}}{c_{6}} & (69) \\ o_{6} = [\begin{matrix} c_{1} (- c_{23} (c_{4} c_{5} s_{6} + s_{4} c_{6}) + s_{23} s_{5} s_{6}) + s_{1} (s_{4} c_{5} s_{6} - c_{4} c_{6}) \\ s_{1} (- c_{23} (c_{4} c_{5} s_{6} + s_{4} c_{6}) + s_{23} s_{5} s_{6}) - c_{1} (s_{4} c_{5} s_{6} - c_{4} c_{6}) \\ (c_{4} c_{5} s_{6} + s_{4} c_{6}) s_{23} + s_{5} s_{6} c_{23} \end{matrix}] & (70) \\ c_{1} o_{x 6} + s_{1} o_{y 6} = - c_{23} (c_{4} c_{5} s_{6} + s_{4} c_{6}) + s_{23} s_{5} s_{6} & (71) \\ s_{23} (c_{1} o_{x 6} + s_{1} o_{y 6}) + c_{23} o_{x 6} = s_{5} s_{6} & (72) \\ s_{5} = \frac{s_{23} (c_{1} o_{x 6} + s_{1} o_{y 6}) + c_{23} o_{z 6}}{s_{6}} & (73) \\ a_{6} = [\begin{matrix} c_{1} (c_{23} c_{4} s_{5} + s_{23} c_{5}) - s_{1} s_{4} s_{5} \\ s_{1} (c_{23} c_{4} s_{5} + s_{23} c_{5}) + c_{1} s_{4} s_{5} \\ - s_{23} c_{4} s_{5} + c_{23} c_{5} \end{matrix}] & (74) \\ c_{1} a_{x 6} + s_{1} a_{y 6} = c_{23} c_{4} s_{5} + s_{23} c_{5} & (75) \\ c_{23} a_{z 6} + s_{23} (c_{1} a_{x 6} + s_{1} a_{y 6}) = c_{5} & (76) \\ c_{5} = c_{23} a_{z 6} + s_{23} (c_{1} a_{x 6} + s_{1} a_{y 6}) & (77) \end{matrix}$

(72) In the similar manner to the above, s4 and c4 are calculated.
cos .sub.1 n.sub.ysin .sub.1 n.sub.x=cos .sub.4 sin .sub.6+sin .sub.4 cos .sub.5 cos .sub.6(*5)
cos .sub.1 o.sub.ysin .sub.1 o.sub.x=cos .sub.4 cos .sub.6sin .sub.4 cos .sub.5 cos .sub.6(*6)
sin .sub.6 (cos .sub.1 n.sub.ysin .sub.1 n.sub.x)+cos .sub.6 (cos .sub.1 o.sub.ysin .sub.1 o.sub.x)=cos .sub.4(*7)
cos .sub.6 (cos .sub.1 n.sub.ysin .sub.1 n.sub.x)+sin .sub.6 (cos .sub.1 o.sub.ysin .sub.1 o.sub.x)=sin .sub.4 cos .sub.5(*8)

(73) From formulae (*7) and (*8), sin .sub.4 cos .sub.4 can be obtained, so that the angle .sub.4 can be calculated. An angle .sub.5 has already been obtained, the formula (*8) can be divided by cos .sub.5 or formulae (*9) and (*10) can be used instead.
cos .sub.1.sub.y6sin .sub.1.sub.x6=sin .sub.4 sin .sub.5(*9)
cos .sub.5 (sin .sub.4 cos .sub.5)+sin .sub.5 (sin .sub.4 sin .sub.5)=sin .sub.4(*10)

(74) The inverse transform process has been outlined above. The operations of the embodiment will now be described with reference to FIGS. 7 and 8. FIG. 7 shows a flowchart explaining the inverse transform process performed by the controller 3.

(75) First of all, a motion range of the sixth axis is calculated (step S1). FIG. 8 explains the motion range, in which it is assumed that d2=d3. In this calculation, the angle of the third axis J3 is set at a value obtained when the first arm 7 and the second arm 8 are extended to their maximum lengths, while the first axis J1 and the second axis J2 are changed in their angles and orientation to calculate a sphere A which can be reached by the fourth axis J4 and which is calculated from the base 4. The angle of the sixth axis J6 is than changed to calculate a sphere B which can be reached by the fourth axis P4 and which is calculated on the flange shown in FIG. 2.

(76) If the region occupied by the sphere B includes a part which is located outside the sphere A, the part expresses a range positioned outside the motion range (YES at step S2), which shows a failure of the inverse transform process. When the sphere B is included in the sphere A (NO at step S2), an initial value of the sixth axis angle, .sub.6INT is calculated so that this initial angle is provisionally decided as being 6A (step S3). The intimal value .sub.6INT is for example set to be the lowest angle value of the sixth axis which can be taken in its motion range. Alternatively, this initial angle value may be set to a current angle of the sixth angle. Still alternatively, if the calculated angle allows the fourth axis position to be located outside the motion range, the initial angle value may be set to an angle which allows the fourth axis position to be located inside the motion range and which is the closest to the calculated angle outside the motion range. Still alternatively, the foregoing initial angle value may be set to an angle which allows the fourth axis position to be at the center of the sphere B.

(77) In a case of d2d3, the following motion range inside/outside determination is performed. When the step S2 determines that the part of the sphere B is located outside the sphere A, a value calculated in the root part in the formula (24) or (44) becomes negative. If correction is made to make the determination result in inside the motion range, the formula (24) is changed to:
l.sub.14.sup.2=r.sub.14.sup.2
Assuming that L=|r.sub.14||l.sub.14|, it is sufficient to correct the position by an amount of:
w=L/|l.sub.14|,[wx.sub.4 wy.sub.4 0].sup.T

(78) In the case of the formula (44),

(79) $k_{2} = 4 m_{14}^{2} a_{2}^{2} - k_{1}^{2} = - m_{14}^{4} + 2 (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) m_{14}^{2} - {(a_{2}^{2} - a_{3}^{2} - d_{4}^{2})}^{2}$ $k_{2} = 0 is set with :$ $\begin{matrix} m_{140}^{2} = (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) \sqrt{{(a_{2}^{2} + a_{3}^{2} + d_{4}^{2})}^{2} - {(a_{2}^{2} - a_{3}^{2} - d_{4}^{2})}^{2}} \\ = (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) \sqrt{{a_{2}^{2} + (a_{3}^{2} + d_{4}^{2})}^{2} - {a_{2}^{2} - (a_{3}^{2} + d_{4}^{2})}^{2}} \\ = (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) \\ \sqrt{{a_{2}^{4} + 2 a_{2}^{2} (a_{3}^{2} + d_{4}^{2}) + {(a_{3}^{2} + d_{4}^{2})}^{4}} - {a_{2}^{4} - 2 a_{2}^{2} (a_{3}^{2} + d_{4}^{2}) + {(a_{3}^{2} + d_{4}^{2})}^{4}}} \\ = (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}} \end{matrix}$

(80) In case of:

(81) $m_{14}^{2} > (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) + 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}}, as$ $L = .Math. m_{14} .Math. - \sqrt{(a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) + 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}}}$

(82) L is moved to approach the origin in the z plane. If the correction has been already made in the formula (24), this correction makes the formula (24) unestablished again. In such a case, only the z-axis is corrected under k.sub.2=0. Further, in case of:

(83) $m_{14}^{2} < (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) - 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}}, as$ $L = \sqrt{(a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) - 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}}} - .Math. m_{14} .Math.$

(84) L is moved away from the origin in the z plane.

(85) Then, an amount of zero inputted into a variable n for deciding a wrist form (step S4), and a provisional target position PA is calculated (step S5). When a position vector [x,y,z]T is denoted by p, the provisional target position P.sub.A is calculated based on the formula (21) such that:
p+(n sin .sub.6A+o cos .sub.6A)d.sub.5
A homogeneous transformation matrix T.sub.A is decided by a rotation matrix R and a target position p.sub.A. In the present case, the provisional target position p.sub.6A has a relationship to the target position P.sub.6 such that;
p.sub.6A=p.sub.6+(n.sub.6 sin .sub.6A+o.sub.6 cos .sub.6A)d.sub.5
In other words, a value which can be obtained by subtracting an offset distance d.sub.5 from the target position p.sub.6 becomes the provisional target position P.sub.6A.

(86) The variable n is used to, as will be described later, decide which is the correct number of the wrist form setting for the inverse transform process, in which the variable n is numbered to wrist forms which are sorted according to an order of amounts of possible angles of the sixth axis.

(87) Then, based on the calculated target position P.sub.A, the inverse transform process is performed (step S6). In this case, a function g()=(T, F) is for calculating a homogeneous transformation matrix T and a form F of the robot 2 from an angle =[.sub.1, .sub.2, .sub.3, .sub.4, .sub.5, .sub.6].sup.T under the condition of d.sub.5=0. Hence, an inverse function g.sup.1(T, F) is for inverse-calculating the angle based the homogeneous transformation matrix T and the form F of the robot 2 under the condition of d.sub.5=0.

(88) An amount v which estimates a result of the inverse transform based on an evaluation function Q(, .sub.6A) is then calculated (step S7). The evaluation function is for example an inner production which can be calculated based on the foregoing formula (65). Furthermore, it is determined whether or not the calculated evaluation amount v has an absolute value which is equal to or less than a threshold v.sub.M which is set close to zero (step S8). If such an absolute value is over the threshold v.sub.M (NO at step S8), the angle of the sixth axis, .sub.6A, is updated by adding (>0, by way of example (step S11). It is further determined whether or not the updated angle .sub.6A enables the sixth axis to be located within the motion range (step S12). If this determination shows within the motion range (YES at step S12), the process is returned to step S5, while the determination reveals outside the motion range (NO at step S12), it can be estimated that the inverse transform process results in failure.

(89) Practically, the angle .sub.6A is changed to search for all of the possible angles of the sixth axis. In addition, the initial angle value .sub.6INT may be set to the highest value among all the possible angles of the sixth axis during which the sixth axis exists within the motion range. In such an alternative example, the angle can be updated by subtracting . An alternative update method can be provided such that:
.sub.6A.sub.6A(.sub.6.sub.6A),0<1

(90) Still alternatively, the values and/or may be changed every time the update calculation is repeated. Still, an optimization technique such as a golden section method or a simplex method may be applied to the update process.

(91) As shown in FIG. 9, since the robot according to the present embodiment has the third offset arm 8 which provides d50, a single position of the end effector is realized any one of various wrist forms of up to four. Hence, the number of angles of the sixth axis which meets the evaluation at step S9 is also four which is a maximum. For this reason, the absolute value of the evaluation value v at step S8 becomes equal to less than the threshold value v.sub.M (YES at step S8), the variable n is incremented (step S9). It is then determined whether or not the value of the variable n equals to the previously designated N-th angle of the sixth axis (step S10). If n=N is established (YES at step S10), the inverse transform process is ended, while if there is no determination of n=N (i.e., nN) (NO at step S10), the process will proceed to step S11. The case where it has been determined YES at step S8 reveals that the angle .sub.6 of the sixth axis, which has been subjected to the inverse transform, becomes approximately equal to the provisionally decided angle .sub.6A.

(92) Accordingly, in the present embodiment, the controller 3 is configured to control the industrial robot 2 provided with the vertically articulated 6-axis arm, in which the third offset arm 8 with the fifth axis is provided to mutually connect the fourth and sixth axes and to have a link length d5, resulting in that the shaft centers of the fourth and sixth axes become parallel to each other. The control point is set at the end effector, which is the head portion of the arm, and the inverse transform process is applied to a position and an orientation of the arm, to which the control point is targeted. The position and the orientation of the arm are then subjected to the inverse transform process so as to calculate the angles of the respective axes.

(93) Practically, a provisionally decided angle .sub.6A of the sixth arm is introduced, in which the direction of the offset arm 8 is calculated from the angle of the sixth arm and a provisional target position P.sub.6A is calculated by subtracting the link length d.sub.5 from a target position of the sixth arm. An amount zero is given to the link length d.sub.5 for a provisional target position P.sub.A, before being subjected to the inverse transform process. By evaluating a result of the inverse transform process based on the evaluation function, the axis .sub.6 of the sixth axis is calculated. A difference between this sixth-axis angle .sub.6 and the provisionally decided sixth-axis angle .sub.6A is compared with a predetermined value. Until when the difference becomes equal to or smaller than the predetermined value, the foregoing steps starting from the provisional angle decision for the sixth axis is repeated. By this configuration, the robot 2 having the offset arm 8 of a link length d.sub.5 can undergo the inverse transform process.

(94) In addition, the controller 3 uses, as the evaluation function, an inner product of a.Math.a4. In a state where the inverse transform process is applied effectively, the approach vectors a3 and a4 are perpendicular to each other, so that the inner product is zero. Hence, this evaluation function can used to properly evaluate results of the inverse transform process.

(95) Concerning the angles of the sixth axis, the wrist forms are numbered in the order of angle amounts at the same positions of the end effector, which enables the controller 3 to decide a wrist form based on results of the inverse transform process. That is, the robot provided with the offset arm of which link length d5 is given, maximally, four wrist forms for the same end-effector position. With consideration of this, of the plurality of wrist forms, the angles of any of the fourth to sixth axes, which are directed to wrist forms to be employed, are subjected to the previous numbering in a descendent or ascending order. If an angle of the axis, which results from the inverse transform, becomes equal to any of the previously numbered angles, the inverted transformed result is employed, whereby the wrist form can be decided uniquely.

Second Embodiment

(96) In a second embodiment, the equivalent components to those described in the first embodiment are given the reference numbers for a simplified explanation, thus explaining only different parts from the first embodiment.

(97) In the second embodiment, as to angles of the fourth or fifth axis, not to the sixth axis, the wrist forms are numbered in the order of angle amounts. Hence, as to angles of the k (=4, 5) axis, an N-th angle is designated. As shown in FIG. 10, when being YES at step S9, an angle which has resulted from the current inverse transform is stored in a memory or other storage means (step S13). If the determination at step S12 is NO, of the angles which have been stored so far, the k-axis angles are sorted in the order of their amounts (step S14). The N-th angle is then adopted as a solution (step S15).

(98) Accordingly, in the same manner as that of the first embodiment, the second embodiment allows the inverse transform process to decide a unique wrist from.

Third Embodiment

(99) In a third embodiment, a Jacobian matrix J is used to uniquely decide a wrist form calculated by the inverse transform process. In the third embodiment, the equivalent components to those described in the first embodiment are also given the reference numbers for a simplified explanation, thus explaining only different parts from the first embodiment.

(100) In the first embodiment, there has been explained such that the six-axis angles which meet the evaluation carried out at step S9 are four maximally in number. However, a point which divides those boundaries should be a singular point and a Jacobian matrix J at the singular point has a determinant of zero.

(101) Specifically, in addition to arm forms of RIGHTY and LEFTY and elbow forms ABOVE and BELOW, there are combinations of wrist forms FLIP+, FLIP, NONFLIP+, and NONFLIP. Hence, in a flowchart shown in FIG. 11, the determination YES at step S9 is followed by calculating the determinant based on the Jacobian matrix J (step S16). The Jacobian matrix J is expressed by the following formulae (87) to (99).

(102) $\begin{matrix} \frac{}{_{1}} [\begin{matrix} {Rx}_{6} \\ {Ry}_{6} \\ {Rz}_{6} \end{matrix}] = [\begin{matrix} 0 \\ 0 \\ 1 \end{matrix}] & (87) \\ \frac{}{_{2}} [\begin{matrix} {Rx}_{6} \\ {Ry}_{6} \\ {Rz}_{6} \end{matrix}] = [\begin{matrix} a_{x 1} \\ a_{y 1} \\ a_{z 1} \end{matrix}] = [\begin{matrix} - s_{1} \\ c_{1} \\ 0 \end{matrix}] & (88) \\ \frac{}{_{3}} [\begin{matrix} {Rx}_{6} \\ {Ry}_{6} \\ {Rz}_{6} \end{matrix}] = [\begin{matrix} a_{x 2} \\ a_{y 2} \\ a_{z 2} \end{matrix}] = [\begin{matrix} - s_{1} \\ c_{1} \\ 0 \end{matrix}] & (89) \\ \frac{}{_{4}} [\begin{matrix} {Rx}_{6} \\ {Ry}_{6} \\ {Rz}_{6} \end{matrix}] = [\begin{matrix} a_{x 3} \\ a_{y 3} \\ a_{z 3} \end{matrix}] = [\begin{matrix} c_{1} s_{23} \\ s_{1} s_{23} \\ c_{23} \end{matrix}] & (90) \\ \frac{}{_{5}} [\begin{matrix} {Rx}_{6} \\ {Ry}_{6} \\ {Rz}_{6} \end{matrix}] = [\begin{matrix} a_{x 4} \\ a_{y 4} \\ a_{z 4} \end{matrix}] = [\begin{matrix} - c_{1} c_{23} s_{4} - s_{1} c_{4} \\ c_{1} c_{4} - s_{1} c_{23} s_{4} \\ s_{23} s_{4} \end{matrix}] & (90) \\ \frac{}{_{6}} [\begin{matrix} {Rx}_{6} \\ {Ry}_{6} \\ {Rz}_{6} \end{matrix}] = [\begin{matrix} a_{x 4} \\ a_{y 4} \\ a_{z 4} \end{matrix}] = [\begin{matrix} c_{1} (c_{23} c_{4} c_{5} + c_{23} c_{5}) - s_{1} s_{4} s_{5} \\ s_{1} (c_{23} c_{4} c_{5} + c_{23} c_{5}) + s_{1} s_{4} s_{5} \\ c_{23} c_{5} - c_{23} c_{4} c_{5} \end{matrix}] & (92) \\ \begin{matrix} \frac{}{_{1}} [\begin{matrix} x_{6} \\ y_{6} \\ z_{6} \end{matrix}] = \frac{}{_{1}} [\begin{matrix} {Rx}_{6} \\ {Ry}_{6} \\ {Rz}_{6} \end{matrix}] {}^{0}P_{6} \\ = (\begin{matrix} - s_{1} (a_{1} + a_{2} s_{2} - (a_{3} - s_{4} d_{5} - c_{4} s_{5} d_{6}) c_{23} + \\ (d_{4} + c_{5} d_{6}) s_{23}) - c_{1} (d_{2} - d_{3} - c_{4} d_{5} + s_{4} s_{5} d_{6}) \\ c_{1} (a_{1} + a_{2} s_{2} - (a_{3} - s_{4} d_{5} - c_{4} s_{5} d_{6}) c_{23} + \\ (d_{4} + c_{5} d_{6}) s_{23}) - s_{1} (d_{2} - d_{3} - c_{4} d_{5} + s_{4} s_{5} d_{6}) \\ 0 \end{matrix}) \end{matrix} & (93) \\ \begin{matrix} \frac{}{_{2}} [\begin{matrix} x_{6} \\ y_{6} \\ z_{6} \end{matrix}] = \frac{}{_{2}} [\begin{matrix} {Rx}_{6} \\ {Ry}_{6} \\ {Rz}_{6} \end{matrix}] {}^{1}P_{6} \\ = [\begin{matrix} c_{1} (a_{2} c_{2} + (a_{3} - s_{4} d_{5} - c_{4} s_{5} d_{6}) s_{23} + (d_{4} + c_{5} d_{6}) c_{23}) \\ s_{1} (a_{2} c_{2} + (a_{3} - s_{4} d_{5} - c_{4} s_{5} d_{6}) s_{23} + (d_{4} + c_{5} d_{6}) c_{23}) \\ - a_{2} s_{s} + (a_{3} - s_{4} d_{5} - c_{4} s_{5} d_{6}) c_{23} - (d_{4} + c_{5} d_{6}) s_{23} \end{matrix}] \end{matrix} & (94) \\ \begin{matrix} \frac{}{_{3}} [\begin{matrix} x_{6} \\ y_{6} \\ z_{6} \end{matrix}] = \frac{}{_{3}} [\begin{matrix} {Rx}_{6} \\ {Ry}_{6} \\ {Rz}_{6} \end{matrix}] {}^{2}P_{6} \\ = [\begin{matrix} c_{1} ((a_{3} - s_{4} d_{5} - c_{4} s_{5} d_{6}) s_{23} + (d_{4} + c_{5} d_{6}) c_{23}) \\ s_{1} ((a_{3} - s_{4} d_{5} - c_{4} s_{5} d_{6}) s_{23} + (d_{4} + c_{5} d_{6}) c_{23}) \\ (a_{3} - s_{4} d_{5} - c_{4} s_{5} d_{6}) c_{23} - (d_{4} + c_{5} d_{6}) s_{23} \end{matrix}] \end{matrix} & (95) \\ \begin{matrix} \frac{}{_{4}} [\begin{matrix} x_{6} \\ y_{6} \\ z_{6} \end{matrix}] = \frac{}{_{4}} [\begin{matrix} {Rx}_{6} \\ {Ry}_{6} \\ {Rz}_{6} \end{matrix}] {}^{3}P_{6} \\ = [\begin{matrix} c_{1} c_{23} (c_{4} d_{5} - s_{4} s_{5} d_{6}) - s_{1} (s_{4} d_{5} + c_{4} s_{5} d_{6}) \\ s_{1} c_{23} (c_{4} d_{5} - s_{4} s_{5} d_{6}) + c_{1} (s_{4} d_{5} + c_{4} s_{5} d_{6}) \\ (- c_{4} d_{5} + s_{4} s_{5} d_{6}) s_{23} \end{matrix}] \end{matrix} & (96) \\ \begin{matrix} \frac{}{_{5}} [\begin{matrix} x_{6} \\ y_{6} \\ z_{6} \end{matrix}] = \frac{}{_{5}} [\begin{matrix} {Rx}_{6} \\ {Ry}_{6} \\ {Rz}_{6} \end{matrix}] {}^{4}P_{6} \\ = [\begin{matrix} c_{1} (c_{23} c_{4} c_{5} d_{6} - s_{23} s_{5} d_{6}) - s_{1} s_{4} c_{5} d_{6} \\ s_{1} (c_{23} c_{4} c_{5} d_{6} - s_{23} s_{5} d_{6}) + c_{1} s_{4} c_{5} d_{6} \\ - s_{23} c_{4} c_{5} d_{6} - c_{23} s_{5} d_{6} \end{matrix}] \end{matrix} & (97) \\ \begin{matrix} \frac{}{_{6}} [\begin{matrix} x_{6} \\ y_{6} \\ z_{6} \end{matrix}] = \frac{}{_{5}} [\begin{matrix} {Rx}_{6} \\ {Ry}_{6} \\ {Rz}_{6} \end{matrix}] {}^{5}P_{6} \\ = [\begin{matrix} 0 \\ 0 \\ 0 \end{matrix}] \end{matrix} & (98) \\ \begin{matrix} J = \frac{P}{} = [\frac{}{_{1}} [\begin{matrix} x \\ y \\ z \\ r_{x} \\ r_{y} \\ r_{z} \end{matrix}] \frac{}{_{2}} [\begin{matrix} x \\ y \\ z \\ r_{x} \\ r_{y} \\ r_{z} \end{matrix}] \frac{}{_{3}} [\begin{matrix} x \\ y \\ z \\ r_{x} \\ r_{y} \\ r_{z} \end{matrix}] \frac{}{_{4}} [\begin{matrix} x \\ y \\ z \\ r_{x} \\ r_{y} \\ r_{z} \end{matrix}] \frac{}{_{5}} [\begin{matrix} x \\ y \\ z \\ r_{x} \\ r_{y} \\ r_{z} \end{matrix}] \frac{}{_{6}} [\begin{matrix} x \\ y \\ z \\ r_{x} \\ r_{y} \\ r_{z} \end{matrix}]] \\ = [\begin{matrix} \frac{x}{_{1}} & \frac{x}{_{2}} & \frac{x}{_{3}} & \frac{x}{_{4}} & \frac{x}{_{5}} & \frac{x}{_{6}} \\ \frac{y}{_{1}} & \frac{y}{_{2}} & \frac{y}{_{3}} & \frac{y}{_{4}} & \frac{y}{_{5}} & \frac{y}{_{6}} \\ \frac{z}{_{1}} & \frac{z}{_{2}} & \frac{z}{_{3}} & \frac{z}{_{4}} & \frac{z}{_{5}} & \frac{z}{_{6}} \\ \frac{r_{x}}{_{1}} & \frac{r_{x}}{_{2}} & \frac{r_{x}}{_{3}} & \frac{r_{x}}{_{4}} & \frac{r_{x}}{_{5}} & \frac{r_{x}}{_{6}} \\ \frac{r_{y}}{_{1}} & \frac{r_{y}}{_{2}} & \frac{r_{y}}{_{3}} & \frac{r_{y}}{_{4}} & \frac{r_{y}}{_{5}} & \frac{r_{y}}{_{6}} \\ \frac{r_{z}}{_{1}} & \frac{r_{z}}{_{2}} & \frac{r_{z}}{_{3}} & \frac{r_{z}}{_{4}} & \frac{r_{z}}{_{5}} & \frac{r_{z}}{_{6}} \end{matrix}] \end{matrix} & (99) \end{matrix}$

(103) Then, it is determined whether or not signs of the calculated determinant agree with a designated wrist form.

(104) In this way, in the third embodiment, the controller 3 calculates the determinant on the Jacobian matrix J which is based on results of the inverse transform process. And, depending on signs of the determinant agree with a designated wrist form, the wrist form can be uniquely decided based on results of the inverse transform process.

(105) For instance, when each of the axes has a movable range of 180 degrees to +180 degrees, in a case where the fifth axis is moved to visually see the form of the wrist, it can be understood that there should be combinations of the two arm forms, the two elbow forms, and the four wrist forms, at a maximum number of combinations.

(106) The points respectively dividing the boundaries of these four wrist forms should be singular points, in which a Jacobian matrix has a determinant of zero at these singular points. As for each of the combinations of the respective forms, it is checked whether or not a wrist form resulting from the inverse transform process and signs of the determinant on the Jacobian matrix correspond to each other. This checking makes it possible to determine the wrist form agrees with a designated wrist form. That is, by considering combinations with a fourth-axis angle belonging to any of a range of 90 degrees to +90 degrees, a range of 180 degrees to 90 degrees, or a range of +90 degrees to +180 degrees, wrist forms, which is four in a maximal number, can be determined from each other.

(107) The present invention can be changed or developed as follows, not being limited to the embodiments described above.

(108) The inverse transform process produces results including not only the foregoing but also other modes. Such modes may include an evaluation of whether or not the forward transform of factors including d.sub.5, which followed the reverse transform, results in a positional error of zero.

(109) In addition, there may be an evaluation of whether or not a direction Z4A of the fifth axis obtained using the flange and a direction Z4 of the fifth axis obtained by the inverse transform agree with each other, i.e., an inner product thereof is equal to 1.

(110) Further, there may be an evaluation of whether or not the forgoing direction Z4A of the fifth axis and a direction Z3 of the fourth axis obtained by the inverse transform become perpendicular to each other, i.e., an inner product thereof is equal to 0.

(111) The foregoing repeated calculation may be conducted in a case where the motion range is divided into a plurality of motion ranges and each of the divided angle ranges is subjected to calculating an evaluation value.

(112) In the case where the motion range is divided into a plurality of motion ranges, an approximate function of the evaluation function may be calculated based on a calculated evaluation value in the vicinity of a solution obtained from the evaluation value, and the approximate function may be used to calculate a more definite angle.

Fourth Embodiment

(113) A fourth embodiment will now be descried.

(114) In the fourth embodiment, the equivalent components to those described in the first embodiment are also given the reference numbers for a simplified explanation, thus explaining only different parts from the first embodiment.

(115) The fourth embodiment differs from the first embodiment in a part of the foregoing inverse transform process. Practically, the processes which follow the foregoing formula (64) differ from those in the first embodiment.

(116) As shown in the foregoing FIG. 6, the formula (64) expresses the angle 3 which provides a LEFTY case producing the arm form ABOVE for .sub.3>3 and the arm form BELOW for .sub.3<3, and a RIGHTY case producing the arm form BELOW for .sub.3> and the arm form ABOVE for .sub.3<3.

(117) Then, angles .sub.5 and .sub.6 will now be obtained.
sin .sub.5 cos .sub.6=sin(.sub.2+.sub.3)(cos .sub.1 n.sub.x+sin .sub.1 n.sub.y)cos(.sub.2+.sub.3)n.sub.z(*1)
sin .sub.5 sin .sub.6=sin(.sub.2+.sub.3)(cos .sub.1 o.sub.x+sin .sub.2 o.sub.y)+cos(.sub.2+.sub.3)o.sub.z(*2)
sin.sup.2 .sub.5=sin.sup.2 .sub.5(sin.sup.2 .sub.6+cos.sup.2 .sub.6)=(sin .sub.5 sin .sub.6).sup.2+(sin .sub.5 cos .sub.6).sup.2(*3)
sin .sub.5={square root over (sin.sup.2 .sub.5)}={square root over ((sin .sub.5 sin .sub.6).sup.2+(sin .sub.5 cos .sub.6).sup.2)}(*4)

(118) From the formula (*4), the angle .sub.5 can be obtained. Additionally, from the formula (*4) and formulae (*1) and (*2), sin.sub.6 and cos .sub.6 are obtained, whereby the angle .sub.6 can be obtained.
cos .sub.1 n.sub.ysin .sub.1 n.sub.x=cos .sub.4 sin .sub.6+sin .sub.4 cos .sub.5 cos .sub.6(*5)
cos .sub.1 o.sub.ysin .sub.1 o.sub.x=cos .sub.4 cos .sub.6sin .sub.4 cos .sub.5 sin .sub.6(*6)
sin .sub.6 (cos .sub.1 n.sub.ysin .sub.1 n.sub.x)+cos .sub.6 (cos .sub.1 o.sub.ysin .sub.1 o.sub.x)=cos .sub.4(*7)
cos .sub.6 (cos .sub.1 n.sub.ysin .sub.1 n.sub.x)sin .sub.6 (cos .sub.1 o.sub.ysin .sub.1 o.sub.x)=sin .sub.4 cos .sub.5(*8)

(119) Moreover, the formulae (*7) and (*8) give sin .sub.4 and cos .sub.4, thereby giving the angle .sub.4. Incidentally, since the formula (*4) includes double signs of , a sign which meets conditions for the fourth-axis angle or the fifth-axis angle is employed. Alternatively to this method, with consideration that the angle .sub.5 is found first, the formula (*8) can be divided by cos .sub.5 or the formulae (*9) and (*10) may be used instead.
cos .sub.1 .sub.y6sin .sub.1 .sub.x6=sin .sub.4 sin .sub.5(*9)
cos .sub.5 (sin .sub.4 cos .sub.5)+sin .sub.5 (sin .sub.4 sin .sub.5)=sin .sub.4(*10)

(120) The inverse transform process according to the present embodiment has been outlined.

(121) With reference to FIGS. 12 and 13, the operations of the present embodiment will now be described. FIG. 12 is a flowchart showing the inverse transform process performed by the controller 3.

(122) First of all, when the position of the sixth axis is provisionally decided as being P6, the position P6 is used to provisionally decide the position of the fifth axis as being P5. These positions are then used to calculate a homogeneous transformation matrix T (step S21). It is then determined whether or not the provisional position P5 is within a motion range (step S22).

(123) FIG. 13 illustrates the motion range on the assumption of d2=d3. A sphere A having a radius of L1 and a sphere B having a radius of L1+L2 are set. The radiuses L1 and L2 are defined as the following formulae (*11) and (*12).

(124) $\begin{matrix} L_{1} = \sqrt{{(d_{2} - d_{3})}^{2} + {(a_{2} + \sqrt{a_{3}^{2} + d_{4}^{2}})}^{2}} & (^{*} 11) \\ L_{2} = \sqrt{d_{5}^{2} + d_{6}^{2}} & (^{*} 12) \end{matrix}$

(125) If the provisional position P5 is located outside the sphere A (step S22; outside the motion range), a distance L from a position defined by the homogeneous transformation matrix T to the boundary of the motion range (step S29). It is then determined whether or not the distance L is equal to or less than the link length d5 (step S30). This determination checks whether the provisional position P6 is within the motion range. When the distance L is over the link length d5 (NO at step S30), the process is terminated as being error.

(126) Meanwhile, when the distance L is equal to or less than the link length d5 (YES at step S30), correction is made to move the provisional position P5 into inside the sphere A (step S24). For example, this correction is made such that the position P5 is moved to an intersection at which a linear line connecting the positions P1 and P5 and the sphere A cross with each other. In response to movement of the position P5, the position P6 is also moved. As a modified process at step S30, L2=L5 is set to determine whether or not the position P5 is outside the sphere B.

(127) In addition, in the case of d2d3, the determination whether or not the position is inside or outside the motion range is carried out as follows. A case where it is determined that the position is outside the motion range at step S22 is limited to a case where the square root in the formula (24) or (44) provides a negative value. When it is desired to correct the position so as to be within the motion range, the case on the formula (24) is calculated such that:
l.sub.14.sup.2=r.sub.14.sup.2
Assuming that L=|lr.sub.14||l.sub.14|, the position is corrected by an amount of:
w=L/|l.sub.14|,[wx.sub.4 wy.sub.4 0].sup.T

(128) In addition, in the case on the formula (44):

(129) $k_{2} = 4 m_{14}^{2} a_{2}^{2} - k_{1}^{2} = - m_{14}^{4} + 2 (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) m_{14}^{2} - {(a_{2}^{2} - a_{3}^{2} - d_{4}^{2})}^{2}$ $k_{2} = 0 is set with :$ $\begin{matrix} m_{140}^{2} = (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) \sqrt{{(a_{2}^{2} + a_{3}^{2} + d_{4}^{2})}^{2} - {(a_{2}^{2} - a_{3}^{2} - d_{4}^{2})}^{2}} \\ = (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) \sqrt{{a_{2}^{2} + (a_{3}^{2} + d_{4}^{2})}^{2} - {a_{2}^{2} - (a_{3}^{2} + d_{4}^{2})}^{2}} \\ = (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) \\ \sqrt{\begin{matrix} {a_{2}^{4} + 2 a_{2}^{2} (a_{3}^{2} + d_{4}^{2}) + {(a_{3}^{2} + d_{4}^{2})}^{4}} - \\ {a_{2}^{4} - 2 a_{2}^{2} (a_{3}^{2} + d_{4}^{2}) + {(a_{3}^{2} + d_{4}^{2})}^{4}} \end{matrix}} \\ = (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}} \end{matrix}$

(130) In case of:

(131) $m_{14}^{2} > (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) + 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}}, as$ $L = .Math. m_{14} .Math. - \sqrt{(a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) + 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}}}$

(132) L is moved to approach the origin in the z plane. If the correction has already been made in the formula (24), this correction makes the formula (24) unestablished again. In such a case, only the z-axis is corrected under k.sub.2=0. Further, in case of:

(133) $m_{14}^{2} < (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) - 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}}, as$ $L = \sqrt{(a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) - 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}}} - .Math. m_{14} .Math.$

(134) L is moved away from the origin in the z plane.

(135) When it is determined at step S22 that the position is within the motion range and the step S34 is executed, the inverse transform process is then performed (step S23). In this process, g.sup.1(T, F) is a function for calculating an angle of =[.sub.1, .sub.2, .sub.3, .sub.4, .sub.5, .sub.6].sup.T based on the homogeneous transformation matrix T and the form F of the robot 2, which are given under the condition d5=0.

(136) At the next step, the angle resulting from the inverse transform is subjected to the forward transform process (step S24), which provides a homogeneous transformation matrix T and a form F. This homogeneous transformation matrix T includes, as its components, a rotation matrix R and a position matrix p.

(137) A difference between a target position p and the obtained position p is then obtained, and a position error vector p.sub.E is obtained. And the rotation matrix of the target position p is multiplied by an inverse vector of the rotation matrix R so as to obtain a rotation vector R.sub.E for the evaluation (step S25). It is then determined whether or not the position error vector p.sub.E has a norm p.sub.E which is equal to or less than a threshold P.sub.EM (step S26), and it is further determined whether or not the rotation matrix R.sub.E has an angle R.sub.E which is equal to or less than a threshold R.sub.EM (step S27). The terms p.sub.E and R.sub.E are defined by formulae (*13) and (*14) which are as follows:

(138) 0 $\begin{matrix} p_{E} = {[\begin{matrix} p_{Ex} & p_{Ey} & p_{Ez} \end{matrix}]}^{T} .Math. p_{E} .Math. = \sqrt{p_{Ex}^{2} + p_{Ey}^{2} + p_{Ez}^{2}} R_{E} = [\begin{matrix} n_{Ex} & o_{Ex} & a_{Ex} \\ n_{Ey} & o_{Ey} & a_{Ey} \\ n_{Ez} & o_{Ez} & a_{Ez} \end{matrix}] & (^{*} 13) \\ \begin{matrix} .Math. R_{E} .Math. = \sin^{- 1} \sqrt{\frac{{(o_{Ez} - a_{Ey})}^{2} + {(a_{Ex} - n_{Ex})}^{2} + {(n_{Ey} - o_{Ex})}^{2}}{2}} \\ = \cos^{- 1} \frac{n_{Ex} + o_{Ey} + a_{Ez} - 1}{2} \end{matrix} & (^{*} 14) \end{matrix}$

(139) In response to the determination of YES at step S27, it is further determined whether or not a wrist form decided by the position p and the rotation matrix R corresponds to a designated wrist form (step S28). When the designated form is obtained, the inverse transform process is ended. However, if the determination of NO is given at step S26 or S27, it is further determined the number of repeated execution times in which both the inverse transform process and the forward transform process have exceeded a preterminal number is determined (step S31). If the number of repeated execution times is equal to or over the predetermined number (YES at step S31), the process will be ended after countermeasures for an error are taken. In contrast, when the number of repeated execution times are not over the predetermined number (NO at step S31), the rotation matrix R is multiplied the evaluating rotation matrix R.sub.E and the position error vector p.sub.E is added to the position matrix p to update the homogeneous transformation matrix T (step S32). Then, similarly to that described at step S22, a determination for the motion range inside/outside is performed (step S33), thus proceeding to step S23 provided that the determination shows the inside of of a motion range, while proceeding to step S34 if the determination reveals the outside of the motion range.

(140) The, the determination at step S28 will now be detailed. As shown in FIG. 9 explained in the first embodiment, if there is provided a robot is provided with a third offset arm providing d50, like those in the present embodiment, four wrist forms are maximally given at a single end-effector position, so that the angles of the sixth axis, which meet the evaluation conditions at step S29, are also four in a maximal number thereof. With consideration of this, the step S28 uses a Jacobian matrix J in order to uniquely decide a wrist form among the wrist forms obtained via the inverse transform process. The sixth-axis angles, whose maximum number is four, have boundaries, in which points dividing the boundaries should be singular points and the Jacobian matrix J has a determinant of zero at the singular points.

(141) By combing the sings of the Jacobian matrix and the fourth-axis angle belonging to which of a range of 90 degrees to +90 degrees, a range of 180 degrees to 90 degrees, or a range of +90 degrees to +180 degrees, which are based on the convectional determining way, the forms of the wrist, which are FLIP+, FLIP, NONFLIP+, NONFLIP, which are thus four in a maximal number, can be determined from each other.

(142) Specifically, in addition to the arm forms of RIGHTY and LEFTY and the elbow forms of ABOVE and BELOW, there are provided combinations with the wrist forms FLIP+, FLIP, NONFLIP+, and NONFLIP. Hence, at step S28, the determinant of the Jacobian matrix J which is expressed by the formulae (87) to (99) is calculated.

(143) It is then checked whether or not the signs of the calculated determinant are in accordance with the designated wrist form.

(144) In the above-explained embodiment, the controller 3 controls the robot 2 provided with the 6-axis vertical articulated type of arm, in which the arm is provided with the third offset arm 8 which mutually connects the fourth and sixth axes and whose link has a length d5. The fifth axis is arranged at the third offset arm 8 and the shaft centers of both the fourth and sixth axes are parallel to each other. The controller 3 controls, as a control point, an end effector which is the head portion of the arm, in which a position and an orientation to which the control point is targeted are subjected to the inverse transform process to calculate the axes of the respective axes.

(145) In addition, the controller 3 provisionally decides the position P6 of the sixth axis, and based on its provisionally decided position P6, decides the position P5 of the fifth axis. And, by the controller 3, it is determined whether both positions P5 and P6 are within a motion range obtained based on the link parameters of the robot 2. If those positions P5 and P6 are within the motion range, a homogeneous transformation matrix obtained from those positions is then subjected to an inverse transform process provided that the link length d5 is set to be zero. The angles of the respective axes, which are from the inverse transform process, are then subjected to a forward transform process.

(146) Furthermore, the controller 3 obtains a position error vector p.sub.E which is a difference between a target position of the control point and a position obtained from the forward transform process. The controller 3 also obtains a rotation matrix R.sub.E by multiplying a rotation matrix based on the target position by an inverse matrix of the rotation matrix obtained by the forward transform process. When the position error vector p.sub.E has a norm which exceeds a threshold or the rotation matrix R.sub.E gives an angle which exceeds a threshold, a homogeneous transformation matrix in which the position error vector p.sub.E and the rotation matrix R.sub.E are reflected is repeatedly subjected to the processes starting from the inverse transform process. When the norm of the position error vector p.sub.E is equal to or less than its threshold and the the angle resulting from the rotation matrix R.sub.E is equal to or less than its threshold, the inverse transform process is completed.

(147) According, even in the present configuration of the robot 2 provided with the offset arm which mutually links the fourth and sixth axes and has a link length d5, it is possible to carry out the inverse transform process. Practically, by provisionally setting the sixth-axis angle, the inverse transform process can be applied. And matrixes p.sub.E and R.sub.E, which are obtained by applying a forward transform process to the inverse-transformed results, are estimated to make the processing converge. Hence, the inverse transform process can be performed for the angle calculation.

(148) Meanwhile, when the positions P5 and P6 are not within the motion range, those positions P5 and P6 are corrected to shift those positions to the inside of the motion range. The position of the control point, which is decided on the homogeneous transformation matrix T, also undergoes the same way as the foregoing. That is, the control point position is determined to be within the motion range or not, and if not within the motion range, the homogeneous transformation matrix T is corrected to make the control point position locate within the motion range. This enables the inverse transform process to be repeated continuously.

(149) The controller 3 is thus able to use results of the inverse transform process to calculate a determinant of the Jacobian matrix J. By checking whether or not the signs of the calculated determinant is in accord with a previously designated wrist form, the results of the inverse transform process can be used to uniquely decide a wrist form in accordance with the processed results, which provides the advantageous operations which similar to those gained in the first embodiment when the Jacobian matrix J is calculated for deciding the wrist form.

Fifth Embodiment

(150) A fifth embodiment will now be described.

(151) The fifth embodiment provides hardware configurations which are identical to those set forth in the first embodiment, in the same way as the fourth embodiment, so that the identical components will be given the same reference numbers. In the fifth embodiment, an inverse transform process is also carried out, a part of which explanations may be duplicated from that adopted in the fourth embodiment, but in order to focus an easier and connected explanation of the whole story, the contents of an inverse transform process performed by the controller 3 will be explained from the beginning.

(152) FIG. 14 is a flowchart showing how the inverse transform process is performed by the controller 3.

(153) First of all, when the position of the sixth axis is provisionally decided as being P6, the position P6 is used to provisionally decide the position of the fifth axis as being P5. These positions are then used to calculate a homogeneous transformation matrix T (step S41). It is then determined whether or not the provisional position P5 is within a motion range (step S42).

(154) The motion range is defined in the same manner as that shown in FIG. 7 in the first embodiment. The motion range is set on the assumption of d2=d3. A sphere A having a radius of L1 and a sphere B having a radius of L1+L2 are set. The radiuses L1 and L2 are defined as the following formulae (*11) and (*12).

(155) $\begin{matrix} L_{1} = \sqrt{{(d_{2} - d_{3})}^{2} + {(a_{2} + \sqrt{a_{3}^{2} + d_{4}^{2}})}^{2}} & (^{*} 11) \\ L_{2} = \sqrt{d_{5}^{2} + d_{6}^{2}} & (^{*} 12) \end{matrix}$

(156) If the provisional position P5 is located outside the sphere A (step S42; outside the motion range), a distance L from a position defined by the homogeneous transformation matrix T to the boundary of the motion range (step S49). It is then determined whether or not the distance L is equal to or less than the link length d5 (step S50). This determination checks whether the provisional position P6 is within the motion range. When the distance L is over the link length d5 (NO at step S50), the process is terminated as being error.

(157) Meanwhile, when the distance L is equal to or less than the link length d5 (YES at step S50), correction is made to move the provisional position P5 into inside the sphere A (step S51). For example, this correction is made such that the position P5 is moved to an intersection at which a linear line connecting the positions P1 and P5 and the sphere A cross with each other. In response to movement of the position P5, the position P6 is also moved. As a modified process at step S50, L2=L5 is set to determine whether or not the position P5 is outside the sphere B.

(158) In addition, in the case of d2d3, the determination whether or not the position is inside or outside the motion range is carried out as follows. A case where it is determined that the position is outside the motion range at step S42 is limited to a case where the square root in the formula (24) or (44) provides a negative value. When it is desired to correct the position so as to be within the motion range, the case on the formula (24) is calculated such that:
l.sub.14.sup.2=r.sub.14.sup.2

(159) Assuming that L=|r.sub.14||l.sub.14|, the position is corrected by an amount of:
w=L/|l.sub.14|,[wx.sub.4 wy.sub.4 0].sup.T

(160) In addition, in the case on the formula (44):

(161) $k_{2} = 4 m_{14}^{2} a_{2}^{2} - k_{1}^{2} = - m_{14}^{4} + 2 (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) m_{14}^{2} - {(a_{2}^{2} - a_{3}^{2} - d_{4}^{2})}^{2}$ $k_{2} = 0 is set with :$ $\begin{matrix} m_{140}^{2} = (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) \sqrt{{(a_{2}^{2} + a_{3}^{2} + d_{4}^{2})}^{2} - {(a_{2}^{2} - a_{3}^{2} - d_{4}^{2})}^{2}} \\ = (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) \sqrt{{a_{2}^{2} + (a_{3}^{2} + d_{4}^{2})}^{2} - {a_{2}^{2} - (a_{3}^{2} + d_{4}^{2})}^{2}} \\ = (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) \\ \sqrt{{a_{2}^{4} + 2 a_{2}^{2} (a_{3}^{2} + d_{4}^{2}) + {(a_{3}^{2} + d_{4}^{2})}^{4}} - {a_{2}^{4} - 2 a_{2}^{2} (a_{3}^{2} + d_{4}^{2}) + {(a_{3}^{2} + d_{4}^{2})}^{4}}} \\ = (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}} \end{matrix}$

(162) In case of:

(163) $m_{14}^{2} > (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) + 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}}, as$ $L = .Math. m_{14} .Math. - \sqrt{(a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) + 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}}}$

(164) L is moved to approach the origin in the z plane. If the correction has already been made in the formula (24), this correction makes the formula (24) unestablished again. In such a case, only the z-axis is corrected under k.sub.2=0. Further, in case of:

(165) $m_{14}^{2} < (a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) - 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}}, as$ $L = \sqrt{(a_{2}^{2} + a_{3}^{2} + d_{4}^{2}) - 2 a_{2} \sqrt{a_{3}^{2} + d_{4}^{2}}} - .Math. m_{14} .Math.$

(166) L is moved away from the origin in the z plane.

(167) When it is determined at step S42 that the position is within the motion range and the step S34 is executed, the inverse transform process is then performed (step S43). In this process, g.sup.1(T, F) is a function for calculating an angle of =[.sub.1, .sub.2, .sub.3, .sub.4, .sub.5, .sub.5, .sub.6].sup.T based on the homogeneous transformation matrix T and the form F of the robot 2, which are given under the condition d5=0.

(168) At the next step, the angle resulting from the inverse transform is subjected to the forward transform process (step S44), which provides a homogeneous transformation matrix T and a form F. This homogeneous transformation matrix T includes, as its components, a rotation matrix R and a position matrix p.

(169) A difference between a target position p and the obtained position p is then obtained, and a position error vector p.sub.E is obtained. And the rotation matrix of the target position p is multiplied by an inverse vector of the rotation matrix R so as to obtain a rotation vector R.sub.E for the evaluation (step S45). It is then determined whether or not the position error vector p.sub.E has a norm P.sub.E which is equal to or less than a threshold P.sub.EM (step S46), and it is further determined whether or not the rotation matrix R.sub.E has an angle R.sub.E which is equal to or less than a threshold R.sub.EM (step S47). The terms p.sub.E and R.sub.E are defined by formulae (*13) and (*14) which are as follows:

(170) $\begin{matrix} p_{E} = {[\begin{matrix} p_{Ex} & p_{Ey} & p_{Ez} \end{matrix}]}^{T}, .Math. p_{E} .Math. = \sqrt{p_{Ex}^{2} + p_{Ey}^{2} + p_{Ez}^{2}} R_{E} = [\begin{matrix} n_{Ex} & o_{Ex} & a_{Ex} \\ n_{Ey} & o_{Ey} & a_{Ey} \\ n_{Ez} & o_{Ez} & a_{Ez} \end{matrix}] & (^{*} 13) \\ \begin{matrix} .Math. R_{E} .Math. = \sin^{- 1} \sqrt{\frac{{(o_{Ez} - a_{Ey})}^{2} + {(a_{Ex} - n_{Ex})}^{2} + {(n_{Ey} - o_{Ex})}^{2}}{2}} \\ = \cos^{- 1} \frac{n_{Ex} + o_{Ey} + a_{Ez} - 1}{2} \end{matrix} & (^{*} 14) \end{matrix}$

(171) In response to the determination of YES at step S47, it is further determined whether or not a wrist form decided by the position p and the rotation matrix R corresponds to a designated wrist form (step S48). When the designated form is obtained, the inverse transform process is ended. Meanwhile, when it is determined NO at step S46 or S47, it is further determined the number of repeated execution times of both the inverse transform process and the forward transform process has exceeded a preterminal number (step S52). If the number of repeated execution times is equal to or over the predetermined number (YES at step S52), the process will be ended after countermeasure for an error are taken.

(172) In contrast, when the number of repeated execution times are not over the predetermined number (NO at step S52), a rotation axis vector q.sub.E is calculated based on the normal, orientation, and approach vectors which are the elements of the rotation matrix R.sub.E (step S53).

(173) $\begin{matrix} q_{E} = {[\begin{matrix} \frac{o_{Ez} - a_{Ey}}{2} & \frac{a_{Ex} - n_{Ex}}{2} & \frac{n_{Ey} - o_{Ex}}{2} \end{matrix}]}^{T} & (^{*} 15) \end{matrix}$

(174) A rotation vector r.sub.E, which is a product of the angle R.sub.E of the rotation matrix R.sub.E and a unit rotation axis vector q.sub.E/q.sub.E, is calculated (step S54).

(175) $\begin{matrix} r_{E} = \frac{\sin^{- 1} .Math. q_{E} .Math.}{.Math. q_{E} .Math.} q_{E} & (^{*} 16) \end{matrix}$

(176) A position matrix P.sub.E, which is calculated on the position matrix p.sub.E and the rotation vector r.sub.E, is multiplied by an inverse matrix of Jacobian matrix J calculated based on the angels of the respective axes, which results from the inverse transform process. Using the multiplied results, the angles are updated (step S55), before proceeding to step S44. The Jacobian matrix J is expressed by the formulae (87) to (99).

(177) The, the determination at step S48 will now be detailed. As shown in FIG. 9, if there is provided a robot is provided with a third offset arm providing d50, like those in the present embodiment, four wrist forms are maximally given at a single end-effector position, so that the angles of the sixth axis, which meet the evaluation conditions at step S49, are also four in a maximal number thereof. With consideration of this, the step S48 uses a Jacobian matrix J in order to uniquely decide a wrist form among the wrist forms obtained via the inverse transform process. The sixth-axis angles, whose maximum number is four, have boundaries, in which points dividing the boundaries should be singular points and the Jacobian matrix J has a determinant of zero at the singular points.

(178) By combing the sis of the Jacobian matrix and the fourth-axis angle belonging to which of a range of 90 degrees to +90 degrees, a range of 180 degrees to 90 degrees, or a range of +90 degrees to +180 degrees, which are based on the convectional determining way, the forms of the wrist, which are FLIP+, FLIP, NONFLIP+, NONFLIP, which are thus four in a maximal number, can be determined from each other.

(179) Specifically, in addition to the arm forms of RIGHTY and LEFTY and the elbow forms of ABOVE and BELOW, there are provided combinations with the wrist forms FLIP+, FLIP, NONFLIP+, and NONFLIP. Hence, at step S48, a determinant of a Jacobian matrix J is calculated.

(180) It is then checked whether or not the signs of the calculated determinant are in accordance with the designated wrist form.

(181) In the above-explained embodiment, in the same manner described before, the controller 3 controls the robot 2 provided with the 6-axis vertical articulated type of arm, in which the arm is provided with the third offset arm 8 which mutually connects the fourth and sixth axes and whose link has a length d5. The fifth axis is arranged at the third offset arm 8 and the shaft centers of both the fourth and sixth axes are parallel to each other. The controller 3 controls, as a control point, an end effector which is the head portion of the arm, in which a position and an orientation to which the control point is targeted are subjected to the inverse transform process to calculate the axes of the respective axes.

(182) In addition, the controller 3 provisionally decide the position P6 of the sixth axis, and based on its provisionally decided position P6, decides the position P5 of the fifth axis. And, by the controller 3, it is determined whether both positions P5 and P6 are within a motion range obtained based on the link parameters of the robot 2. If those positions P5 and P6 are within the motion range, a homogeneous transformation matrix obtained those positions is then subjected to an inverse transform process provided that the link length d5 is set to be zero. The angles of the respective axes, which are from the inverse transform process, are then subjected to a forward transform process.

(183) Furthermore, the controller 3 obtains a position error vector p.sub.E which is a difference between a target position of the control point and a position obtained from the forward transform process. The controller 3 also obtains a rotation matrix R.sub.E by multiplying a rotation matrix based on the target position by an inverse matrix of the rotation matrix obtained by the forward transform process. When the position error vector p.sub.E has a norm p.sub.E which exceeds a threshold or the rotation matrix R.sub.E gives an angle R.sub.E which exceeds a threshold, a rotation axis vector q.sub.E is calculated based on the normal, orientation and approach vectors which are elements of the rotation matrix R.sub.E, and a rotation vector r.sub.E, which is a product between the angle R.sub.E and a unit rotary axis vector q.sub.E/q.sub.E, is then calculated. A position matrix P.sub.E is calculated based on the position matrix p.sub.E and the rotation vector r.sub.E. The angles are updated based on results obtained by multiplying the position matrix P.sub.E by an inverse matrix on the Jacobian matrix J. this inverse matrix is obtained from the angles of the respective axes, which are given by the inverse transform process. This update step is followed by repeated processing starting from the foregoing forward inverse transform.

(184) In the same manner as the previous embodiment, even in the present configuration of the robot 2 provided with the offset arm which mutually links the fourth and sixth axes and has a link length d5, it is possible to carry out the inverse transform process. Practically, by provisionally setting the sixth-axis angle, the inverse transform process can be applied. And matrixes p.sub.E and R.sub.E, which are obtained by applying a forward transform process to the inverse-transformed results, are estimated to make the processing converge. Hence, the inverse transform process can be performed for the angle calculation.

(185) Meanwhile, also in the same manner as that in the previous embodiment, when the positions P5 and P6 are not within the motion range, those positions P5 and P6 are corrected to shift those positions to the inside of the motion range. The position of the control point, which is decided on the homogeneous transformation matrix T, also undergoes the same way as the foregoing. That is, the control point position is determined to be within the motion range or not, and if not within the motion range, the homogeneous transformation matrix T is corrected to make the control point position locate within the motion range. This enables the inverse transform process to be repeated continuously.

(186) In the fifth embodiment, the same or similar advantageous operations as or to those gained in relation to calculating the wrist form based on the Jacobian matrix J in the first embodiment can be provided as well.

(187) The foregoing embodiments may adopt a controller 3 installed in various parts of of the robot 2, which are other than the base 4.

PARTIAL REFERENCE SIGNS LIST

(188) In the drawings:

(189) 1 robot system

(190) 2 robot

(191) 3 controller

(192) 4 base

(193) 5 shoulder

(194) 6 second offset arm

(195) 7 first arm

(196) 8 third offset arm

(197) 9 second arm

(198) 10 fifth offset arm

(199) 11 wrist

(200) 12 hand

Controller for robot and inverse transforming method for robot

Assignee

Inventors

Cpc classification

Classification Explorer

B25J9/1607

PERFORMING OPERATIONS; TRANSPORTING

Classification Explorer

B25J9/106

PERFORMING OPERATIONS; TRANSPORTING

Classification Explorer

B25J9/1664

PERFORMING OPERATIONS; TRANSPORTING

Classification Explorer

B25J9/16

PERFORMING OPERATIONS; TRANSPORTING

Classification Explorer

B25J13/00

PERFORMING OPERATIONS; TRANSPORTING

Classification Explorer

G05B2219/39079

PHYSICS

Classification Explorer

B25J9/06

PERFORMING OPERATIONS; TRANSPORTING

International classification

Classification Explorer

B25J9/16

PERFORMING OPERATIONS; TRANSPORTING

Classification Explorer

B25J13/00

PERFORMING OPERATIONS; TRANSPORTING

Classification Explorer

B25J9/06

PERFORMING OPERATIONS; TRANSPORTING

Classification Explorer

B25J9/10

PERFORMING OPERATIONS; TRANSPORTING

Abstract

Claims

Description