Controller for robot and inverse transforming method for robot

This application is based on and claims the benefit of priority from earlier Japanese Patent Applications No. 2017-225909 filed Nov. 24, 2017; No. 2017-225910 filed Nov. 24, 2017; and No. 2017-225911 filed Nov. 24, 2017, the descriptions of which are incorporated herein by reference.

The present invention relates to a controller controlling operations of a robot, such as an industrial robot, which has an offset arm connecting the fourth and sixth axes of the robot and a method of inverse-transforming the position of an end effector of the robot.

A generally used vertical 6-axis type robot arm is provided with the fourth to sixth axes whose axial centers cross at a one point to each other.

In this robot arm, the fifth axis is rotated to displace the wrist portion including the sixth axis, which will cause a gap in a portion holding the wrist in the arm end portion. Such a gap may accidentally hold foreign matter. With consideration of such a case, by way of example, a Japanese design patent No. 1583755 is provided, in which an industrial robot has an offset arm mutually linking the fourth and sixth axes, whereby there is no gap in the portion holding the wrist. In this structure, the offset arm avoids the axial centers of both of the fourth and sixth axes from crossing to each other and permits both axial centers to be parallel with each other.

In this kind of arm structure, if it is assumed to perform an inverse-transform process in order to obtain the angles of the respective axes based on the position and orientation of the end effector, the conventional technique cannot be applied. One example of such inverse-transform processes is provided by PTL 1, in which it is possible to perform the inverse-transform process based on DH parameters even if robot assembling is associated with errors. In the technique disclosed in PTL 1, when an offset arm has a length of d 5 , a minute region corresponding to the errors is assumed and linearized on an assumption of d 5 =0, so that the convergence calculation is carried out.

Parent Literature

[PTL 1] JP-A 2017-61022

When assuming that the technique disclosed in PTL 1 is applied to the robot having d 5 ≠0, the value of d 5 , which is an initial error in convergence calculation, becomes larger, which results in a large number of repetition times of the calculation or a possibility of divergence in the calculation without convergence. In addition, since there is an offset component, the offset component cannot be reflected correctly in a range even originally reached by the arm, which may be erroneously detected as not being within the reach.

With consideration of the foregoing situations, it is desired to a a robot controller and an inverse transform processing method for robots, in which an inverse transform process can be performed even when the robot is equipped with an offset arm.

In a first exemplary embodiment, a controller is 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 d 5 , 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.

In this configuration, an angular provisional deciding provisionally decides an angle of a sixth angle, a direction calculating unit calculates a direction of the offset arm based on the provisionally decided angle of the sixth axis, and a provisional target position calculating unit calculates a provisional target position which is subtracted from a target position of the sixth axis by the link length d 5 . An inverse transform processing unit applies an inverse transform process to the provisional target position in a state where the link length d 5 is set to 0. Then an estimation unit repeats 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.

According to this configuration, provisionally setting the angle of the sixth axis makes it possible that a robot, such as an industrial robot equipped with an offset arm whose link length is d 5 and which connects the fourth and sixth axes, can be subjected to the inverse transform process.

In a second exemplary embodiment, a controller can be targeted to a robot equipped with a vertical articulated 6-axis arm having the foregoing arm structure.

In the control performed by the controller, by way of example, a position provisional deciding unit provisionally decides a position P 6 ′ of the sixth axis and provisionally decides a position P 5 ′ of the fifth axis based on the provisionally decided position P 6 ′. A motion range determining unit determines whether or not the provisionally decided positions P 5 ′ and P 6 ′ are within a motion range defined based on link parameters of the robot. An inverse transform processing unit performs an inverse transform process of a homogeneous transformation matrix based on the positions P 5 ′ and P 6 ′ in a case where the provisionally decided positions P 5 ′ and P 6 ′ are within the motion range, the inverse transform process being performed under a condition of the link length d 5 =0. A forward transform processing unit performs a forward transform process based on angles of the respective axes obtained by the inverse transform process.

In addition, an evaluation value calculating unit calculates a position matrix p 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 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. A calculation repeating unit repeatedly calculates processes starting from the inverse transform process, based on a homogeneous transformation matrix in which the position matrix p E and the rotation matrix R E are reflected, when the position matrix p E has a norm exceeding a threshold thereof or the rotation matrix R E has an angle exceeding a threshold thereof.

That is, in cases where the norm of the position becomes equal to or less than the threshold thereof and the angle calculated on the rotation matrix R E becomes equal to or less than the threshold, the inverse transform process will be ended. Accordingly, in a robot equipped with an offset arm having a link length d 5 and connecting the fourth and sixth axes, the inverse transform process can be performed by provisionally setting the angle of the sixth axis. Results from this inverse transform process are then subjected to a forward transform process to gain both a matrix p E and a matrix R E . These matrixes are then evaluated and, using evaluated results, calculated to be converged in the calculation, so that the inverse transform process can be used for obtaining the axis angles.

The controller according to the second exemplary embodiment can also be applied to a robot controller according to a third embodiment. Practically, a position provisional deciding unit provisionally decides a position P 6 ′ of the sixth axis and provisionally decides a position P 5 ′ of the fifth axis based on the provisionally decided position P 6 ′. A motion range determining unit determined whether or not the provisionally decided positions P 5 ′ and P 6 ′ are within a motion range defined based on link parameters of the robot. Then, an inverse transform processing unit performs an inverse transform process of a homogeneous transformation matrix based on the positions P 5 ′ and P 6 ′ in a case where the provisionally decided positions P 5 ′ and P 6 ′ are within the motion range, the inverse transform process being performed under a condition of the link length d 5 =0. Moreover, a forward transform processing unit performs a forward transform process based on angles of the respective axes obtained by the inverse transform process.

Further, an evaluation value calculating unit calculates a position matrix p E of a difference between a target position of the control point and a position resulting from the forward transform process, and calculates a rotation matrix R 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. A calculation repeating unit calculates a rotation axis vector q E of the rotation matrix R E when the position matrix p E has a norm ∥p E ∥ exceeding a threshold thereof or the rotation matrix RE provides an angle ∥R E ∥ exceeding a threshold thereof. The calculation repeating unit also calculates a rotation vector r E defined as a product between the angle ∥R E ∥ and a unit rotation axis vector q E /∥q E ∥. Moreover, the calculation repeating unit updates the angles of the respective axes based on results obtained by multiplying a position matrix P E defined by the position matrix p E and the rotation vector r E , by an inverse matrix of the Jacobian matrix obtained from the angels of the respective axes calculated by the inverse transform process. The processes starting from the forward transform process is repeated.

In this configuration of a robot having an offset arm to link the fourth and sixth axes to each other and configured to have a link length d 5 , it is also possible to perform the inverse transform process by provisionally setting the angle of the sixth axis, performing the inverse transform process, and reportedly estimating/calculating a matrix p E and a matrix R E , which are obtained from transform-processed results by the forward transform process, such that the evaluation calculation can be converged.

The processes performed by the controllers according to the foregoing exemplary embodiments can also be practiced in the form of an inverse transform processing, thus providing the similar or equivalent advantageous operations to those described.

In the accompanying drawings:

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

FIG. 2 is a diagram showing a robot coordinate system;

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

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

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

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

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

FIG. 8 is a view explain a motion range in a case of d 2 =d 3 ;

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

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

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

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

FIG. 13 is an illustration explaining a motion range provided when d 2 =d 3 in the fourth embodiment; and

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

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

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

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.

On the base 4 , there is provided a first axis J 1 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 J 2 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 J 3 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 9 a and a head portion 9 b

The second arm 9 has a fourth axis J 4 having an x-directional shaft center, in which the head portion 9 b is twistably coupled with the base portion 9 a . The head portion 9 b of the second arm 9 is provided with a fifth axis J 5 having a y-directional shaft center, and a wrist 11 is coupled with the head portion 9 b 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 J 6 having the x-directional shaft center. At the respective axes J 1 to J 6 of the robot 2 , an electric motor (not shown) is provided which serves as a drive source.

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 J 1 to J 6 , detects a motor rotating position, and based on this detection, drive of the motor at each axis is controlled by, for example, feedback control.

Practically, the controller includes the CPU (central processing unit) 3 A, a ROM (read-only memory) 3 B, a RAM (random access memory) 3 C, drive circuits 3 D, position detecting circuits 3 E, 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 3 F and connected to the drive circuits 3 D and the position detecting circuits 3 E via an interface 3 G. In the present embodiment, a display unit 3 G and an input unit 3 H are connected to the internal bus 3 F. These components are known, except for an inverse transform process performed by the CPU 3 A.

The ROM 3 B is provided to previously store therein system programs and operation programs for the robot 2 . The RAM 3 B 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 3 B, 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 3 E 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 3 D drive the motors at the respective axes J 1 to J 6 , under the control of the controller 3 .

The controller 3 (precisely, the CPU 3 A) 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 3 E.

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.

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

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

The reference symbols d 1 to d 6 and a 2 , a 3 shown in FIG. 2 are defined as follows:

d 1 : a link length from the origin of the 0-th coordinate system Σ 0 to the origin of the first coordinate system Σ 1 ,

d 2 : a link length from the origin of the first coordinate system Σ 1 to the base portion of the first arm 7 ,

a 2 : 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 Σ 2 ,

d 3 : a link length from the origin of the second coordinate system Σ 2 to the head portion of the third offset arm 8 ,

a 3 : a distance between the shaft centers of the third and fourth axes J 3 and J 4 ,

d 4 : a link length from the origin of the third coordinate system Σ 3 to the origin of the fourth coordinate system Σ 4 ,

d 5 : a link length from the origin of the fourth coordinate system Σ 4 to the origin of the fifth coordinate system Σ 5 , and

d 6 : a link length from the origin of the fifth coordinate system Σ 5 to the origin of the sixth coordinate system Σ 6 .

The distance a 3 is illustrated in FIG. 3 for reference. The link length d 5 corresponds to the link length of the fifth offset arm 10 .

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.

<Forward Transform Process>

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. θ i indicates a rotation angle of each of the joints which rotates from an angular state shown in FIG. 2 .

Homogeneous transformation matrixes expressing a base coordinate system Σ 0 to a mechanical interface coordinate system Σ 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 θ i and cos θ i are denoted as s i and c i , respectively. In addition, for example, S 23 denotes sin(θ 2 +θ 3 ).

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

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

These matrixes are homogeneous transformation matrixes, so that these matrixes can be converted to a position vector in the base coordinate system Σ 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 n 6 , o 6 and a 6 of the sixth axis.

<Inverse Transform Process>

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 Σ 0 shown in FIG. 2 is denoted by p i . From an approach vector a 6 , a wrist position p 5 is obtained based on:

p 5 =p 6 −d 6 a 6   (20)

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 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.

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 4 can be obtained by using formulae (11) and (12).

In this case, as shown in FIG. 4 , a αr coordinate system is set by rotating the xy axes by θ 1 , in which, however, since a link parameter a 1 is a 1 =0, a formula for a 1 ≠0 becomes different for that for a 1 =0. The position p 4 is decided, a formula which do not use the one-axis angle can express an α coordinate. In addition, an arm form for α 14 ≥0 is defined as LEFTY, while that for α 14 <0 is defined as being RIGHTY. In a case α 14 shows an imaginary, the arm does not reach a target, thus providing no solution.

l 14 =√{square root over ( x 4 2 +y 4 2 )}  (22)

r 14 =d 2 −d 3   (23)

α 14 =±√{square root over ( l 14 2 −r 14 2 )}  (24)

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

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 4 . Formulae (29) and (30) are established based on an arm length from p 1 to p 2 and an arm length from p 2 to p 4 .

α 12 2 +z 12 2 =α 2 2   (29)

α 24 2 +z 24 2 =α 3 2 +d 4 2   (30)

The following amounts can be decided in advance.

z 14 =z 4 −d 1   (31)

m 14 =√{square root over (α 14 2 +z 14 2 )}  (32)

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

For k 2 <0, the arm cannot reach a target, thus being no solution. Z 12 can be obtained in the same manner as the foregoing.

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.

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.

Next, forms of the arm, BELOW and ABOVE, are defined as shown in FIG. 6 , the LEFTY provides ABOVE for θ 3 >φ 3 and BELOW for θ 3 <φ 3 . Meanwhile, the RIGHTY provides BELOW for θ 3 >φ 3 and ABOVE for Θ 3 <φ 3 . The signs of z 12 can be determined, and φ 3 can be expressed by

φ3 =a tan 2( a 3 ,d 4 )  (64).

As described, the angles of the first, second and third axes are decided, the head portion of the arm has provided p 4 , p 5 and p 6 which converge at p 4 . Hence, a 3 and a 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.

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), s 5 and c 5 are calculated during which the denominator should not be zero.

In the similar manner to the above, s 4 and c 4 are calculated.

cos θ 1 n y −sin θ 1 n x =cos θ 4 sin θ 6 +sin θ 4 cos θ 5 cos θ 6   (*5)

cos θ 1 o y −sin θ 1 o x =cos θ 4 cos θ 6 −sin θ 4 cos θ 5 cos θ 6   (*6)

sin θ 6 (cos θ 1 n y −sin θ 1 n x )+cos θ 6 (cos θ 1 o y −sin θ 1 o x )=cos θ 4   (*7)

cos θ 6 (cos θ 1 n y −sin θ 1 n x )+sin θ 6 (cos θ 1 o y −sin θ 1 o x )=sin θ 4 cos θ 5   (*8)

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

cos θ 1 α y6 −sin θ 1 α x6 =sin θ 4 sin θ 5   (*9)

cos θ 5 (sin θ 4 cos θ 5 )+sin θ 5 (sin θ 4 sin θ 5 )=sin θ 4   (*10)

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 .

First of all, a motion range of the sixth axis is calculated (step S 1 ). FIG. 8 explains the motion range, in which it is assumed that d 2 =d 3 . In this calculation, the angle of the third axis J 3 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 J 1 and the second axis J 2 are changed in their angles and orientation to calculate a sphere A which can be reached by the fourth axis J 4 and which is calculated from the base 4 . The angle of the sixth axis J 6 is than changed to calculate a sphere B which can be reached by the fourth axis P 4 and which is calculated on the flange shown in FIG. 2 .

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 S 2 ), which shows a failure of the inverse transform process. When the sphere B is included in the sphere A (NO at step S 2 ), an initial value of the sixth axis angle, θ 6INT is calculated so that this initial angle is provisionally decided as being θ 6 A (step S 3 ). The intimal value θ 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.

In a case of d 2 ≠d 3 , the following motion range inside/outside determination is performed. When the step S 2 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 14 2 =r 14 2

Assuming that L=|r 14 |−|l 14 |, it is sufficient to correct the position by an amount of:

w=L/|l 14 |,[ wx 4 wy 4 0] T

In the case of the formula (44),

In case of:

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 2 =0. Further, in case of:

L is moved away from the origin in the αz plane.

Then, an amount of zero inputted into a variable n for deciding a wrist form (step S 4 ), and a provisional target position PA is calculated (step S 5 ). When a position vector [x,y,z]T is denoted by p, the provisional target position P A is calculated based on the formula (21) such that:

p +( n sin θ 6A +o cos θ 6A ) d 5

A homogeneous transformation matrix T A is decided by a rotation matrix R and a target position p A . In the present case, the provisional target position p 6A has a relationship to the target position P 6 such that;

p 6A =p 6 +( n 6 sin θ 6A +o 6 cos θ 6A ) d 5

In other words, a value which can be obtained by subtracting an offset distance d 5 from the target position p 6 becomes the provisional target position P 6A .

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.

Then, based on the calculated target position P A , the inverse transform process is performed (step S 6 ). 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 θ=[θ 1 , θ 2 , θ 3 , θ 4 , θ 5 , θ 6 ] T under the condition of d 5 =0. Hence, an inverse function g −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 5 =0.

An amount v which estimates a result of the inverse transform based on an evaluation function Q(θ, θ 6A ) is then calculated (step S 7 ). 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 M which is set close to zero (step S 8 ). If such an absolute value is over the threshold v M (NO at step S 8 ), the angle of the sixth axis, θ 6A , is updated by adding β(>0, by way of example (step S 11 ). It is further determined whether or not the updated angle θ 6A enables the sixth axis to be located within the motion range (step S 12 ). If this determination shows “within the motion range” (YES at step S 12 ), the process is returned to step S 5 , while the determination reveals “outside the motion range” (NO at step S 12 ), it can be estimated that the inverse transform process results in failure.

Practically, the angle θ 6A is changed to search for all of the possible angles of the sixth axis. In addition, the initial angle value θ 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:

θ 6A ←θ 6A α(θ 6 −θ 6A ),0<α≤1

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.

As shown in FIG. 9 , since the robot according to the present embodiment has the third offset arm 8 which provides d 5 ≠0, 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 S 9 is also four which is a maximum. For this reason, the absolute value of the evaluation value v at step S 8 becomes equal to less than the threshold value v M (YES at step S 8 ), the variable n is incremented (step S 9 ). 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 S 10 ). If n=N is established (YES at step S 10 ), the inverse transform process is ended, while if there is no determination of n=N (i.e., n≠N) (NO at step S 10 ), the process will proceed to step S 11 . The case where it has been determined YES at step S 8 reveals that the angle θ 6 of the sixth axis, which has been subjected to the inverse transform, becomes approximately equal to the provisionally decided angle θ 6A .

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 d 5 , 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.

Practically, a provisionally decided angle θ 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 6A is calculated by subtracting the link length d 5 from a target position of the sixth arm. An amount zero is given to the link length d 5 for a provisional target position P 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 θ 6 of the sixth axis is calculated. A difference between this sixth-axis angle θ 6 and the provisionally decided sixth-axis angle θ 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 5 can undergo the inverse transform process.

In addition, the controller 3 uses, as the evaluation function, an inner product of “a·a 4 ”. In a state where the inverse transform process is applied effectively, the approach vectors a 3 and a 4 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.

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 d 5 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

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.

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 S 9 , an angle θ which has resulted from the current inverse transform is stored in a memory or other storage means (step S 13 ). If the determination at step S 12 is NO, of the angles θ which have been stored so far, the k-axis angles are sorted in the order of their amounts (step S 14 ). The N-th angle is then adopted as a solution (step S 15 ).

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

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.

In the first embodiment, there has been explained such that the six-axis angles which meet the evaluation carried out at step S 9 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.

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 S 9 is followed by calculating the determinant based on the Jacobian matrix J (step S 16 ). The Jacobian matrix J is expressed by the following formulae (87) to (99).

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

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.

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.

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.

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

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 5 , which followed the reverse transform, results in a positional error of zero.

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

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

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.

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

A fourth embodiment will now be descried.

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.

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.

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 θ 3 >φ 3 and the arm form BELOW for Θ 3 <φ 3 , and a RIGHTY case producing the arm form BELOW for θ 3 >φ and the arm form ABOVE for θ 3 <φ 3 .

Then, angles θ 5 and θ 6 will now be obtained.

sin θ 5 cos θ 6 =−sin(θ 2 +θ 3 )(cos θ 1 n x +sin θ 1 n y )−cos(θ 2 +θ 3 ) n z   (*1)

sin θ 5 sin θ 6 =sin(θ 2 +θ 3 )(cos θ 1 o x +sin θ 2 o y )+cos(θ 2 +θ 3 ) o z   (*2)

sin 2 θ 5 =sin 2 θ 5 (sin 2 θ 6 +cos 2 θ 6 )=(sin θ 5 sin θ 6 ) 2 +(sin θ 5 cos θ 6 ) 2   (*3)

sin θ 5 =±√{square root over (sin 2 θ 5 )}=±√{square root over ((sin θ 5 sin θ 6 ) 2 +(sin θ 5 cos θ 6 ) 2 )}  (*4)

From the formula (*4), the angle θ 5 can be obtained. Additionally, from the formula (*4) and formulae (*1) and (*2), sinθ 6 and cos θ 6 are obtained, whereby the angle θ 6 can be obtained.

cos θ 1 n y −sin θ 1 n x =cos θ 4 sin θ 6 +sin θ 4 cos θ 5 cos θ 6   (*5)

cos θ 1 o y −sin θ 1 o x =cos θ 4 cos θ 6 −sin θ 4 cos θ 5 sin θ 6   (*6)

sin θ 6 (cos θ 1 n y −sin θ 1 n x )+cos θ 6 (cos θ 1 o y −sin θ 1 o x )=cos θ 4   (*7)

cos θ 6 (cos θ 1 n y −sin θ 1 n x )−sin θ 6 (cos θ 1 o y −sin θ 1 o x )=sin θ 4 cos θ 5   (*8)

Moreover, the formulae (*7) and (*8) give sin θ 4 and cos θ 4 , thereby giving the angle θ 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 θ 5 is found first, the formula (*8) can be divided by cos θ 5 or the formulae (*9) and (*10) may be used instead.

cos θ 1 α y6 −sin θ 1 α x6 =sin θ 4 sin θ 5   (*9)

cos θ 5 (sin θ 4 cos θ 5 )+sin θ 5 (sin θ 4 sin θ 5 )=sin θ 4   (*10)

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

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 .

First of all, when the position of the sixth axis is provisionally decided as being P 6 ′, the position P 6 ′ is used to provisionally decide the position of the fifth axis as being P 5 ′. These positions are then used to calculate a homogeneous transformation matrix T′ (step S 21 ). It is then determined whether or not the provisional position P 5 ′ is within a motion range (step S 22 ).

FIG. 13 illustrates the motion range on the assumption of d 2 =d 3 . A sphere A having a radius of L 1 and a sphere B having a radius of L 1 +L 2 are set. The radiuses L 1 and L 2 are defined as the following formulae (*11) and (*12).

If the provisional position P 5 ′ is located outside the sphere A (step S 22 ; 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 S 29 ). It is then determined whether or not the distance L is equal to or less than the link length d 5 (step S 30 ). This determination checks whether the provisional position P 6 ′ is within the motion range. When the distance L is over the link length d 5 (NO at step S 30 ), the process is terminated as being error.

Meanwhile, when the distance L is equal to or less than the link length d 5 (YES at step S 30 ), correction is made to move the provisional position P 5 ′ into inside the sphere A (step S 24 ). For example, this correction is made such that the position P 5 ′ is moved to an intersection at which a linear line connecting the positions P 1 and P 5 ′ and the sphere A cross with each other. In response to movement of the position P 5 ′, the position P 6 ′ is also moved. As a modified process at step S 30 , L 2 =L 5 is set to determine whether or not the position P 5 ′ is outside the sphere B.

In addition, in the case of d 2 ≠d 3 , 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 S 22 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 14 2 =r 14 2

Assuming that L=|lr 14 |−|l 14 |, the position is corrected by an amount of:

w=L/|l 14 |,[ wx 4 wy 4 0] T

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

In case of:

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 2 =0. Further, in case of:

L is moved away from the origin in the αz plane.

When it is determined at step S 22 that the position is within the motion range and the step S 34 is executed, the inverse transform process is then performed (step S 23 ). In this process, g −1 (T′, F) is a function for calculating an angle of θ′=[θ 1 ′, θ 2 ′, θ 3 ′, θ 4 ′, θ 5 ′, θ 6 ′] T based on the homogeneous transformation matrix T′ and the form F of the robot 2 , which are given under the condition d 5 =0.

At the next step, the angle θ′ resulting from the inverse transform is subjected to the forward transform process (step S 24 ), 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″.

A difference between a target position p and the obtained position p″ is then obtained, and a position error vector p 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 E for the evaluation (step S 25 ). It is then determined whether or not the position error vector p E has a norm ∥p E ∥ which is equal to or less than a threshold P EM (step S 26 ), and it is further determined whether or not the rotation matrix R E has an angle ∥R E ∥ which is equal to or less than a threshold R EM (step S 27 ). The terms ∥p E ∥ and ∥R E ∥ are defined by formulae (*13) and (*14) which are as follows:

In response to the determination of YES at step S 27 , 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 S 28 ). When the designated form is obtained, the inverse transform process is ended. However, if the determination of NO is given at step S 26 or S 27 , 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 S 31 ). If the number of repeated execution times is equal to or over the predetermined number (YES at step S 31 ), 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 S 31 ), the rotation matrix R′ is multiplied the evaluating rotation matrix R E and the position error vector p E is added to the position matrix p′ to update the homogeneous transformation matrix T′ (step S 32 ). Then, similarly to that described at step S 22 , a determination for the motion range inside/outside is performed (step S 33 ), thus proceeding to step S 23 provided that the determination shows the inside of of a motion range, while proceeding to step S 34 if the determination reveals the outside of the motion range.

The, the determination at step S 28 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 d 5 ≠0, 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 S 29 , are also four in a maximal number thereof. With consideration of this, the step S 28 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.

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.

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 S 28 , the determinant of the Jacobian matrix J which is expressed by the formulae (87) to (99) is calculated.

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

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 d 5 . 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.

In addition, the controller 3 provisionally decides the position P 6 ′ of the sixth axis, and based on its provisionally decided position P 6 ′, decides the position P 5 ′ of the fifth axis. And, by the controller 3 , it is determined whether both positions P 5 ′ and P 6 ′ are within a motion range obtained based on the link parameters of the robot 2 . If those positions P 5 ′ and P 6 ′ 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 d 5 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.

Furthermore, the controller 3 obtains a position error vector p 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 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 E has a norm which exceeds a threshold or the rotation matrix R E gives an angle which exceeds a threshold, a homogeneous transformation matrix in which the position error vector p E and the rotation matrix R E are reflected is repeatedly subjected to the processes starting from the inverse transform process. When the norm of the position error vector p E is equal to or less than its threshold and the the angle resulting from the rotation matrix R E is equal to or less than its threshold, the inverse transform process is completed.

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 d 5 , 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 E and R 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.

Meanwhile, when the positions P 5 ′ and P 6 ′ are not within the motion range, those positions P 5 ′ and P 6 ′ 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.

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

A fifth embodiment will now be described.

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.

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

First of all, when the position of the sixth axis is provisionally decided as being P 6 ′, the position P 6 ′ is used to provisionally decide the position of the fifth axis as being P 5 ′. These positions are then used to calculate a homogeneous transformation matrix T′ (step S 41 ). It is then determined whether or not the provisional position P 5 ′ is within a motion range (step S 42 ).

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 d 2 =d 3 . A sphere A having a radius of L 1 and a sphere B having a radius of L 1 +L 2 are set. The radiuses L 1 and L 2 are defined as the following formulae (*11) and (*12).

If the provisional position P 5 ′ is located outside the sphere A (step S 42 ; 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 S 49 ). It is then determined whether or not the distance L is equal to or less than the link length d 5 (step S 50 ). This determination checks whether the provisional position P 6 ′ is within the motion range. When the distance L is over the link length d 5 (NO at step S 50 ), the process is terminated as being error.

Meanwhile, when the distance L is equal to or less than the link length d 5 (YES at step S 50 ), correction is made to move the provisional position P 5 ′ into inside the sphere A (step S 51 ). For example, this correction is made such that the position P 5 ′ is moved to an intersection at which a linear line connecting the positions P 1 and P 5 ′ and the sphere A cross with each other. In response to movement of the position P 5 ′, the position P 6 ′ is also moved. As a modified process at step S 50 , L 2 =L 5 is set to determine whether or not the position P 5 ′ is outside the sphere B.

In addition, in the case of d 2 ≠d 3 , 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 S 42 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 14 2 =r 14 2

Assuming that L=|r 14 |−|l 14 |, the position is corrected by an amount of:

w=L/|l 14 |,[ wx 4 wy 4 0] T

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

In case of:

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 2 =0. Further, in case of:

L is moved away from the origin in the αz plane.

When it is determined at step S 42 that the position is within the motion range and the step S 34 is executed, the inverse transform process is then performed (step S 43 ). In this process, g −1 (T′, F) is a function for calculating an angle of θ′=[θ 1 ′, θ 2 ′, θ 3 ′, θ 4 ′, θ 5 ′, θ 5 ′, θ 6 ′] T based on the homogeneous transformation matrix T′ and the form F of the robot 2 , which are given under the condition d 5 =0.

At the next step, the angle θ′ resulting from the inverse transform is subjected to the forward transform process (step S 44 ), 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″.

A difference between a target position p and the obtained position p″ is then obtained, and a position error vector p 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 E for the evaluation (step S 45 ). It is then determined whether or not the position error vector p E has a norm ∥P E ∥ which is equal to or less than a threshold P EM (step S 46 ), and it is further determined whether or not the rotation matrix R E has an angle ∥R E ∥ which is equal to or less than a threshold R EM (step S 47 ). The terms ∥p E ∥ and ∥R E ∥ are defined by formulae (*13) and (*14) which are as follows:

In response to the determination of YES at step S 47 , 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 S 48 ). When the designated form is obtained, the inverse transform process is ended. Meanwhile, when it is determined NO at step S 46 or S 47 , 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 S 52 ). If the number of repeated execution times is equal to or over the predetermined number (YES at step S 52 ), the process will be ended after countermeasure for an error are taken.

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

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

A position matrix P E , which is calculated on the position matrix p E and the rotation vector r 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 S 55 ), before proceeding to step S 44 . The Jacobian matrix J is expressed by the formulae (87) to (99).

The, the determination at step S 48 will now be detailed. As shown in FIG. 9 , if there is provided a robot is provided with a third offset arm providing d 5 ≠0, 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 S 49 , are also four in a maximal number thereof. With consideration of this, the step S 48 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.

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.

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 S 48 , a determinant of a Jacobian matrix J is calculated.

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

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 d 5 . 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.

In addition, the controller 3 provisionally decide the position P 6 ′ of the sixth axis, and based on its provisionally decided position P 6 ′, decides the position P 5 ′ of the fifth axis. And, by the controller 3 , it is determined whether both positions P 5 ′ and P 6 ′ are within a motion range obtained based on the link parameters of the robot 2 . If those positions P 5 ′ and P 6 ′ 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 d 5 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.

Furthermore, the controller 3 obtains a position error vector p 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 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 E has a norm ∥p E ∥ which exceeds a threshold or the rotation matrix R E gives an angle ∥R E ∥ which exceeds a threshold, a rotation axis vector q E is calculated based on the normal, orientation and approach vectors which are elements of the rotation matrix R E , and a rotation vector r E , which is a product between the angle ∥R E ∥ and a unit rotary axis vector q E /∥q E ∥, is then calculated. A position matrix P E is calculated based on the position matrix p E and the rotation vector r E . The angles θ′ are updated based on results obtained by multiplying the position matrix P 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.

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 d 5 , 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 E and R 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.

Meanwhile, also in the same manner as that in the previous embodiment, when the positions P 5 ′ and P 6 ′ are not within the motion range, those positions P 5 ′ and P 6 ′ 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.

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.

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

In the drawings:

1 robot system

2 robot

3 controller

4 base

5 shoulder

6 second offset arm

7 first arm

8 third offset arm

9 second arm

10 fifth offset arm

11 wrist

12 hand