Manipulator simulation method and apparatus

Abstract

An apparatus, and method of use therefor, adapted to evaluate angles of rotation of joints for coupling arm-to-arm to each other on the basis of information representative of position and orientation of a tip of a manipulator having six or less arms, which are sequentially coupled with each other, so as to implement the position and the orientation of the tip of the manipulator. A matrix equation is evaluated beforehand in accordance with a mathematical expression processing. Parameters, which represent a figure of the manipulator, are inputted into the resultant matrix equation. Further, the information representative of position and orientation of the tip of the manipulator is entered to evaluate the angles of rotation of joints in accordance with a numerical resolution.

Description

BACKGROUND OF THE INVENTION

1. Field of the Invention

The present invention relates to a manipulator simulation method and apparatus adapted to evaluate angles of rotation of joints for coupling arm-to-arm to each other on the basis of information representative of position and attitude or orientation of a tip of a manipulator having six or less arms, which are sequentially coupled with each other, so as to implement the position and the attitude or orientation of the tip of the manipulator.

2. Description of the Related Art

FIG. 9 is a typical illustration of an example of a manipulator comprising a plurality of arms and joints for coupling the arms to each other.

In FIG. 9, a plurality of arms 2.sub.-- 1, 2.sub.-- 2, . . . , 2.sub.-- 5 are sequentially coupled with each other by joints 3.sub.-- 1, 3.sub.-- 2, . . . , 3.sub.-- 5 on a substrate or a base 1. A hand 4 is coupled with the top arm 2.sub.-- 5. The hand 4 serves to grasp a work object 5 and let loose the same through switching of a finger 4a constituting the hand 4. The joints 3.sub.-- 1, 3.sub.-- 2, . . . , 3.sub.-- 5 are arranged in such a manner that the arms 2.sub.-- 1, 2.sub.-- 2, . . . , 2.sub.-- 5 coupled with each other by joints 3.sub.-- 1, 3.sub.-- 2, . . . , 3.sub.-- 5 are rotatably movable. The use of such a manipulator makes it possible to perform, through a rotatable movement of the arms such that the hand 4 of the tip of the arms 2.sub.-- 1, 2.sub.-- 2, . . . , 2.sub.-- 5 is in a predetermined position and takes a predetermined attitude, a work, for example, such that the work object 5 is grasped and carried to another position.

It is referred to as the inverse kinematics that in the manipulator arranged, for example, as illustrated in FIG. 9, information as to the position and orientation of the tip of the manipulator is provided, so that angles of the respective joints are evaluated to move the tip of the manipulator to the given position with the given attitude.

Such inverse kinematics are applicable to the following fields

(1) Robots

For a robot having an arbitrary link configuration, the inverse kinematics is automatically solved, so that applications to the work plans of the robot are facilitated.

(2) CAD systems for a mechanism design

Incorporation of the inverse kinematics into the CAD system for a mechanism design, in which a design for parts having a link mechanism is often performed, makes it possible to simulate beforehand as to whether products having the link mechanism operate as desired.

(3) Production of an animation in the multimedia

In the multimedia, various applications in mixing of computer graphics, video and audio are expected. On the other hand, in the field of the commercial and a movie, it is desired to produce an animation, which is high in reality, using the computer graphics. Hitherto, in the field of the computer graphics, the animation is produced on an off-line basis. Particularly, with regard to a motion for an android model, there have been adopted a very troublesome scheme in which an angle is designated for each joint and a motion capturing scheme in which a specific sensor is attached to the human body so as to take in the motion into a computer. Incorporation of the inverse kinematics into an animation producing tool makes it possible to very efficiently produce a motion of the animation model such as the android.

(4) Ergonomics

Hitherto, in a design of products having some contact with the human, such as chairs, desks, keyboards and the like, there is seen a lack of arguments as to the readiness-to-use, the operability, and the amenity on an ergonomics basis. It is noted, however, that incorporation of the inverse kinematics into the android model produced by the computer graphics makes it possible to perform an inspection of manufactured goods through various motions of the android model at the stage of the simulation.

Hitherto, in connection with the problem of the inverse kinematics, in a case where analytic solutions exist, an algebraic operation is performed one by one for each object to evaluate the analytic solution. In this case, it is difficult to discriminate between the case of the existence of the analytic solution and the case of the non-existence of the analytic solution. This involves a lack of generality for a system in its entirety. Further, in a design, even a manipulator having the analytic solutions, when such a manipulator is manufactured indeed, it happens that the calibration involves a minor offset and as a result the inverse kinematics with the analytic solution is not useful.

On the other hand, in connection with the case of non-existence of the analytic solution, there has been developed a method referred to as a homotopy scheme or a continuation scheme in which the precision is improved by means of repeating the numerical solution starting from the approximate solution, specifically, constituting a predictor-corrector on a Newton-Raphson basis.

This method involves, however, the following drawbacks and cannot be used practically.

(1) There is a case where numerical instability exists.

(2) There is not seen dependency to the physical constant (D-H parameter).

(3) A calculation load is large.

SUMMARY OF THE INVENTION

In view of the foregoing, it is therefore an object of the present invention to provide a manipulator simulation method and apparatus capable of solving the inverse kinematics at higher speed as compared with the prior art.

To achieve the above-mentioned objects, according to the present invention, there is provided a first manipulator simulation method adapted to evaluate angles of rotation of joints for coupling arm-to-arm to each other on the basis of information representative of position and orientation of a tip of a manipulator having six or less arms, which are sequentially coupled with each other, so as to implement the position and the orientation of the tip of the manipulator, wherein

when a homogeneous coordinates transformation matrix A.sub.hand and representative of the position and the orientation of the tip of the manipulator looking at an absolute coordinate system is expressed by ##EQU1## where C.sub.hand denotes an orientation, and t.sub.hand denotes a position, and the homogeneous coordinates transformation matrix A.sub.i representative of the position and the orientation of the (i+1)th coordinate system looking at the i-th coordinate system, of all the coordinate systems sequentially assigned from the rear end of the manipulator toward the top, is expressed by ##EQU2## where: i=1, 2, . . . , 6 C.sub.i =C.sub.z,i .multidot.C.sub.x,i ##EQU3## c.sub.i =cos.theta..sub.i, s.sub.i =sin.theta..sub.i .lambda..sub.i =cos.alpha..sub.i, .mu..sub.i =sin.alpha..sub.i .theta..sub.i denotes an angle of rotation of i-th joint, .alpha..sub.i denotes an angle of gradient of axis of rotation of i-th joint to an axis of rotation of i-1th joint, t.sub.i denotes a three-dimensional parameter defining a configuration of i-th link, and the homogeneous coordinates transformation matrix A.sub.i may be a unit matrix, a matrix equation (1) is evaluated based on said homogeneous coordinates transformation matrix A.sub.hand and said homogeneous coordinates transformation matrix A.sub.i, ##EQU4## where U denotes 20.times.16 of real number matrix, V denotes 20.times.18 of real number matrix, ##EQU5## and X.sub.i =tan(.theta..sub.i /2),

by a numerical value substitution of respective components of the parameter t.sub.i into the matrix equation expressed by equation (1), derived is a matrix equation expressed by equation (2) in which respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, ##EQU6## where U(A.sub.hand) and V(A.sub.hand) stand for matrices in which the respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, said matrices being produced by numerical value substitution of respective components of the parameter t.sub.i into said matrices U and V, respectively,

by a numerical value substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand into the matrix equation expressed by equation (2), derived is a numerical value matrix equation expressed by equation (3), ##EQU7## where U.sub.num and V.sub.num stand for numerical value matrices produced by numerical value substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand into said matrices U(A.sub.hand) and V(A.sub.hand), respectively, and

angles .theta..sub.i.sup.(k) of rotation (k stands for positive integers to the maximum 16 for discriminating mutually between a plurality of angles .theta..sub.i) of the joints are evaluated on the basis of the numerical value matrix equation expressed by equation (3).

Here, it is acceptable to adopt a manipulator simulation method, wherein to evaluate said angles .theta..sub.i.sup.(k) of rotation on the basis of the numerical value matrix equation expressed by equation (3):

equations, in which .theta..sub.4 and .theta..sub.5 are eliminated, are evaluated on the basis of the numerical value matrix equation given by equation (3); eigenvalue equations or generalized eigenvalue equations are evaluated on the basis of the resultant equations in which .theta..sub.4 and .theta..sub.5 are eliminated; and said angles .theta..sub.i.sup.(k) of rotation are evaluated on the basis of the resultant eigenvalue equations or generalized eigenvalue equations.

Further, to achieve the above-mentioned objects, according to the present invention, there is provided a second manipulator simulation method adapted to evaluate angles of rotation of joints for coupling arm-to-arm to each other on the basis of information representative of position and orientation of a tip of a manipulator having six or less arms, which are sequentially coupled with each other, so as to implement the position and the orientation of the tip of the manipulator, wherein

when a homogeneous coordinates transformation matrix A.sub.hand representative of the position and the orientation of the tip of the manipulator looking at an absolute coordinate system is expressed by ##EQU8## where C.sub.hand denotes an orientation, and t.sub.hand denotes a position, and the homogeneous coordinates transformation matrix A.sub.i representative of the position and the orientation of the (i+1)th coordinate system looking at the i-th coordinate system, of all the coordinate systems sequentially assigned from the rear end of the manipulator toward the top, is expressed by ##EQU9## where: i=1, 2, . . . , 6 C.sub.i =C.sub.z,i .multidot.C.sub.x,i ##EQU10## c.sub.i =cos.theta..sub.i, s.sub.i sin.theta..sub.i .lambda..sub.i =cos.alpha..sub.i, .mu..sub.i =sin.alpha..sub.i .theta..sub.i denotes an angle of rotation of i-th joint, .alpha..sub.i denotes an angle of gradient of axis of rotation of i-th joint to an axis of rotation of i-1th joint, t.sub.i denotes a three-dimensional parameter defining a configuration of i-th link, and the homogeneous coordinates transformation matrix A.sub.i may be a unit matrix, a matrix equation (1) is evaluated based on said homogeneous coordinates transformation matrix A.sub.hand and said homogeneous coordinates transformation matrix A.sub.i, ##EQU11## where U denotes 20.times.16 of real number matrix, V denotes 20.times.18 of real number matrix, ##EQU12## .rho.=�x.sub.4.sup.2 x.sub.5.sup.2, x.sub.4.sup.2 x.sub.5, x.sub.4.sup.2, x.sub.4 x.sub.5.sup.2, x.sub.4 x.sub.5, x.sub.4, x.sub.5.sup.2, x.sub.5, 1!.sup.T x.sub.i =tan(.theta..sub.i /2),

by a numerical value substitution of respective components of the parameter t.sub.i into the matrix equation expressed by equation (4), derived is a matrix equation expressed by equation (5) in which respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, ##EQU13## where U(A.sub.hand) and V(A.sub.hand) stand for matrices in which the respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, said matrices being produced by numerical value substitution of respective components of the parameter t.sub.i into said matrices U and V, respectively,

by numerical substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand into the matrix equation expressed by equation (5), derived is a numerical value matrix equation expressed by equation (6), ##EQU14## where U.sub.num and V.sub.num stand for numerical value matrices produced by numerical value substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand into said matrices U(A.sub.hand) and V(A.sub.hand), respectively, and

angles .theta..sub.i.sup.(k) of rotation (k stands for positive integers to the maximum 16 for discriminating mutually between a plurality of angles .theta..sub.i) of the joints are evaluated on the basis of the numerical value matrix equation expressed by equation (6).

Here, it is acceptable to adopt a manipulator simulation method, wherein to evaluate said angles .theta..sub.i.sup.(k) of rotation on the basis of the numerical value matrix equation expressed by equation (6):

equations, in which .theta..sub.1 and .theta..sub.2 are eliminated, are evaluated on the basis of the numerical value matrix equation given by equation (6); eigenvalue equations or generalized eigenvalue equations are evaluated on the basis of the resultant equations in which .theta..sub.1 and .theta..sub.2 are eliminated; and said angles .theta..sub.i.sup.(k) of rotation are evaluated on the basis of the resultant eigenvalue equations or generalized eigenvalue equations.

Furthermore, to achieve the above-mentioned objects, according to the present invention, there is provided a first manipulator simulation apparatus adapted to evaluate angles of rotation of joints for coupling arm-to-arm to each other on the basis of information representative of position and orientation of a tip of a manipulator having six or less arms, which are sequentially coupled with each other, so as to implement the position and the orientation of the tip of the manipulator, wherein

when a homogeneous coordinates transformation matrix A.sub.hand and representative of the position and the orientation of the tip of the manipulator looking at an absolute coordinate system is expressed by ##EQU15## where C.sub.hand denotes an orientation, and t.sub.hand denotes a position, and the homogeneous coordinates transformation matrix A.sub.i representative of the position and the orientation of the (i+1)th coordinate system looking at the i-th coordinate system, of all the coordinate systems sequentially assigned from the rear end of the manipulator toward the top, is expressed by ##EQU16## where: i=1, 2, . . . , 6 C.sub.i =C.sub.z,i .multidot.C.sub.x,i ##EQU17## c.sub.i =cos.theta..sub.i, s.sub.i =sin.theta..sub.i .lambda..sub.i =cos.alpha..sub.i, .mu..sub.i =sin.alpha..sub.i .theta..sub.i denotes an angle of rotation of i-th joint, .alpha..sub.i denotes an angle of gradient of axis of rotation of i-th joint to an axis of rotation of i-1th joint, t.sub.i denotes a three-dimensional parameter defining a configuration of i-th link, and the homogeneous coordinates transformation matrix A.sub.i may be a unit matrix, said apparatus comprises: storage means for storing a matrix equation (1) evaluated based on said homogeneous coordinates transformation matrix A.sub.hand and said homogeneous coordinates transformation matrix A.sub.i, ##EQU18## where U denotes 20.times.16 of real number matrix, V denotes 20.times.18 of real number matrix, ##EQU19## and X.sub.i =tan(.theta..sub.i /2); first input means for inputting respective components of the parameter t.sub.i in the form of numerical value; parameter numerical value substitution means for, by a numerical value substitution of respective components of the parameter t.sub.i entered through said first input means into the matrix equation expressed by equation (1), deriving a matrix equation expressed by equation (2) in which respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, ##EQU20## where U(A.sub.hand) and V(A.sub.hand) stand for matrices in which the respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, said matrices being produced by numerical value substitution of respective components of the parameter t.sub.i into said matrices U and V, respectively; second input means for inputting respective components of said homogeneous coordinates transformation matrix A.sub.hand in the form of numerical value; numerical value matrix equation derivation means for, by numerical value substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand entered through said second input means into the matrix equation expressed by equation (2), deriving a numerical value matrix equation expressed by equation (3), ##EQU21## where U.sub.num and V.sub.num stand for numerical value matrices produced by numerical substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand into said matrices U(A.sub.hand) and V(A.sub.hand), respectively; and joint rotation angle computing means for evaluating angles .theta..sub.i.sup.(k) of rotation (k stands for positive integers to the maximum 16 for discriminating mutually between a plurality of angles .theta..sub.i) of the joints on the basis of the numerical value matrix equation expressed by equation (3).

Still further, to achieve the above-mentioned objects, according to the present invention, there is provided a second manipulator simulation apparatus adapted to evaluate angles of rotation of joints for coupling arm-to-arm to each other on the basis of information representative of position and orientation of a tip of a manipulator having six or less arms, which are sequentially coupled with each other, so as to implement the position and the orientation of the tip of the manipulator, wherein

when a homogeneous coordinates transformation matrix A.sub.hand representative of the position and the orientation of the tip of the manipulator looking at an absolute coordinate system is expressed by ##EQU22## where C.sub.hand denotes an orientation, and t.sub.hand denotes a position, and the homogeneous coordinates transformation matrix A.sub.i representative of the position and the orientation of the (i+1)th coordinate system looking at the i-th coordinate system, of all the coordinate systems sequentially assigned from the rear end of the manipulator toward the top, is expressed by ##EQU23## where: i=1, 2, . . . , 6 C.sub.i =C.sub.z,i .multidot.C.sub.x,i ##EQU24## c.sub.i =cos.theta..sub.i, s.sub.i =sin.theta..sub.i .lambda..sub.i =cos.alpha..sub.i, .mu..sub.i =sin.alpha..sub.i .theta..sub.i denotes an angle of rotation of i-th joint, .alpha..sub.i denotes an angle of gradient of axis of rotation of i-th joint to an axis of rotation of i-1th joint, t.sub.i denotes a three-dimensional parameter defining a configuration of i-th link, and the homogeneous coordinates transformation matrix A.sub.i may be a unit matrix, said apparatus comprises: storage means for storing a matrix equation expressed by equation (2) in which respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, ##EQU25## where U(A.sub.hand) and V(A.sub.hand) stand for matrices in which the respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, said matrices being produced by a numerical value substitution of respective components of the parameter t.sub.i into said matrices U and V, respectively, wherein said matrix equation expressed by equation (2) is obtained by a numerical value substitution of respective components of the parameter t.sub.i into a matrix equation (1) evaluated based on said homogeneous coordinates transformation matrix A.sub.hand and said homogeneous coordinates transformation matrix A.sub.i, ##EQU26## where U denotes 20.times.16 of real number matrix, V denotes 20.times.18 of real number matrix, ##EQU27## and X.sub.i =tan(.theta..sub.i /2); input means for inputting respective components of said homogeneous coordinates transformation matrix A.sub.hand ; numerical value matrix equation derivation means for, by numerical value substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand entered through said input means into the matrix equation expressed by equation (2), deriving a numerical value matrix equation expressed by equation (3), ##EQU28## where U.sub.num and V.sub.num stand for numerical value matrices produced by numerical substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand into said matrices U(A.sub.hand) and V(A.sub.hand), respectively; and joint rotation angle computing means for evaluating angles .theta..sub.i.sup.(k) of rotation (k stands for positive integers to the maximum 16 for discriminating mutually between a plurality of angles .theta..sub.i) of the joints on the basis of the numerical value matrix equation expressed by equation (3).

Here, in the first manipulator simulation apparatus and the second manipulator simulation apparatus, it is preferable that said joint rotation angle computing means comprises:

means for evaluating equations, in which .theta..sub.4 and .theta..sub.5 are eliminated, on the basis of the numerical value matrix equation given by equation (3); means for evaluating eigenvalue equations or generalized eigenvalue equations on the basis of the resultant equations in which .theta..sub.4 and .theta..sub.5 are eliminated; and means for evaluating said angles .theta..sub.i.sup.(k) of rotation on the basis of the resultant eigenvalue equations or generalized eigenvalue equations.

Still further, to achieve the above-mentioned objects, according to the present invention, there is provided a third manipulator simulation apparatus adapted to evaluate angles of rotation of joints for coupling arm-to-arm to each other on the basis of information representative of position and orientation of a tip of a manipulator having six or less arms, which are sequentially coupled with each other, so as to implement the position and the orientation of the tip of the manipulator, wherein

when a homogeneous coordinates transformation matrix A.sub.hand representative of the position and the orientation of the tip of the manipulator looking at an absolute coordinate system is expressed by ##EQU29## where C.sub.hand denotes an orientation, and t.sub.hand denotes a position, and the homogeneous coordinates transformation matrix A.sub.i representative of the position and the orientation of the (i+1)th coordinate system looking at the i-th coordinate system, of all the coordinate systems sequentially assigned from the rear end of the manipulator toward the top, is expressed by ##EQU30## where: i=1, 2, . . . , 6 C.sub.i =C.sub.z,i .multidot.C.sub.x,i ##EQU31## c.sub.i =cos.theta..sub.i, s.sub.i =sin.theta..sub.i .lambda..sub.i =cos.alpha..sub.i, .mu..sub.i =sin.alpha..sub.i .theta..sub.i denotes an angle of rotation of i-th joint, .alpha..sub.i denotes an angle of gradient of axis of rotation of i-th joint to an axis of rotation of i-1th joint, t.sub.i denotes a three-dimensional parameter defining a configuration of i-th link, and the homogeneous coordinates transformation matrix A.sub.i may be a unit matrix, said apparatus comprises: storage means for storing a matrix equation (4) evaluated based on said homogeneous coordinates transformation matrix A.sub.hand and said homogeneous coordinates transformation matrix A.sub.i, ##EQU32## where U denotes 20.times.16 of real number matrix, V denotes 20.times.18 of real number matrix, ##EQU33## .rho.=�x.sub.4.sup.2 x.sub.5.sup.2, x.sub.4.sup.2 x.sub.5, x.sub.4.sup.2, x.sub.4 x.sub.5.sup.2, x.sub.4 x.sub.5, x.sub.4, x.sub.5.sup.2, x.sub.5, 1!.sup.T x.sub.i =tan(.theta..sub.i /2), first input means for inputting respective components of the parameter t.sub.i in the form of numerical value; parameter numerical value substitution means for, by a numerical value substitution of respective components of the parameter t.sub.i entered through said first input means into the matrix equation expressed by equation (4), deriving a matrix equation expressed by equation (5) in which respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, ##EQU34## where U(A.sub.hand) and V(A.sub.hand) stand for matrices in which the respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, said matrices being produced by numerical value substitution of respective components of the parameter t.sub.i into said matrices U and V, respectively; second input means for inputting respective components of said homogeneous coordinates transformation matrix A.sub.hand ; numerical value matrix equation derivation means for, by numerical value substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand entered through said second input means into the matrix equation expressed by equation (5), deriving a numerical value matrix equation expressed by equation (6), ##EQU35## where U.sub.num and V.sub.num stand for numerical value matrices produced by numerical substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand into said matrices U(A.sub.hand) and V(A.sub.hand), respectively; and joint rotation angle computing means for evaluating angles .theta..sub.i.sup.(k) of rotation (k stands for positive integers to the maximum 16 for discriminating mutually between a plurality of angles .theta..sub.i) of the joints on the basis of the numerical value matrix equation expressed by equation (6).

Still further, to achieve the above-mentioned objects, according to the present invention, there is provided a fourth manipulator simulation apparatus adapted to evaluate angles of rotation of joints for coupling arm-to-arm to each other on the basis of information representative of position and orientation of a tip of a manipulator having six or less arms, which are sequentially coupled with each other, so as to implement the position and the orientation of the tip of the manipulator, wherein

when a homogeneous coordinates transformation matrix A.sub.hand representative of the position and the orientation of the tip of the manipulator looking at an absolute coordinate system is expressed by ##EQU36## where C.sub.hand denotes an orientation, and t.sub.hand denotes a position, and the homogeneous coordinates transformation matrix A.sub.i representative of the position and the orientation of the (i+1)th coordinate system looking at the i-th coordinate system, of all the coordinate systems sequentially assigned from the rear end of the manipulator toward the top, is expressed by ##EQU37## where: i=1, 2, . . . , 6 C.sub.i =C.sub.z,i .multidot.C.sub.x,i ##EQU38## c.sub.i =cos.theta..sub.i, s.sub.i =sin.theta..sub.i .lambda..sub.i =cos.alpha..sub.i, .mu..sub.i =sin.alpha..sub.i .theta..sub.i denotes an angle of rotation of i-th joint, .alpha..sub.i denotes an angle of gradient of axis of rotation of i-th joint to an axis of rotation of i-1th joint, t.sub.i denotes a three-dimensional parameter defining a configuration of i-th link, and the homogeneous coordinates transformation matrix A.sub.i may be a unit matrix, said apparatus comprises: storage means for storing a matrix equation expressed by equation (5) in which respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, ##EQU39## where U(A.sub.hand) and V(A.sub.hand) stand for matrices in which the respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, said matrices being produced by a numerical value substitution of respective components of the parameter t.sub.i into said matrices U and V, respectively, wherein said matrix equation expressed by equation (5) is obtained by a numerical value substitution of respective components of the parameter t.sub.i into a matrix equation (4) evaluated based on said homogeneous coordinates transformation matrix A.sub.hand and said homogeneous coordinates transformation matrix A.sub.i, ##EQU40## where U denotes 20.times.16 of real number matrix, V denotes 20.times.18 of real number matrix, ##EQU41## .rho.=�x.sub.4.sup.2 x.sub.5.sup.2, x.sub.4.sup.2 x.sub.5, x.sub.4.sup.2, x.sub.4 x.sub.5.sup.2, x.sub.4 x.sub.5, x.sub.4, x.sub.5.sup.2, x.sub.5, 1!.sup.T x.sub.i =tan(.theta..sub.i /2), input means for inputting respective components of said homogeneous coordinates transformation matrix A.sub.hand ; numerical value matrix equation derivation means for, by numerical value substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand entered through said input means into the matrix equation expressed by equation (5), deriving a numerical value matrix equation expressed by equation (6), ##EQU42## where U.sub.num and V.sub.num stand for numerical value matrices produced by numerical substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand into said matrices U(A.sub.hand) and V(A.sub.hand), respectively; and joint rotation angle computing means for evaluating angles .theta..sub.i.sup.(k) of rotation (k stands for positive integers to the maximum 16 for discriminating mutually between a plurality of angles .theta..sub.i) of the joints on the basis of the numerical value matrix equation expressed by equation (6).

Here, in the third manipulator simulation apparatus and the fourth manipulator simulation apparatus, it is preferable that said joint rotation angle computing means comprises:

means for evaluating equations, in which .theta..sub.1 and .theta..sub.2 are eliminated, on the basis of the numerical value matrix equation given by equation (6); means for evaluating eigenvalue equations or generalized eigenvalue equations on the basis of the resultant equations in which .theta..sub.1 and .theta..sub.2 are eliminated; and means for evaluating said angles .theta..sub.i.sup.(k) of rotation on the basis of the resultant eigenvalue equations or generalized eigenvalue equations.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a flowchart useful for understanding the first embodiment of the first manipulator simulation method according to the present invention;

FIG. 2 is a view partially illustrating links and joints, useful understanding a D-H parameter;

FIG. 3 is a detailed flowchart of step (f) of FIG. 1;

FIG. 4 is a detailed flowchart of step (f) of FIG. 1 in connection with the second embodiment;

FIG. 5 is a block diagram showing an embodiment of the first manipulator simulation apparatus according to the present invention;

FIG. 6 is a detailed block diagram of means for calculating angles of rotation of joints as shown in FIG. 5;

FIG. 7 is a block diagram showing an embodiment of the second manipulator simulation apparatus according to the present invention;

FIG. 8 is a detailed block diagram of means for calculating angles of rotation of joints as shown in FIG. 5 in connection with the fifth embodiment; and

FIG. 9 is a typical illustration of an example of a manipulator comprising a plurality of arms and joints for coupling the arms to each other.

DESCRIPTION OF THE PREFERRED EMBODIMENTS

Hereinafter, there will be described embodiments of the present invention.

Hereinafter, there will be dealt with the inverse kinematics of 6 degree of freedom of manipulator through the combination of rotary joints.

FIG. 1 is a flowchart useful for understanding an embodiment of the first manipulator simulation method according to the present invention. Hereinafter, this embodiment will be referred to as "the first embodiment".

First, in step (a), a matricial equation in the above-mentioned equation (1) is evaluated.

A homogeneous coordinates transformation matrix A.sub.hand representative of the position and the orientation of the tip of the manipulator looking at an absolute coordinate system, for example, a coordinate system assigned to the substrate or the base 1 shown in FIG. 9, is expressed by ##EQU43## where C.sub.hand denotes an orientation, and

t.sub.hand denotes a position.

The homogeneous coordinates transformation matrix A.sub.i representative of the position and the orientation of the (i+1)th coordinate system looking at the i-th coordinate system, of all the coordinate systems sequentially assigned from the rear end of the manipulator toward the top, and the inverse matrix A.sub.i.sup.-1 are respectively expressed by ##EQU44## i=1, 2, . . . , 6 where .epsilon.R(4.times.4) implies that the matrix set forth in the left side of this symbol is a real number matrix of 4 columns.

Here, the rotary portions C.sub.i, C.sub.z,i and C.sub.x,i are defined as follows:

C.sub.i =C.sub.z,i .multidot.C.sub.x,i ##EQU45## c.sub.i =cos.theta..sub.i, s.sub.i =sin.theta..sub.i .lambda..sub.i =cos.alpha..sub.i, .mu..sub.i =sin.alpha..sub.i .theta..sub.i denotes an angle of rotation of i-th joint, .alpha..sub.i denotes an angle of gradient of axis of rotation of i-th joint to an axis of rotation of i-1th joint, t.sub.i denotes a three-dimensional parameter defining a configuration of i-th link. Specifically, here, a Demerit-Hartenberg (D-H) parameter as set forth below is adopted. t.sub.i =�a.sub.i 0d.sub.i ! (10) i=1, 2, . . . , 6

Here, while there is dealt with a manipulator having a degree of freedom 6, in a case where the number of degree of freedom is less than 6, anyone or more of the above-mentioned A.sub.1 (i=1, 2, . . . , 6) may be regarded as a unit matrix by the corresponding reduced number. Thus, it is possible for also a manipulator having the number of degree of freedom less than 6 to apply this argument as it is.

Next, the D-H parameter will be explained in accordance with the definition set forth in the publication "ROBOTICS for mechanical systems" by Shigeki Tohyama, published by Sohgoh Densi Publishing Company. The D-H parameter is well known, and thus here the definition thereof will be simply explained.

FIG. 2 is a view partially illustrating links and joints, useful understanding the D-H parameter.

First, a base coordinate system (right.sub.hand system) is set up. The z.sub.0 -axis is given so as to direct to a direction of the arm shoulder portion in a direction of rotation of joint 1. The x.sub.0 -axis and y.sub.0 -axis are given on a suitable basis.

Next, with respect to the respective links i (i=1, 2, . . . , 6), the coordinate systems are set up in accordance with the following items (1) to (4).

(1) Set up the z.sub.i -axis in a direction of motion of joint i+1.

(2) Determine as the origin of the i-th coordinate system the intersection point of the z.sub.i -axis and the z.sub.i-1 -axis or the intersection point of the z.sub.i -axis and the common normal of the z.sub.i -axis and the z.sub.i-i -axis. When the z.sub.i -axis and the z.sub.i-1 -axis are parallel to each other, the origin of the i-th coordinate system is determined in such a manner that the link parameter d.sub.i, which will be described later, becomes 0.

(3) Determine x.sub.i set forth below, when z.sub.i-1 and z.sub.i are defined as vectors directed toward the directions of the z.sub.i-1 -axis and the z.sub.i -axis, respectively,

x.sub.i =.+-.(z.sub.i-1 .times.z.sub.i)/.vertline..vertline.z.sub.i-1 .times.z.sub.i .vertline..vertline. or set up the x.sub.i -axis in the direction of the common normal of the z.sub.i -axis and the z.sub.i-1 -axis.

(4) Set up the y.sub.i -axis to form the right hand system.

When the coordinate system is set up in accordance with the above-mentioned manner, a distance between the origin of the i-1th coordinate system and the intersection point of the z.sub.i-1 -axis and the x.sub.i -axis, extending in the direction of the z.sub.i-1 -axis, is defined as a link parameter d.sub.i. Further, a distance between the intersection point of the z.sub.i-1 -axis and the x.sub.i -axis and the origin of the i-th coordinate system, in which the negative direction of the z.sub.i -axis is of positive, is defined as a link parameter a.sub.i.

Here, as the parameter t.sub.i, there is adopted the equation (10) as noted above, that is,

t.sub.i =�a.sub.i 0d.sub.i ! (10) i=1, 2, . . . , 6

Particularly, it is noted that the above-mentioned items .theta..sub.i and .alpha..sub.i are also the link parameters constituting the D-H parameter. The link parameters .theta..sub.i denotes an angle of rotation on the z.sub.i-1 -axis from the x.sub.i-1 -axis to the x.sub.i -axis. The link parameter .alpha..sub.i denotes an angle of rotation on the x.sub.i -axis from the z.sub.i-1 -axis to the z.sub.i -axis.

Returning to FIG. 1, there will be continued the explanation as to the method of deriving the matricial equation (1) in step (a).

The forward kinematics equation of the manipulator is given by equation (11).

A.sub.hand =A.sub.1 .multidot.A.sub.2 .multidot.A.sub.3 .multidot.A.sub.4 .multidot.A.sub.5 .multidot.A.sub.6 (11)

The forward kinematics implies that when information as to angles of rotation of the respective joints for coupling link-to-link constituting a manipulator with each other is provided, a position and an orientation of the tip of the manipulator is determined.

Here, 14 fundamental equations (2 scalar equations and 4 vector equations) are derived in accordance with the above equation (11), and then further 6 fundamental equations are derived using the resultant 14 fundamental equations. The combination of the total 20 fundamental equations produces the matricial equation given by the above equation (1).

(Derivation of 14 fundamental equations)

The above equation (11) is rewritten as follows.

A.sub.3 .multidot.A.sub.4 .multidot.A.sub.5 =A.sub.2.sup.-1 .multidot.A.sub.1.sup.-1 .multidot.A.sub.hand .multidot.A.sub.6.sup.-1(12)

First, the following fundamental vector equations are derived based on the equation (12). Incidentally, a quantity derived from the left side member of the equation (12) is expressed by the suffix A, and a quantity derived from the right side member of the equation (12) is expressed by the suffix B.

z.sub.A .ident.A.sub.3 A.sub.4 A.sub.5 e.sub.3 =A.sub.2.sup.-1 A.sub.1.sup.-1 A.sub.hand A.sub.6.sup.-1 e.sub.3 .ident.z.sub.B(13) p.sub.A .ident.A.sub.3 A.sub.4 A.sub.5 e.sub.4 =A.sub.2.sup.-1 A.sub.1.sup.-1 A.sub.hand A.sub.6.sup.-1 e.sub.4 .ident.p.sub.B(14) where e.sub.i (i=1 . . . 4) denote unit vectors, and represent: e.sub.1 =�1000!.sup.T e.sub.2 =�0100!.sup.T e.sub.3 =�0010!.sup.T e.sub.4 =�0001!.sup.T

In equations (13) and (14), the fourth component becomes an obvious equation like 1=1, 0=0. Thus, hereinafter, the fourth component will be neglected, and it is assumed that equations (13) and (14) denote each a three-dimensional vector comprising the first component, the second component and the third component.

Taking the internal product, the external product and the vector sum based on p and z of equations (13) and (14) may derive the following equations.

(p.sup.T .multidot.p).sub.A =(p.sup.T .multidot.p).sub.B (15) (p.sup.T .multidot.z).sub.A =(p.sup.T .multidot.z).sub.B (16) (p.sup.T .times.z).sub.A =(p.sup.T .times.z).sub.B (17) ##EQU46##

Equation (18) may be rewritten as follows. ##EQU47##

As a result of the precise analysis using the mathematical expression processing, it is understood that equations (13) to (18) or (19) are of linear with respect to cos.theta..sub.i, sin.theta..sub.i. On the other hand, other combinations each comprising p and z lack a linearity.

To apply the mathematical expression processing, equations (13) to (18) or (19) are resolved beforehand using equations (8) and (9) as follows. ##EQU48##

(Property of the fundamental equations)

Fundamental equations (20) to (25) have the following characteristics.

(a) The respective elements of the matrix A.sub.hand representative of the position an orientation of the tip of the manipulator appears only at the right side member. (b) All the equations do not depend on .theta..sub.6.

Because, in equations (20) to (25) .theta..sub.6 appears in the form of C.sub.6.sup.T e.sub.3 =C.sub.x,6.sup.T C.sub.z,6.sup.T e.sub.3, but C.sub.z,6.sup.T e.sub.3 =�001!.sup.T.

(c) z.sub.3, p.sub.3, p.sup.T .multidot.z, p.sup.T .multidot.p, (p.sup.T .times.z).sub.3, �2(p.sup.T .multidot.z)p-(p.sup.T .multidot.p)z!.sub.3 does not depend on .theta..sub.3. Here, for example, z.sub.3 and p.sub.3 stand for the third elements of z and p, respectively. This is the similar as to the matter of others.

Because, the items (p.sup.T .multidot.z) and (p.sup.T .multidot.p) do not depend on C.sub.z,3. With respect to z.sub.3, p.sub.3, (p.sup.T .times.z).sub.3, �2(p.sup.T .multidot.z)p-(p.sup.T .multidot.p)z!.sub.3, .theta..sub.3 always appears in the form of e.sub.3.sup.T C.sub.z,3, but e.sub.3.sup.T C.sub.z,3 =e.sub.3.sup.T.

(d) z.sub.1, z.sub.2, p.sub.1, p.sub.2, (p.sup.T .times.z).sub.1, (p.sup.T.times.z).sub.2, �2(p.sup.T .multidot.z)p-(p.sup.T .multidot.p)z!.sub.1, �2(p.sup.T .multidot.z)p-(p.sup.T .multidot.p)z!.sub.2 is linear with respect to tan(.theta..sub.3 /2). Because, when x.sub.i =tan(.theta..sub.i /2), the following equations consist: cos .theta..sub.i =(1-x.sub.i.sup.2)/(1+x.sub.i.sup.2) sin .theta..sub.i =2x.sub.i /(1+x.sub.i.sup.2) X.sub.3.sup.- .multidot.C.sub.z,3 =X.sub.3.sup.+ ##EQU49##

The above results can be obtained by means of multiplying, in z, p, p.sup.T xz, 2(p.sup.T .multidot.z)p-(p.sup.T .multidot.p)z, by X.sub.3.sup.- from the left.

(e) .theta..sub.1, .theta..sub.2, .theta..sub.4, .theta..sub.5 always appear in the form of a linearity with respect to cos.theta..sub.i, sin.theta..sub.i (i=1, 2, 4, 5), but the items of (cos.theta..sub.i).sup.2, (sin.theta..sub.i).sup.2, cos.theta..sub.i .multidot.sin.theta..sub.i do not appear.

Because, in z, p, p.sup.T .multidot.p, p.sup.T .multidot.z, p.sup.T .times.z, there simply appears C.sub.z,i once for each item. Further, with respect to 2(p.sup.T .multidot.z)p-(p.sup.T .multidot.p)z, it is understood that this is of linear through such a process that the values of p, z, p.sup.T .multidot.z, p.sup.T .multidot.p are substituted and arranged, taking account of (cos.theta..sub.i).sup.2 +(sin.theta..sub.i).sup.2 =1, (cos.alpha..sub.i).sup.2 +(sin.alpha..sub.i).sup.2 =1, using a mathematical expression processing.

(Matrix form of fundamental equations)

The fundamental equations of equations (20) to (25) can be expressed in the matrix form as set forth below. ##EQU50##

U', V'.epsilon.R(14.times.18) .xi.', .rho..epsilon.C.sup.9 (.epsilon.C.sup.9 indicates that the symbol of the left is nine-dimensional complex vector) .xi.'=�s.sub.4 .multidot.s.sub.5, s.sub.4 .multidot.c.sub.5, c.sub.4 .multidot.s.sub.5, c.sub.4 .multidot.c.sub.5, s.sub.4, c.sub.4, s.sub.5, c.sub.5, 1!.sup.T .rho.=�x.sub.1.sup.2 x.sub.2.sup.2, x.sub.1.sup.2 x.sub.2, x.sub.1.sup.2, x.sub.1 x.sub.2.sup.2, x.sub.1 x.sub.2, x.sub.1, x.sub.2.sup.2, x.sub.2, 1!.sup.T

The components of U' and V' can be obtained through such a process that equations (20) to (25) are carefully subjected to a mathematical expression processing; (cos.theta..sub.i).sup.2 +(sin.theta..sub.i).sup.2 =1(i=4, 5), (cos.alpha..sub.i).sup.2 +(sin.alpha..sub.i).sup.2 +1(i=1, . . . , 6), cos.theta..sub.i =(1-x.sub.i.sup.2)/(1+x.sub.i.sup.2), sin.theta..sub.i =2x.sub.i /(1+x.sub.i.sup.2) (i=1, 2, 3) are substituted; and in the resultant algebraic equations the coefficients for the respective items of .xi.' and .rho. are evaluated.

Each component of U' and V' is a polynomial of several variables in which the components of parameters t.sub.i and A.sub.hand are given in the form of variables.

(Addition of fundamental equations)

With respect to z.sub.3, p.sub.3, p.sup.T .multidot.z, p.sup.T .multidot.p, (p.sup.T .times.z).sub.3, �2(p.sup.T .multidot.z)p-(p.sup.T .multidot.p)z!.sub.3, they do not depend on .theta..sub.3. Hence, it is possible to derive 6 equations as set forth below by means of being multiplied by x.sub.3 =tan(.theta..sub.3 /2) and taking the linear sum.

�x.sub.3 z.sub.3 +p.sub.3 !.sub.A =�x.sub.3 z.sub.3 +p.sub.3 !.sub.B(28) �x.sub.3 p.sub.3 +z.sub.3 !.sub.A =�x.sub.3 p.sub.3 +z.sub.3 !.sub.B(29) �x.sub.3 p.sup.T .multidot.p+p.sup.T .multidot.z!.sub.A =�x.sub.3 p.sup.T .multidot.p+p.sup.T .multidot.z!.sub.B (30) �x.sub.3 p.sup.T .multidot.z+p.sup.T .multidot.p!.sub.A =�x.sub.3 p.sup.T .multidot.z+p.sup.T .multidot.p!.sub.B (31) �x.sub.3 (p.sup.T .times.z).sub.3 +z.sub.3 !.sub.A =�x.sub.3 (p.sup.T .times.z).sub.3 +z.sub.3 !.sub.B (32) ##EQU51##

When equations (28) to (33) are arranged in a similar fashion to that of equation (27), in which a part of the left side member is transferred to the right side member to be rearranged together with equation (27), there is obtained a matrix equation expressed by the above-mentioned equation (1). ##EQU52##

Incidentally, in the above case, there is adopted as parameter t.sub.i the above-mentioned equation (10), that is, the D-H parameter as set forth below.

t.sub.i =�a.sub.i 0d.sub.i !.sup.T (10)

However, it is acceptable that instead of the above-mentioned equation (10), there is adopted as parameter t.sub.i a so-called modification D-H parameter as set forth below.

t.sub.i =�.xi..sub.i .eta..sub.i .zeta..sub.i +.gamma..sub.i !.sup.T(28)

In the D-H parameter, as mentioned above, a length of the common normal of the axes of the adjacent two joints is defined as the link parameter a.sub.i. On the other hand, according to the modification D-H parameter, instead of the link parameter a.sub.i, there are introduced a vector D.sub.i =(.xi..sub.i, .eta..sub.i, .zeta..sub.i) which is perpendicular to only an axis in one direction, and an arbitrary length of parameter .gamma..sub.i along the axis z.sub.i-1 of rotation instead of the link parameter d.sub.i representative of a distance between link-to-link (cf. "ROBOTICS for mechanical systems" by Shigeki Tohyama, published by Sohgoh Densi Publishing Company). While the D-H parameter uses the common normal of the axes of two joints, it is possible according to the modification D-H parameter to set up the coordinate system to a desired place, since the parameter .gamma..sub.i can be optionally set up.

An adoption of the parameter of equation (28) causes sensitivity to the variation of parameters to be smooth by the corresponding increment of the parameters of the coordinate system representative of the mechanical system, thereby being easy to perform the calibration of the mechanical parameters.

Next, there will be explained step (b) et seqq. in FIG. 1 in connection with the first embodiment.

It is sufficient for step (a) to be carried out once only beforehand. Hereafter, in step (b), matrix equations suitable for individual manipulators are formed.

In case of a system in which there is no existence of a manipulator in the form of the actual hardware, for example, a computer graphics, it is acceptable to adopt as the above-mentioned parameter t.sub.i the D-H parameter (equation (10)) and the modification D-H parameter (equation (28)) as well. On the other hand, in case of a system in which there is existence of a manipulator in the form of the actual hardware, or in a case where it may be presumed to constitute a manipulator in the form of the actual hardware even if there is not yet existence of the manipulator, it is preferable to adopt as parameter t.sub.i the modification D-H parameter (equation (28)).

When the D-H parameter (equation (10)) is adopted as the above-mentioned parameter t.sub.i, the specific values of the respective components a.sub.i, d.sub.i (i=1, 2, . . . , 6) of the parameter t.sub.i are substituted into the above-mentioned equation (1).

This is the similar as to the matter of an adoption of the modification D-H parameter (equation (28)) as the parameter t.sub.i. In this case, first, the calibration of the modification D-H parameter is performed, and then the numerical values of the respective components (.xi..sub.i, .eta..sub.i, and .zeta..sub.i +.gamma..sub.i) of the parameter t.sub.i, which are the resultant values of the calibration, are substituted into the above-mentioned equation (1). When the manipulator is constituted in the form of the actual hardware, it will be out of the ideal manipulator owing to the looseness and errors in size. In view of this matter, the numerical values suitable for the manipulator, which is arranged indeed, are evaluated and substituted.

The calibration itself is not the subject here, and thus the explanation will be omitted (In detail, refer to the aforementioned Shigeki "ROBOTICS for mechanical systems" by Shigeki Tohyama).

In this manner, the numerical substitution of the respective components of the parameter t.sub.i into the matrix equation expressed by equation (1) makes it possible to derive the matrix equation expressed by the above-mentioned equation (2) in which the respective components of A.sub.hand are given as variables, that is, the equation as set forth below (step (c) in FIG. 1). ##EQU53##

The matrix equations using the numerical processing terminate at equation (2), that is, step (c) in FIG. 1, and thereafter the whole processes deal with the numerical matrix.

That is, a numerical matrix equation with the numerical matrices U.sub.num, V.sub.num is evaluated (step (e) in FIG. 1) through a numerical substitution (step (d) in FIG. 1) of the respective components of A.sub.hand representative of the position and the orientation of the tip of the manipulator into equation (2), and angles .theta..sub.i.sup.(k) of rotation (k stands for positive integers to the maximum 16 for discriminating mutually between a plurality of angles .theta..sub.i) of the respective joints i are evaluated on the basis of the numerical matrix equation (step (f) in FIG. 1). This numerical matrix equation is expressed by the above-mentioned equation (3), that is, ##EQU54##

Repetition of step (d) to step (f) makes it possible to perform various simulations on the manipulator model.

Next, there will be explained details of a numerical solution of .theta..sub.i.sup.(k), that is, step (f) in FIG. 1 on the basis of the numerical matrix equation given by equation (3).

FIG. 3 is a detailed flowchart of step (f) in FIG. 1.

While some numerical solutions will be explained hereinafter, all the numerical solutions comply with step (g) to step (j) in FIG. 3. That is, in any solutions which will be explained hereinafter, to determine .theta..sub.i.sup.(k), first, equations, in which .theta..sub.4 and .theta..sub.5 are eliminated, are evaluated on the basis of the numerical matrix equation given by equation (3) (step (g)). Next, eigenvalue equations or generalized eigenvalue equations are evaluated on the basis of the resultant equations in which .theta..sub.4 and .theta..sub.5 are eliminated (step(h)). Thereafter, .theta..sub.3.sup.(k), .theta..sub.2.sup.(k), .theta..sub.1.sup.(k) are evaluated on the basis of the resultant eigenvalue equations or generalized eigenvalue equations (step (i)). Then, further, .theta..sub.4.sup.(k), .theta..sub.5.sup.(k), .theta..sub.6.sup.(k) are evaluated (step (j)).

Hereinafter, there will be explained several numerical solutions according to the flowchart of FIG. 3.

(Numerical solution 1)

To erase .theta..sub.4 and .theta..sub.5 from the matricial equation given by equation (3), it is multiplied by 20.times.20 of real matrix W from the left of the equation. The respective components of the real matrix W are produced on a random basis. When the followings are given, ##EQU55## (.epsilon.: infinitesimal constant) is to be satisfied.

If det(U.sub.16ran).apprxeq.0, the components of the real matrix W is to be reset.

At that time, equation (3) is expressed as follows: ##EQU56##

Hence, ##EQU57##

This is substituted into the remaining four equations of equation (29), so that .theta..sub.4 and .theta..sub.5 are eliminated. Thus, the following equation is formed. ##EQU58##

(Derivation of eigenvalue equation (generalized eigenvalue equation))

When the following equation is given, ##EQU59## the above equation (31) is expressed by ##EQU60##

When both the side members of equation (32) are multiplied by x.sub.1 x.sub.2, x.sub.1, x.sub.2, further 12 equations are formed, so that total 16 equations are derived. These are expressed by:

G.multidot..rho.+x.sub.3 H.multidot..rho.=0 (33) G, H.epsilon.R(16.times.16) .rho..epsilon.C.sup.16 x.sub.3 .epsilon.C ##EQU61## where a, . . . , t are expressed as follows: U.sub.4ran U.sub.16ran.sup.-1 1 V.sub.16ran -.sup.1 V.sub.4ran =�abcdefghj! U.sub.4ran U.sub.16ran.sup.-1 x V.sub.16ran -.sup.x V.sub.4ran =�klmnpqrst! a, b, c, d, e, f, g, h, j, k, l, m, n, p, q, r, s, t, .epsilon.R.sup.4 (.epsilon.R.sup.4 stands for that the symbol of the left side thereof is representative of four-dimensional real number vector).

In case of .vertline.det(H).vertline.>.epsilon. (.epsilon.: infinitesimal constant), the following eigenvalue equation is given:

H.sup.-1 G.rho.+x.sub.3 .rho.=0 (36)

(Numerical solutions of eigenvalue equation (generalized eigenvalue equation))

In case of .vertline.det(H).vertline.>.epsilon., the eigenvalue equation expressed by equation (36) is solved in accordance with a QR scheme, and in case of det(H).apprxeq.0, the generalized eigenvalue equation expressed by equation (33) is solved in accordance with a QZ scheme. In this manner, the eigenvalue and the eigenvector are evaluated. With respect to the QR scheme and the QZ scheme, please refer to .left brkt-top.Gene H. Golub, Charles F. van Loan, "Matrix Computations", The John Hopkins University Press.right brkt-bot.. Sets of eigenvalue and the eigenvector exist generally 16 ways, including complex solutions. With respect to the eigenvector, it is normalized in such a manner that the sixteenth component is 1.

At that time, x.sub.3, x.sub.1 and x.sub.2 are derived from the value of the eigenvalue, the twelve component of the eigenvector and the fifth component of the eigenvector, respectively. Hence, the values of the first, second and third joint angles are evaluated from the following equation:

.theta..sub.i.sup.(k) 2 arc tan (x.sub.i.sup.(k)) (37) i=1, 2, 3; k=1, . . . , 16 (Computation of .theta..sub.4.sup.(k), .theta..sub.5.sup.(k), .theta..sub.6.sup.(k))

With respect to .theta..sub.4.sup.(k) and .theta..sub.5.sup.(k), .xi. is evaluated from equation (30); c.sub.i.sup.(k) and s.sub.i.sup.(k) are evaluated using the thirteenth to sixteenth components of vector .xi.; and angles .theta..sub.i.sup.(k) (i=4, 5; k=1, . . . , 16), which are coincident with c.sub.i.sup.(k) and s.sub.i.sup.(k), are determined.

With respect to .theta..sub.6.sup.(k), the values of c.sub.6.sup.(k) and s.sub.6.sup.(k) are evaluated from the following equation:

C.sub.z,6 =A.sub.5.sup.-1 A.sub.4.sup.-1 A.sub.3.sup.-1 A.sub.2.sup.-1 A.sub.1.sup.-1 A.sub.hand C.sub.x,6.sup.T (38) and .theta..sub.6.sup.(k), which is coincident with c.sub.6.sup.(k) and s.sub.6.sup.(k), are determined. (Numerical solution 2)

According to the above-mentioned Numerical solution 1, while the square matrix W is produced on a random basis to normalize U.sub.16ran, here U.sub.num is subjected to the singular value resolution and the square matrix W is determined as follows:

Q.sup.T U.sub.num P=diag(.sigma..sub.1 . . . , .sigma..sub.p).epsilon.R(20.times.16) (39) Q.epsilon.R(20.times.20): orthogonal matrix P.epsilon.R(20.times.16): orthogonal matrix W=Q.sup.T

where U.sub.16ran is normalized.

Subsequent processings are similar to the matter of Numerical solution 1.

(Numerical solution 3)

A solution of the generalized eigenvalue equation needs 2.5 to 3 times of computing time as compared with the usual eigenvalue equation. Therefore, here, there is adopted a scheme in which a direct solution of the generalized eigenvalue equation is avoided as far as possible.

As the square matrix W, the respective components are produced on a random basis in a similar fashion to that of Numerical solution 1.

In equation (33), in case of det(H).apprxeq.0, the following rational transformation is performed instead of solving the generalized eigenvalue equation in accordance with the QZ scheme.

x.sub.3 =(ax.sub.3 '+b)/(cx.sub.3 '+d) (40)

In this case, the generalized eigenvalue equation of equation (33) is expressed as follows:

G'.multidot..rho.+x.sub.3 'H'.multidot..rho.=0 (41) G'=dG+bH H'=cG+aH G', H'.epsilon.R(16.times.16) .rho..epsilon.C.sup.16 x.sub.3 '.epsilon.C

Generally, .vertline.det(H').vertline.>.epsilon.

Thus, the following eigenvalue equation is solved.

H'.sup.-1 G'.multidot..rho.+x.sub.3 '.rho.=0 (42)

In case of det(H').apprxeq.0, the generalized eigenvalue equation of equation (41) is directly solved.

The subsequent processings are similar to the matter of Numerical solution 1.

(Numerical solution 4)

As the square matrix W, there is used Q.sup.T which appears when U.sub.num is subjected to the singular value resolution in Numerical solution 2. The subsequent processings are similar to the matter of Numerical solution 3.

It is possible to perform at high speed the simulation of the manipulator in accordance with the above-mentioned manner. It is possible, for example, to display the result of the simulation on a screen of a display on a real time basis for use in computer graphics, or to transmit the result of the simulation to a manipulator as a hardware so that the manipulator operates in accordance with the simulation which is carried out on a real time basis.

Next, there will be explained an embodiment of the second manipulator simulation method according to the present invention. Hereinafter, this embodiment will be referred to as "the second embodiment". Also in the second embodiment, the flowchart of FIG. 1 is applicable. Thus, the second embodiment will be explained referring to the flowchart of FIG. 1.

Here, first, in step (a), a matricial equation in the above-mentioned equation (4) is evaluated.

The forward kinematics equation of the manipulator is given by the above-mentioned equation (11), that is:

A.sub.hand =A.sub.1 .multidot.A.sub.2 .multidot.A.sub.3 .multidot.A.sub.4 .multidot.A.sub.5 .multidot.A.sub.6 (11)

Here, in a similar fashion to that of the first embodiment, 14 fundamental equations (2 scalar equations and 4 vector equations) are derived in accordance with the above equation (11), and then further 6 fundamental equations are derived using the resultant 14 fundamental equations. The combination of the total 20 fundamental equations produces the matricial equation given by the above equation (4).

(Derivation of 14 fundamental equations)

The above equation (14) is rewritten as follows.

A.sub.3 .multidot.A.sub.4 .multidot.A.sub.5 =A.sub.2.sup.-1 .multidot.A.sub.1.sup.-1 .multidot.A.sub.hand .multidot.A.sub.6.sup.-1(43)

First, the following fundamental vector equations are derived based on the equation (43).

z.sub.A .ident.A.sub.3 A.sub.4 A.sub.5 e.sub.3 =A.sub.2.sup.-1 A.sub.1.sup.-1 A.sub.hand A.sub.6.sup.-1 e.sub.3 .ident.z.sub.B(44) p.sub.A .ident.A.sub.3 A.sub.4 A.sub.5 e.sub.4 =A.sub.2.sup.-1 A.sub.1.sup.-1 A.sub.hand A.sub.6.sup.-1 e.sub.4 .ident.p.sub.B(45) where e.sub.i (i=1 . . . 4) denote unit vectors. In equations (44) and (45), the fourth component becomes an obvious equation like 1=1, 0=0. Thus, hereinafter, the fourth component will be neglected, and it is assumed that equations (44) and (45) denote each a three-dimensional vector comprising the first component, the second component and the third component.

Taking the internal product, the external product and the vector sum based on p and z of equations (44) and (45) may derive the following equations.

(p.sup.T .multidot.p).sub.A =(p.sup.T .multidot.p).sub.B (46) (p.sup.T .multidot.z).sub.A =(p.sup.T .multidot.z).sub.B (47) (p.sup.T .times.z).sub.A =(p.sup.T .times.z).sub.B (48) ##EQU62##

Equation (49) may be rewritten as follows. ##EQU63##

As a result of the precise analysis using the mathematical expression processing, it is understood that equations (44) to (49) or (50) are of linear with respect to cos.theta..sub.i, sin.theta..sub.i. On the other hand, other combinations each comprising p and z lack a linearity.

To apply the mathematical expression processing, equations (44) to (49) or (50) are resolved beforehand using equations (8) and (9) as follows. ##EQU64##

(Property of the fundamental equations)

Fundamental equations (51) to (56) have the following characteristics.

(a) The respective elements of the matrix A.sub.hand representative of the position and attitude of the tip of the manipulator appears only at the right side member. (b) All the equations do not depend on .theta..sub.6.

Because, in equations (51) to (56) .theta..sub.6 appears in the form of C.sub.6.sup.T e.sub.3 =C.sub.x,6.sup.T C.sub.z,6.sup.T e.sub.3, but C.sub.z,6.sup.T e.sub.3 =�0 0 1!.sup.T.

(c) z.sub.3, p.sub.3, p.sup.T .multidot.z, p.sup.T .multidot.p, (p.sup.T .times.z).sub.3, �2(p.sup.T .multidot.z)p-(p.sup.T .multidot.p)z!.sub.3 does not depend on .theta..sub.3.

Because, the items (p.sup.T .multidot.z) and (p.sup.T .multidot.p) do not depend on C.sub.z,3. With respect to z.sub.3, (p.sup.T.times.z).sub.3, �2(p.sup.T .multidot.z)p-(p.sup.T .multidot.p)z!.sub.3, .theta..sub.3 always appears in the form of e.sub.3.sup.T C.sub.z,3, but e.sub.3.sup.T C.sub.z,3 =e.sub.3.sub.3.sup.T.

(d) z.sub.1, z.sub.2, p.sub.1, p.sub.2, (p.sup.T .times.z).sub.1, (p.sup.T .times.z).sub.2, �2(p.sup.T .multidot.z)p-(p.sup.T .multidot.p)z!.sub.1, �2(p.sup.T .multidot.z)p-(p.sup.T .multidot.p)z!.sub.2 is linear with respect to tan(.theta..sub.3 /2). Because, when x.sub.i =tan(.theta..sub.i /2), the following equations consist: cos .theta..sub.i =(1-x.sub.i.sup.2)/(1+x.sub.i.sup.2) sin .theta..sub.i =2x.sub.i /(1+x.sub.i.sup.2) X.sub.3.sup.- .multidot.C.sub.z,3 =X.sub.3.sup.+ ##EQU65##

The above results can be obtained by means of multiplying, in z, p, p.sup.T .times.z, 2(p.sup.T .multidot.z)p-(p.sup.T .multidot.p)z, by X.sub.3.sup.- from the left.

(e) .theta..sub.1, .theta..sub.2, .theta..sub.4, .theta..sub.5 always appear in the form of a linearity with respect to cos.theta..sub.i, sin.theta..sub.i (i=1, 2, 4, 5), but the items of (cos.theta..sub.i).sup.2, (sin.theta..sub.i).sup.2, cos.theta..sub.i .multidot.sin.theta..sub.i do not appear.

Because, in z, p, p.sup.T .multidot.p, p.sup.T .multidot.z, p.sup.T .times.z, there simply appears C.sub.z,i once for each item. Further, with respect to 2(p.sup.T .multidot.z)p-(p.sup.T .multidot.p)z, it is understood that this is of linear through such a process that the values of p, z, p.sup.T .multidot.z, p.sup.T .multidot.p are substituted and arranged, taking account of (cos.theta..sub.i).sup.2 +(sin.theta..sub.i).sup.2 =1,(cos.alpha..sub.i).sup.2 +(sin.alpha..sub.i).sup.2 =1, using a mathematical expression processing.

(Matrix form of fundamental equations)

The fundamental equations of equations (51) to (56) can be expressed in the matrix form as set forth below. ##EQU66##

The components of U' and V' can be obtained through such a process that equations (51) to (56) are carefully subjected to a mathematical expression processing; (cos.theta..sub.i).sup.2 +(sin.theta..sub.i).sup.2 =1(i=1, 2), (cos.alpha..sub.i).sup.2 +(sin.alpha..sub.i).sup.2 =1(i=1, . . . , 6), cos.theta..sub.i =(1-x.sub.i.sup.2)/(1+x.sub.i.sup.2), sin.theta..sub.i =2x.sub.i /(1+x.sub.i.sup.2) (i=3, 4, 5) are substituted; and in the resultant algebraic equations the coefficients for the respective items of .xi.' and .rho. are evaluated.

Each component of U' and V' is a polynomial of several variables in which the components of parameters t.sub.i and A.sub.hand are given in the form of variables.

Incidentally, in the second embodiment, i=4, 5 are substituted by i=1, 2 as compared with the first embodiment.

(Addition of fundamental equations)

With respect to z.sub.3, p.sub.3, p.sup.T .multidot.z, p.sup.T .multidot.p, (p.sup.T .times.z).sub.3, �2(p.sup.T .multidot.z)p-(p.sup.T .multidot.p)z!.sub.3, they do not depend on .theta..sub.3. Hence it is possible to derive 6 equations as set forth below by means of being multiplied by x.sub.3 =tan(.theta..sub.3 /2) and taking the linear sum.

�x.sub.3 z.sub.3 +p.sub.3 !.sub.A =�x.sub.3 z.sub.3 +p.sub.3 !.sub.B(59) �x.sub.3 p.sub.3 +z.sub.3 !.sub.A =�x.sub.3 p.sub.3 +z.sub.3 !.sub.B(60) �x.sub.3 p.sup.T .multidot.p+p.sup.T .multidot.z!.sub.A =�x.sub.3 p.sup.T .multidot.p+p.sup.T .multidot.z!.sub.B (61) �x.sub.3 p.sup.T .multidot.z+p.sup.T .multidot.p!.sub.A =�x.sub.3 p.sup.T .multidot.z+p.sup.T .multidot.p!.sub.B (62) �x.sub.3 (p.sup.T .times.z).sub.3 +z.sub.3 !.sub.A =�x.sub.3 (p.sup.T .times.z).sub.3 +z.sub.3 !.sub.B (63) ##EQU67##

When equations (59) to (64) are arranged in a similar fashion to that of equation (58), in which a part of the left side member is transferred to the right side member to be rearranged together with equation (58), there is obtained a matrix equation expressed by the above-mentioned equation (4). ##EQU68##

Incidentally, also in the second embodiment, in a similar fashion to that of the above-mentioned first embodiment, it is acceptable that there is adopted as parameter t.sub.i the above-mentioned equation (10), that is, the D-H parameter as set forth below.

t.sub.i =�a.sub.i 0d.sub.i !.sup.T (10)

Further, it is acceptable that instead of the above-mentioned equation (10), there is adopted as parameter t.sub.i a so-called modification D-H parameter as set forth below.

t.sub.i =�.xi..sub.i .eta..sub.i .zeta..sub.i +.gamma..sub.i !.sup.T(28)

The above-mentioned matters concern the explanation of step (a) of FIG. 1 in connection with the second embodiment.

With respect to the process of step (b) as to the second embodiment, it is simply different from that of the first embodiment in only the point that the numerical substitution of the respective components of the parameter t.sub.i into the matrix equation expressed by equation (4) is performed, instead of the point that the numerical substitution of the respective components of the parameter t.sub.i into the matrix equation expressed by equation (1) is performed in accordance with the first embodiment. Thus, redundant explanation will be omitted.

The numerical substitution of the respective components of the parameter t.sub.i into the matrix equation expressed by equation (4) makes it possible to derive the matrix equation expressed by the above-mentioned equation (5) in which the respective components of A.sub.hand are given as variables, that is, the equation as set forth below. ##EQU69##

The matrix equations using the numerical processing terminate at equation (5), that is, step (c) in FIG. 1, and thereafter the whole processes deal with the numerical matrix.

That is, a numerical matrix equation with the numerical matrices U.sub.num, V.sub.num is evaluated (step (e) in FIG. 1) through a numerical substitution (step (d) in FIG. 1) of the respective components of A.sub.hand representative of the position and the attitude of the tip of the manipulator into equation (2), and angles .theta..sub.i.sup.(k) of rotation (k stands for positive integers to the maximum 16 for discriminating mutually between a plurality of angles .theta..sub.i) of the respective joints i are evaluated on the basis of the numerical matrix equation (step (f) in FIG. 1). This numerical matrix equation is expressed by the above-mentioned equation (6), that is, ##EQU70##

Repetition of step (d) to step (f) makes it possible to perform various simulations on the manipulator model.

Next, there will be explained details of a numerical solution of .theta..sub.i.sup.(k), that is, step (f) in FIG. 1 on the basis of the numerical matrix equation given by equation (6).

FIG. 4 is a detailed flowchart of step (f) in FIG. 1 regarding the second embodiment.

While some numerical solutions will be explained hereinafter, all the numerical solutions comply with step (k) to step (n) in FIG. 4. That is, in any solutions which will be explained hereinafter, to determine .theta..sub.i.sup.(k), first, equations, in which .theta..sub.1 and .theta..sub.2 are eliminated, are evaluated on the basis of the numerical matrix equation given by equation (6) (step (k)). Next, eigenvalue equations or generalized eigenvalue equations are evaluated on the basis of the resultant equations in which .theta..sub.1 and .theta..sub.2 are eliminated (step (l)). Thereafter, .theta..sub.3.sup.(k), .theta..sub.4.sup.(k), .theta..sub.5.sup.(k) are evaluated on the basis of the resultant eigenvalue equations or generalized eigenvalue equations (step (m)). Then, further, .theta..sub.1.sup.(k), .theta..sub.2.sup.(k), .theta..sub.6.sup.(k) are evaluated (step (n)).

Difference between the second embodiment and the first embodiment resides in the point that first, .theta..sub.3.sup.(k), .theta..sub.4.sup.(k), .theta..sub.5.sup.(k) are evaluated, and then .theta..sub.1.sup.(k), .theta..sub.2.sup.(k), .theta..sub.6.sup.(k) are evaluated. Specifically, the difference between both the embodiments appears in the form of a difference in components of the U and V matrices. In general, according to the second embodiment, a rank of the U and V matrices is high as compared with the first embodiment. This feature may involve a stability in numerical computation.

Hereinafter, there will be explained several numerical solutions according to the flowchart of FIG. 4.

(Numerical solution 5)

(Elimination of .theta..sub.1 and .theta..sub.2)

To erase .theta..sub.1 and .theta..sub.2 from the matricial equation given by equation (6), it is multiplied by 20.times.20 of real matrix W from the left of the equation. The respective components of the real matrix W are produced on a random basis. When the followings are given,

WU.sub.num .ident.U.sub.ran =�U.sub.16ran, U.sub.4ran !.sup.T, U.sub.16ran .epsilon.R(16.times.16), U.sub.4ran .epsilon.R(4.times.16), WV.sub.num .ident.V.sub.ran =�V.sub.16ran, V.sub.4ran !.sup.T, V.sub.16ran .epsilon.R(16.times.18), V.sub.4ran .epsilon.R(4.times.18), .vertline.det(U.sub.16ran).vertline.>.epsilon. (.epsilon.: infinitesimal constant) is to be satisfied.

If det(U.sub.16ran).apprxeq.0, the components of the real matrix W is to be reset.

At that time, equation (6) is expressed as follows: ##EQU71##

Hence, ##EQU72##

This is substituted into the remaining four equations of equation (65), so that .theta..sub.1 and .theta..sub.2 are eliminated. Thus, the following equation is formed. ##EQU73##

(Derivation of eigenvalue equation (generalized eigenvalue equation))

When the following equation is given,

V.sub.16ran =�.sup.x V.sub.16ran, .sup.1 V.sub.16ran!, V.sub.4ran =�.sup.x V.sub.4ran, .sup.1 V.sub.4ran!, .sup.x V.sub.16ran, .sup.1 V.sub.16ran .epsilon.R(16.times.9), .sup.x V.sub.4ran, .sup.1 V.sub.4ran .epsilon.R(4.times.9) the above equation (67) is expressed by ##EQU74##

When both the side members of equation (68) are multiplied by x.sub.4 x.sub.5, x.sub.4, x.sub.5, further 12 equations are formed, so that total 16 equations are derived. These are expressed by:

G.multidot..rho.+x.sub.3 H.multidot..rho.=0 (69) G, H.epsilon.R(16.times.16) .rho..epsilon.C.sup.16 x.sub.3 .epsilon.C ##EQU75## where a, . . . , t are expressed as follows: U.sub.4ran U.sub.16ran.sup.-1 1 V.sub.16ran -.sup.1 V.sub.4ran =�abcdefghj! U.sub.4ran U.sub.16ran.sup.-1 x V.sub.16ran -.sup.x V.sub.4ran =�klmnpqrst! a, b, c, d, e, f, g, h, j, k, l, m, n, p, q, r, s, t, .epsilon.R.sup.4.

In case of .vertline.det(H).vertline.>.epsilon. (.epsilon.: infinitesimal constant), the following eigenvalue equation is given:

H.sup.-1 G.rho.+x.sub.3 .rho.=0 (72)

(Numerical solutions of eigenvalue equation (generalized eigenvalue equation))

In case of .vertline.det(H).vertline.>.epsilon., the eigenvalue equation expressed by equation (72) is solved in accordance with a QR scheme, and in case of det(H).apprxeq.0, the generalized eigenvalue equation expressed by equation (69) is solved in accordance with a QZ scheme. In this manner, the eigenvalue and the eigenvector are evaluated. With respect to the QR scheme and the QZ scheme, please refer to the above-referenced .left brkt-top.Gene H. Golub, Charles F. van Loan "Matrix Computations", The John Hopkins University Press.right brkt-bot.. Sets of eigenvalue and the eigenvector exist generally 16 ways, including complex solutions. With respect to the eigenvector, it is normalized in such a manner that the sixteenth component is 1.

At that time, x.sub.3, x.sub.4 and x.sub.5 are derived from the value of the eigenvalue, the twelve component of the eigenvector and the fifth component of the eigenvector, respectively. Hence, the values of the first, second and third joint angles are evaluated from the following equation:

.theta..sub.i.sup.(k) =2 arc tan (x.sub.i.sup.(k)) (73) i=3, 4, 5; k=1, . . . , 16 (Computation of .theta..sub.1.sup.(k), .theta..sub.2.sup.(k), .theta..sub.6.sup.(k))

With respect to .theta..sub.1.sup.(k) and .theta..sub.2.sup.(k), .xi. is evaluated from equation (66); c.sub.i.sup.(k) and s.sub.i.sup.(k) are evaluated using the thirteenth to sixteenth components of vector .xi.; and angles .theta..sub.i.sup.(k) (i=1, 2; k=1, . . . , 16), which are coincident with c.sub.i.sup.(k) and s.sub.i.sup.(k), are determined.

With respect to .theta..sub.6.sup.(k), the values of c.sub.6.sup.(k) and s.sub.6.sup.(k) are evaluated from the following equation:

C.sub.z,6 =A.sub.5.sup.-1 A.sub.4.sup.-1 A.sub.3.sup.-1 A.sub.2.sup.-1 A.sub.1.sup.-1 A.sub.hand C.sub.x,6.sup.T (74) and .theta..sub.6.sup.(k), which is coincident with c.sub.6.sup.(k) and s.sub.6.sup.(k), are determined. (Numerical solution 6)

According to the above-mentioned Numerical solution 5, while the square matrix W is produced on a random basis to normalizes U.sub.16tan, here, U.sub.num is subjected to the singular value resolution and the square matrix W is determined as follows:

Q.sup.T U.sub.num P=diag(.sigma..sub.1 . . . , .sigma..sub.p).epsilon.R(20.times.16) (75) Q.epsilon.R(20.times.20): orthogonal matrix P.epsilon.R(20.times.16): orthogonal matrix W=Q.sup.T

Where U.sub.16ran is normalized.

Subsequent processings are similar to the matter of Numerical solution 5.

(Numerical solution 7)

A solution of the generalized eigenvalue equation needs 2.5 to 3 times of computing time as compared with the usual eigenvalue equation. Therefore, here, there is adopted a scheme in which a direct solution of the generalized eigenvalue equation is avoided as far as possible.

As the square matrix W, the respective components are produced on a random basis in a similar fashion to that of Numerical solution 5.

In equation (69), in case of det(H).apprxeq.0, the following rational transformation is performed instead of solving the generalized eigenvalue equation in accordance with the QZ scheme.

x.sub.3 =(ax.sub.3 '+b)/(cx.sub.3 '+d) (76)

In this case, the generalized eigenvalue equation of equation (69) is expressed as follows:

G'.multidot..rho.+x.sub.3 'H'.multidot..rho.=0 (77) G'=dG+bH H'=cG+aH G', H'.epsilon.R(16.times.16) .rho..epsilon.C.sup.16 x.sub.3 '.epsilon.C

Generally, .vertline.det(H').vertline.>.epsilon.

Thus, the following eigenvalue equation is solved.

H'.sup.-1 G'.multidot..rho.+x.sub.3 '.rho.=0 (78)

In case of det(H').apprxeq.0, the generalized eigenvalue equation of equation (77 is directly solved.

The subsequent processings are similar to the matter of Numerical solution 5.

(Numerical solution 8)

As the square matrix W, there is used Q.sup.T which appears when U.sub.num is subjected to the singular value resolution in Numerical solution 6. The subsequent processings are similar to the matter of Numerical solution 7.

(Numerical solution 9)

Here, .xi. is evaluated directly from equation (6) instead of passing through equation (66) in the process in which .theta..sub.1 and .theta..sub.2 are eliminated from equation (6) in step (k) of FIG. 3.

In equation (6), while U.sub.num is not the square matrix, its rank is 16. Consequently, when Gaussian elimination method is applied to U.sub.num, the matrix U.sub.num may be transformed as follows: ##EQU76##

When a method of elimination of equation (79) is applied to both the side members of equation (6), equation (6) may be transformed as follows: ##EQU77##

V'.sub.16num .epsilon.R(16.times.18), V'.sub.4num .epsilon.R(4.times.18)

The following equation may be derived from equation (80). ##EQU78##

V'.sub.4num is resolved as follows:

V'.sub.4num =�.sup.x V'.sub.4ran, .sup.1 V'.sub.4ran ! .sup.x V'.sub.4ran, .sup.1 V'.sub.4ran .epsilon.R(4.times.9)

And when the following equations are given,

.sup.1 V'.sub.4ran =�abcdefghj! .sup.x V'.sub.4ran =�klmnpqrst! there are obtained the eigenvalue equations similar to equations (69)-(71).

The subsequent processings are similar to the numerical solution 5.

An adoption of the above-mentioned numerical solution 9 makes it possible to desirably maintain a regularity (det(H).noteq.0) of matrix H as compared with the numerical solutions 5 to 8, thereby evaluating the solutions of the eigenvalue equations with greater accuracy.

(Numerical solution 10)

When the regularity of the matrix H determined in numerical solution 9 is not maintained, a rational transformation of equation (76) is performed. The subsequent processings are similar to the numerical solution 7.

Also in the second embodiment, similar to the first embodiment, it is possible to perform at high speed the simulation of the manipulator in accordance with the above-mentioned manner. Therefore, it is possible to display the result of the simulation on a screen of a display, while the simulation is performed on a real time basis in computer graphics, and also to control a manipulator as a hardware, while the simulation is performed on a real time basis.

FIG. 5 is a block diagram showing an embodiment of the first manipulator simulation apparatus according to the pre sent invention. Hereinafter, this embodiment is referred to as "third embodiment". With respect to the operational algorithm, it has been described in detail in connection with the first embodiment and the second embodiment. Consequently, in the third embodiment and the subsequent embodiments, there will be omitted the detailed description of the operational algorithm, and in case of need the number of the above-mentioned mathematical expressions is simply referred to.

A manipulator simulation apparatus shown in FIG. 5 is provided with storage means 11 for storing the above-mentioned matrix equation (1). While it is acceptable that the storage means 11 is one for storing the matrix equation (1) on a fixed storage basis, there is no need to adopt such a type of storage means. It is also acceptable to adopt storage means for storing the matrix equation (1) through loading the same from an external storage medium such as a floppy disk.

The matrix equation (1) stored in storage means 11 is read out on parameter numerical value substitution means 12 in which the numerical values of the respective components of parameter t.sub.i inputted from input means 13 are substituted into the read matrix equation (1) thereby producing the above-mentioned matrix equation (2). While it is acceptable that the input means 13 may be, for example, a keyboard and the like, one for manually inputting the respective components of parameter t.sub.i, the input means 13 is not restricted to such a type of input means as mentioned above. It is also acceptable to adopt, for example, one in which numerical information of the respective components of parameter t.sub.i stored beforehand is read out to be loaded into the parameter numerical value substitution means 12, or another in which numerical information of the respective components of parameter t.sub.i transmitted from the exterior passing through communication lines is received and transferred to the parameter numerical value substitution means 12.

The matrix equation (2) produced by the parameter numerical value substitution means 12 is inputted to numerical value matrix equation derivation means 14 in which the numerical values of the respective components (elements) of A.sub.hand inputted from input means 15 are substituted into the matrix equation (2) thereby producing the above-mentioned numerical matrix equation (3). The input means 15 may also be given with various constitutions in a similar fashion to that of the above-mentioned input means 13. It is acceptable that the input means 15 is physically the same as the above-mentioned input means 13.

The numerical matrix equation (3) derived by the numerical value matrix equation derivation means 14 is inputted to a joint rotation angle computing means 16 in which rotation angle .theta..sub.i.sup.(k) of the joint is evaluated in accordance with any one of the above-mentioned numerical value solutions 1 to 4, or their combination.

The rotation angle .theta..sub.i.sup.(k) of the joint thus evaluated is transmitted to a display means 17 to be displayed on a computer graphics display basis, and/or transmitted to a control means 18 to be used for a control of the rotation angle of the joint of a manipulator as the actual hardware.

It is sufficient for the manipulator simulation apparatus, at the time of the actual simulation except for an initial preparation, to repeat solving the numerical matrix equation adopting the numerical value input and any one of the above-mentioned numerical value solutions 1 to 4, thereby implementing a high speed simulation.

FIG. 6 is a detailed block diagram of the joint rotation angle computing means 16 in the third embodiment as shown in FIG. 5.

The joint rotation angle computing means 16 comprises means 161 for eliminating .theta..sub.4 and .theta..sub.5 on the basis of the numerical matrix equation given by equation (3), means 162 for determining eigenvalue equations or generalized eigenvalue equations on the basis of the resultant equations in which .theta..sub.4 and .theta..sub.5 are eliminated, means 163 for computing .theta..sub.3.sup.(k), .theta..sub.2.sup.(k), .theta..sub.1.sup.(k) on the basis of the resultant eigenvalue equations or generalized eigenvalue equations, and means 164 for computing .theta..sub.4.sup.(k), .theta..sub.5.sup.(k), .theta..sub.6.sup.(k). The algorithm to be effected in each of means 161 to 164 has been explained in connection with the above-mentioned numerical value solutions 1 to 4. Consequently, the redundant explanation will be omitted.

FIG. 7 is a block diagram showing an embodiment of the second manipulator simulation apparatus according to the present invention. Hereinafter, this embodiment will be referred to as the fourth embodiment.

The manipulator simulation apparatus shown in FIG. 7, is provided with storage means 19 for storing the above-mentioned matrix equation (2), that is, the matrix equation after numerical entry of the respective components of the parameter t.sub.i. Other arrangements are similar to that of the third embodiment, and thus the redundant explanation will be omitted.

While it is acceptable that the storage means 19 is, similar to the storage means 11 in the third embodiment of FIG. 5, one for storing the matrix equation (2) on a fixed storage basis, there is no need to adopt such a type of storage means as mentioned above. It is also acceptable to adopt storage means for temporarily storing during the simulation the matrix equation (2) loaded from an external storage medium such as a floppy disk, or the matrix equation (2) produced by arithmetic operation means not illustrated.

When there is a need to perform a simulation of various constitutions of manipulator, as in the third embodiment of FIG. 5, it is necessary to input numerical values as to the respective components of the parameter t.sub.i suitable for the manipulator on which the simulation is intended to be effected. However, in case of apparatuses for implementing the simulation of the existing specified manipulator, for example, an apparatus which is exclusively used for a control of the manipulator as the specified existing hardware, there is no need to provide such an arrangement that the respective components of the parameter t.sub.i can be entered in the form of the numerical value. It is acceptable, as in the fourth embodiment shown in FIG. 7, to store beforehand the matrix equation (2) in which the respective components of the parameter t.sub.i as to the specified manipulator are entered in the form of the numerical value.

Next, there will be explained an embodiment of the third manipulator simulation apparatus according to the present invention. Hereinafter, this embodiment will be referred to as the fifth embodiment.

The block diagram of the fifth embodiment is the same as that of the third embodiment shown in FIG. 5. Consequently, the fifth embodiment will be explained again referring to FIG. 5 hereinafter.

A manipulator simulation apparatus, as the fifth embodiment, shown in FIG. 5 is provided with storage means 11 for storing the above-mentioned matrix equation (1). While it is acceptable that the storage means 11 is one for storing the matrix equation (1) on a fixed storage basis, there is no need to adopt such a type of storage means. It is also acceptable to adopt storage means for storing the matrix equation (1)through loading the same from an external storage medium such as a floppy disk.

The matrix equation (4) stored in storage means 11 is read out on parameter numerical value substitution means 12 in which the numerical values of the respective components of parameter t.sub.i inputted from input means 13 are substituted into the read matrix equation (4) thereby producing the above-mentioned matrix equation (5). While it is acceptable that the input means 13 may be, for example, a keyboard and the like, one for manually inputting the respective components of parameter t.sub.i, the input means 13 is not restricted to such a type of input means as mentioned above. It is also acceptable to adopt, for example, one in which numerical information of the respective components of parameter t.sub.i stored beforehand is read out to be loaded into the parameter numerical value substitution means 12, or another in which numerical information of the respective components of parameter t.sub.i transmitted from the exterior passing through communication lines is received and transferred to the parameter numerical value substitution means 12.

The matrix equation (5) produced by the parameter numerical value substitution means 12 is inputted to numerical value matrix equation derivation means 14 in which the numerical values of the respective components (elements) of A.sub.hand inputted from input means 15 are substituted into the matrix equation (5) thereby producing the above-mentioned numerical matrix equation (6). The input means 15 may also be given with various constitutions in a similar fashion to that of the above-mentioned input means 13. It is acceptable that the input means 15 is physically the same as the above-mentioned input means 13.

The numerical matrix equation (6) derived by the numerical value matrix equation derivation means 14 is inputted to a joint rotation angle computing means 16 in which rotation angle .theta..sub.i.sup.(k) of the joint is evaluated in accordance with any one of the above-mentioned numerical value solutions 5 to 10 of the equation (6), or their combination.

The rotation angle .theta..sub.i.sup.(k) of the joint thus evaluated is transmitted to a display means 17 to be displayed on a computer graphics display basis, and/or transmitted to a control means 18 to be used for a control of the rotation angle of the joint of a manipulator as the actual hardware.

It is sufficient for the manipulator simulation apparatus, at the time of the actual simulation except for an initial preparation, to repeat solving the numerical matrix equation adopting the numerical value input and any one of the above-mentioned numerical value solutions 5 to 10, thereby implementing a high speed simulation.

FIG. 8 is a detailed block diagram of the joint rotation angle computing means 16 in the fifth embodiment as shown in FIG. 5.

The joint rotation angle computing means 16 comprises means 165 for eliminating .theta..sub.1 and .theta..sub.2 on the basis of the numerical matrix equation given by equation (6), means 166 for determining eigenvalue equations or generalized eigenvalue equations on the basis of the resultant equations in which .theta..sub.1 and t.sub.2 are eliminated, means 167 for computing .theta..sub.3.sup.(k), .theta..sub.2.sup.(k), .theta..sub.1.sup.(k) on the basis of the resultant eigenvalue equations or generalized eigenvalue equations, and means 168 for computing .theta..sub.1.sup.(k), .theta..sub.2.sup.(k), .theta..sub.6.sup.(k). The algorithm to be effected in each of means 165 to 168 has been explained in connection with the above-mentioned numerical value solutions 5 to 10. Consequently, the redundant explanation will be omitted.

Next, there will be explained an embodiment of the fourth manipulator simulation apparatus according to the present invention. Hereinafter, this embodiment will be referred to as the sixth embodiment.

The block diagram of the sixth embodiment is the same as that of the fourth embodiment shown in FIG. 7. Consequently, the sixth embodiment will be explained again referring to FIG. 7 hereinafter.

The manipulator simulation apparatus shown in FIG. 7 according to the sixth embodiment, is provided with storage means 19 for storing the above-mentioned matrix equation (5), that is, the matrix equation after numerical entry of the respective components of the parameter t.sub.i. Other arrangements are similar to that of the fifth embodiment shown in FIG. 5, and thus the redundant explanation will be omitted.

While it is acceptable that the storage means 19 is, similar to the storage means 11 in the third embodiment of FIG. 5, one for storing the matrix equation (5) on a fixed storage basis, there is no need to adopt such a type of storage means as mentioned above. It is also acceptable to adopt storage means for temporarily storing during the simulation the matrix equation (5) loaded from an external storage medium such as a floppy disk, or the matrix equation (5) produced by arithmetic operation means not illustrated.

When there is a need to perform a simulation of various constitutions of manipulator, as in the fifth embodiment of FIG. 5, it is necessary to input numerical values as to the respective components of the parameter t.sub.i suitable for the manipulator on which the simulation is intended to be effected. However, in case of apparatuses for implementing the simulation of the existing specified manipulator, for example, an apparatus which is exclusively used for a control of the manipulator as the specified existing hardware, there is no need to provide such an arrangement that the respective components of the parameter t.sub.i can be entered in the form of the numerical value. It is acceptable, as in the sixth embodiment shown in FIG. 7, to store beforehand the matrix equation (5) in which the respective components of the parameter t.sub.i as to the specified manipulator are entered in the form of the numerical value.

As described above, according to the present invention, objects, which can be processed in accordance with the mathematical expression processing, are processed in accordance with the mathematical expression processing as much as possible, and finally only portions, which are obliged to be processed in accordance with the numerical value solutions, are processed in accordance with the numerical value processing through inputting A.sub.hand. This feature makes it possible to reduce an operating time, and thus the system according to the present invention is suitable for a real time processing.

While the present invention has been described with reference to the particular illustrative embodiments, it is not to be restricted by those embodiments but only by the appended claims. It is to be appreciated that those skilled in the art can change or modify the embodiments without departing from the scope and spirit of the present invention.

Claims

1. A manipulator simulation method, for use on a computer, adapted to evaluate angles of rotation of joints for coupling arm-to-arm to each other on the basis of information representative of position and orientation of a tip of a manipulator having six or less arms, which are sequentially coupled with each other, so as to implement the position and the orientation of the tip of the manipulator, the method comprising:

when a homogeneous coordinates transformation matrix A.sub.hand representative of the position and the orientation of the tip of the manipulator using an absolute coordinate system expressed by ##EQU79## where C.sub.hand denotes an orientation, and

t.sub.hand denotes a position,

and the homogeneous coordinates transformation matrix A, representative of the position and the orientation of the (i+1)th coordinate system using the i-th coordinate system, of all the coordinate systems sequentially assigned from the rear end of the manipulator toward the top, is expressed by ##EQU80## where: i=1, 2, . . . , 6

C.sub.i =C.sub.z,i .multidot.C.sub.x,i ##EQU81## c.sub.i =cos.theta..sub.i, S.sub.i =sin.theta..sub.i .lambda..sub.i =cos.alpha..sub.i, .mu..sub.i =sin.alpha..sub.i

.theta..sub.i denotes an angle of rotation of the i-th joint,

.alpha..sub.i denotes an angle of gradient of axis of rotation of i-th joint to an axis of rotation of i-1th joint,

t.sub.i denotes a three-dimensional parameter defining a configuration of i-th link, and the homogeneous coordinates transformation matrix A.sub.i may be a unit matrix;

evaluating a matrix equation (1) based on said homogeneous coordinates transformation matrix A.sub.hand and said homogeneous coordinates transformation matrix A.sub.i, matrix equation (1) being: ##EQU82## where U denotes 20.times.16 of real number matrix, V denotes 20.times.18 real number matrix, ##EQU83## X.sub.i =tan(.theta..sub.i /2); deriving a matrix equation (2) by a numerical value substitution or respective components of the parameter t.sub.i into the matrix equation expressed by equation (1) in which respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, matrix equation (2) being: ##EQU84## where U(A.sub.hand) and V(A.sub.hand) stand for matrices in which the respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, said matrices being produced by numerical value substitution of respective components of the parameter t.sub.i into said matrices U and V, respectively;

deriving a numerical value matrix equation (3) by a numerical value substitution of respective components of said homogeneous coordinates transformation matrix A.sub.hand into the matrix equation expressed by equation (2), matrix equation (3) being: ##EQU85## where U.sub.num and V.sub.num stand for numerical value matrices produced by numerical value substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand into said matrices U(A.sub.hand) and V(A.sub.hand), respectively;

evaluating angles .theta..sub.i.sup.(k) of rotation, where k stands for positive integers to the maximum 16 for discriminating mutually between a plurality of angles .theta..sub.i, of the joints on the basis of the numerical value matrix equation expressed by equation (3); and

determining whether a manipulator constructed based on the simulated manipulator will operate as desired and is capable of implementing a desired position and orientation of the tip of the manipulator.

2. A manipulator simulation method according to claim 1, wherein to evaluate said angles .theta..sub.i.sup.(k) of rotation on the basis of the numerical value matrix equation expressed by equation (3):

equations, in which .theta..sub.4 and .theta..sub.5 are eliminated, and evaluated on the basis of the numerical value matrix equation given by equation (3);

eigenvalue equations or generalized eigenvalue equations are evaluated on the basis of the resultant equations in which .theta..sub.4 and .theta..sub.5 are eliminated; and

said angles .theta..sub.i.sup.(k) of rotation are evaluated on the basis of the resultant eigenvalue equations or generalized eigenvalue equations.

3. A manipulator simulation method, for use on a computer, adapted to evaluate angles of rotation of joints for coupling arm-to-arm to each other on the basis of information representative of position and orientation of a tip of a manipulator having six or less arms, which are sequentially coupled with each other, so as to implement the position and the orientation of the tip of the manipulator, the method comprising:

when a homogeneous coordinates transformation matrix A.sub.hand representative of the position and the orientation of the tip of the manipulator using an absolute coordinate system is expressed by ##EQU86## where C.sub.hand denotes an orientation, and

t.sub.hand denotes a position,

and the homogeneous coordinates transformation matrix A.sub.i representative of the position and the orientation of the (1+I)th coordinate system looking a the i-th cordinate system, of all the coordinate systems sequentially assigned form the rear end of the maipulator toward the top, is expressed by ##EQU87## where i=1, 2, . . . , 6

C=C.sub.z,i .multidot.C.sub.x,i ##EQU88## c.sub.i =cos.theta..sub.i, S.sub.i =sin.theta..sub.i .lambda..sub.i =cos.alpha..sub.i, .mu..sub.i =sin.alpha..sub.i

.theta..sub.i denotes an angle of rotation of i-th joint,

.alpha..sub.i denotes an angle of gradient of axis of rotation of i-th joint to an axis of rotation of i-1th joint,

t.sub.i denotes a three-dimensional parameter defining a configuration of i-th link, and the homogeneous coordinates transformation matrix A.sub.i may be a unit matrix;

evaluating a matrix equation (1) based on said homogeneous coordinates transformation matrix A.sub.hand and said homogeneous coordinates tranformation matrix A.sub.i, matrix equation (1) being: ##EQU89## where U denotes 20.times.16 of real number matrix, V denotes 20.times.18 or real number matrix, ##EQU90## X.sub.i =tan(.theta..sub.i /2); deriving a matrix equation (5) by a numerical value substitution of respective components of the parameter t.sub.i into the matrix equation expressed by equation (4) in which respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, matrix equation (5) being: ##EQU91## where U(A.sub.hand) and V(A.sub.hand) stand for matrices in which the respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, said matrices being produced by a numerical value substitution of respective components of the parameter t.sub.i into said matrices U and V, respectively;

deriving a numerical value matrix equation (6) by a numerical value substitution of respective components of said homogeneous coordinates transformation matrix A.sub.hand into the matrix equation expressed by equation (5), numerical value matrix equation (6) being: ##EQU92## where U.sub.num and V.sub.num stand for numerical value matrices produced by numerical value substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand into said matrices U(A.sub.hand) and V(A.sub.hand), respectively;

evaluating angles .theta..sub.i.sup.(k) of rotation, where k stands for positive integers to the maximum 16 for discriminating mutually between a plurality of angles .theta..sub.i, of the joints on the basis of the numerical value matrix equation expressed by equation (6); and

determining whether a manipulator constructed based on the simulated manipulator will operate as desired and is capable of implementing a desired position and orientation of the tip of the manipulator.

4. A manipulator simulation method according to claim 3, wherein to evaluate said angles .theta..sub.i.sup.(k) of rotation on the basis of the numerical value matrix equation expressed by equation (6):

equations, in which .theta..sub.1 and .theta..sub.2 are eliminated, are evaluated on the basis of the numerical value matrix equation given by equation (6);

eigenvalue equations or generalized eigenvalue equations are evaluated on the basis of the resultant equations in which .theta..sub.1 and .theta..sub.2 are evaluated on the basis of the resultant equations in which .theta..sub.1 and .theta..sub.2 are eliminated; and

said angles .theta..sub.1.sup.(k) of rotation are evaluated on the basis of the resultant eigenvalue equations or generalized eigenvalue equations.

5. A manipulator simulation apparatus embodied on a computer, adapted to evaluate angles of rotation of joints for coupling arm-to-arm to each other on the basis of information representative of position and orientation of a tip of a manipulator having six or less arms, which are sequentially coupled with each other, so as to implement the position and the orientation of the tip of the manipulator, wherein

when a homogeneous coordinates transformation matrix A.sub.hand representative of the position and the orientation of the tip of the manipulator looking at an absolute coordinate system is expressed by ##EQU93## where C.sub.hand denotes an orientation, and t.sub.hand denotes a position,

and the homogeneous coordinates transformation matrix A.sub.i representative of the position and the orientation of the (i+1)th coordinate system using the i-th coordinate system, of all the coordinate systems sequentially assigned from the rear end of the manipulator toward the top, is expressed by ##EQU94## where: i=1, 2, . . . , 6

C.sub.i =C.sub.z,i .multidot.C.sub.x,i ##EQU95## c.sub.i =cos.theta..sub.i, S.sub.i =sin.theta..sub.i .lambda..sub.i =cos.alpha..sub.i, .mu..sub.i =sin.alpha..sub.i

.theta..sub.i denotes an angle of rotation of i-th joint,

.alpha..sub.i denotes an angle of gradient of axis of rotation of i-th joint to an axis of rotation of i-1th joint,

t.sub.i denotes a three-dimensional parameter defining a configuration of i-th link, and

the homogeneous coordinates transformation matrix A.sub.i may be a unit matrix,

said apparatus comprises:

storage means for storing a matrix equation (1) evaluated based on said homogeneous coordinates transformation matrix A.sub.hand and said homogeneous coordinates transformation matrix A.sub.i, ##EQU96## where U denotes 20.times.16 of real number matrix, V denotes 20.times.18 of real number matrix,

.xi.=�x.sub.3 s.sub.4 s.sub.5, x.sub.3 s.sub.4 c.sub.5, x.sub.3 c.sub.4 s.sub.5, x.sub.3 c.sub.4 c.sub.5, x.sub.3 s.sub.4, x.sub.3 c.sub.4, x.sub.3 s.sub.5, x.sub.3 c.sub.5, s.sub.4 s.sub.5, s.sub.4 c.sub.5, c.sub.4 s.sub.5, c.sub.4 c.sub.5, s.sub.4, c.sub.4, s.sub.5, c.sub.5 !.sup.T,

.rho.=�x.sub.1.sup.2 x.sub.2.sup.2, x.sub.1.sup.2 x.sub.2, x.sub.1.sup.2, x.sub.1 x.sub.2.sup.2, x.sub.1 x.sub.2, x.sub.1, x.sub.2.sup.2, x.sub.2, 1!.sup.T,

and

X.sub.i =tan (.theta..sub.i /2);

first input means for inputting respective components of the parameter t.sub.i in the form of numerical value;

parameter numerical value substitution means for, by a numerical value substitution of respective components of the parameter t.sub.i entered through said first input means into the matrix equation expressed by equation (1), deriving a matrix equation expressed by equation (2) in which respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, ##EQU97## where U(A.sub.hand) and V(A.sub.hand) stand for matrices in which the respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, said matrices being produced by numerical value substitution of respective components of the parameter t.sub.i into said matrices U and V, respectively;

second input means for inputting respective components of said homogeneous coordinates transformation matrix A.sub.hand in the form of numerical value;

numerical value matrix equation derivation means for, by numerical value substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand entered through said second input means into the matrix equation expressed by equation (2), deriving a numerical value matrix equation expressed by equation (3), ##EQU98## where U.sub.num and V.sub.num stand for numerical value matrices produced by numerical substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand into said matrices U(A.sub.hand) and V(A.sub.hand), respectively; and

means for evaluating angles .theta..sub.i.sup.(k) of rotation (k stands for positive integers to the maximum 16 for discriminating mutually between a plurality of angles .theta..sub.i) of the joints on the basis of the numerical value matrix equation expressed by equation (3) to determine whether a manipulator constructed based on the simulated manipulator will operate as desired and is capable of implementing a desired position and orientation of the tip of the manipulator.

6. A manipulator simulation apparatus, embodied on a computer, adapted to evaluate angles of rotation of joints for coupling arm-to-arm to each other on the basis of information representative of position and orientation of a tip of a manipulator having six or less arms, which are sequentially coupled with each other, so as to implement the position and the orientation of the tip of the manipulator, wherein

when a homogeneous coordinates transformation matrix A.sub.hand representative of the position and the orientation of the tip of the manipulator looking at an absolute coordinate system is expressed by ##EQU99## where C.sub.hand denotes an orientation, and

t.sub.hand denotes a position,

and the homogeneous coordinates transformation matrix A.sub.i representative of the position and the orientation of the (i+1)th coordinate system using the i-th coordinate system, of all the coordinate systems sequentially assigned from the rear end of the manipulator toward the top, is expressed by ##EQU100## where: i=1, 2, . . . , 6

C.sub.i =C.sub.z,i .multidot.C.sub.x,i ##EQU101## c.sub.i =cos.theta..sub.i, s.sub.i =sin.theta..sub.i .lambda..sub.i =cos.alpha..sub.i, .mu..sub.i =sin.alpha..sub.i

.theta..sub.i denotes an angle of rotation of i-th joint,

.alpha..sub.i denotes an angle of gradient of axis of rotation of i-th joint to an axis of rotation of i-1th joint,

t.sub.i denotes a three-dimensional parameter defining a configuration of i-th link, and

the homogeneous coordinates transformation matrix A.sub.i may be a unit matrix,

said apparatus comprises:

storage means for storing a matrix equation expressed by equation (2) in which respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, ##EQU102## where U(A.sub.hand) and V(A.sub.hand) stand for matrices in which the respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, said matrices being produced by a numerical value substitution of respective components of the parameter t.sub.i into said matrices U and V, respectively,

wherein said matrix equation expressed by equation (2) is obtained by a numerical value substitution of respective components of the parameter t.sub.i into a matrix equation (1) evaluated based on said homogeneous coordinates transformation matrix A.sub.hand and said homogeneous coordinates transformation matrix A.sub.i, ##EQU103## where U denotes 20.times.16 of real number matrix, V denotes 20.times.18 of real number matrix,

.xi.=�x.sub.3 s.sub.4 s.sub.5, x.sub.3 s.sub.4 c.sub.5, x.sub.3 c.sub.4 s.sub.5, x.sub.3 c.sub.4 c.sub.5, x.sub.3 s.sub.4, x.sub.3 c.sub.4, x.sub.3 s.sub.5, x.sub.3 c.sub.5, s.sub.4 s.sub.5, s.sub.4 c.sub.5, c.sub.4 s.sub.5, c.sub.4 c.sub.5, s.sub.4, c.sub.4, s.sub.5, c.sub.5 !.sup.T,

.rho.=�x.sub.1.sup.2 x.sub.2.sup.2, x.sub.1.sup.2 x.sub.2, x.sub.1.sup.2, x.sub.1 x.sub.2.sup.2, x.sub.1 x.sub.2, x.sub.1, x.sub.2.sup.2, x.sub.2, 1!.sup.T,

and

X.sub.i =tan (.theta..sub.i /2);

input means for inputting respective components of said homogeneous coordinates transformation matrix A.sub.hand ;

numerical value matrix equation derivation means for, by numerical value substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand entered through said input means into the matrix equation expressed by equation (2), deriving a numerical value matrix equation expressed by equation (3), ##EQU104## where U.sub.num and V.sub.num stand for numerical value matrices produced by numerical substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand into said matrices U(A.sub.hand) and V(A.sub.hand) , respectively; and

joint rotation angle computing means for evaluating angles .theta..sub.i.sup.(k) of rotation (k stands for positive integers to the maximum 16 for discriminating mutually between a plurality of angles .theta..sub.i) of the joints on the basis of the numerical value matrix equation expressed by equation (3) to determine whether a manipulator constructed based on the simulated manipulator will operate as desired and is capable of implementing a desired position and orientation of the tip of the manipulator.

7. A manipulator simulation apparatus according to claim 5, wherein said joint rotation angle computing means comprises:

means for evaluating equations, in which .theta..sub.4 and .theta..sub.5 are eliminated, on the basis of the numerical value matrix equation given by equation (3);

means for evaluating eigenvalue equations or generalized eigenvalue equations on the basis of the resultant equations in which .theta..sub.4 and .theta..sub.5 are eliminated; and

means for evaluating said angles .theta..sub.i.sup.(k) of rotation on the basis of the resultant eigenvalue equations or generalized eigenvalue equations.

8. A manipulator simulation apparatus, embodied on a computer, adapted to evaluate angles of rotation of joints for coupling arm-to-arm to each other on the basis of information representative of position and orientation of a tip of a manipulator having six or less arms, which are sequentially coupled with each other, so as to implement the position and the orientation of the tip of the manipulator, wherein

when a homogeneous coordinates transformation matrix A.sub.hand representative of the position and the orientation of the tip of the manipulator looking at an absolute coordinate system is expressed by ##EQU105## where C.sub.hand denotes an orientation, and

t.sub.hand denotes a position,

and the homogeneous coordinates transformation matrix A.sub.i representative of the position and the orientation of the (i+1)th coordinate system using the i-th coordinate system, of all the coordinate systems sequentially assigned from the rear end of the manipulator toward the top, is expressed by ##EQU106## where: i=1, 2, . . . , 6

C.sub.i =C.sub.z,i .multidot.C.sub.x,i ##EQU107## c.sub.i =cos.theta..sub.i, s.sub.i =sin.theta..sub.i .lambda..sub.i =cos.alpha..sub.i, .mu..sub.i =sin.alpha..sub.i

.theta..sub.i denotes an angle of rotation of i-th joint,

.alpha..sub.i denotes an angle of gradient of axis of rotation of i-th joint to an axis of rotation of i-1th joint,

t.sub.i denotes a three-dimensional parameter defining a configuration of i-th link, and

the homogeneous coordinates transformation matrix A.sub.i may be a unit matrix,

said apparatus comprises:

storage means for storing a matrix equation (4) evaluated based on said homogeneous coordinates transformation matrix A.sub.hand and said homogeneous coordinates transformation matrix A.sub.i, ##EQU108## where U denotes 20.times.16 of real number matrix, V denotes 20.times.18 of real number matrix,

.xi.=�x.sub.3 s.sub.1 s.sub.2, x.sub.3 s.sub.1 c.sub.2, x.sub.3 c.sub.1 s.sub.2, x.sub.3 c.sub.1 c.sub.2, x.sub.3 s.sub.1, x.sub.3 c.sub.1, x.sub.3 s.sub.2, x.sub.3 c.sub.2, s.sub.1 s.sub.2, s.sub.1 c.sub.2, c.sub.1 s.sub.2, c.sub.1 c.sub.2, s.sub.1, c.sub.1, s.sub.2, c.sub.2 !.sup.T

.rho.=�x.sub.4.sup.2 x.sub.5.sup.2, x.sub.4.sup.2 x.sub.5, x.sub.4.sup.2, x.sub.4 x.sub.5.sup.2, x.sub.4 x.sub.5, x.sub.4, x.sub.5.sup.2, x.sub.5, 1!.sup.T

X.sub.i =tan (.theta..sub.i /2),

first input means for inputting respective components of the parameter t.sub.i in the form of numerical value;

parameter numerical value substitution means for, by a numerical value substitution of respective components of the parameter t.sub.i entered through said first input means into the matrix equation expressed by equation (4), deriving a matrix equation expressed by equation (5) in which respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, ##EQU109## where U(A.sub.hand) and V(A.sub.hand) stand for matrices in which the respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, said matrices being produced by numerical value substitution of respective components of the parameter t.sub.i into said matrices U and V, respectively;

second input means for inputting respective components of said homogeneous coordinates transformation matrix A.sub.hand ;

numerical value matrix equation derivation means for, by numerical value substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand entered through said second input means into the matrix equation expressed by equation (5), deriving a numerical value matrix equation expressed by equation (6), ##EQU110## where U.sub.num and V.sub.num stand for numerical value matrices produced by numerical substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand into said matrices U(A.sub.hand) and V(A.sub.hand), respectively; and

joint rotation angle computing means for evaluating angles .theta..sub.i.sup.(k) of rotation (k stands for positive integers to the maximum 16 for discriminating mutually between a plurality of angles .theta..sub.i) of the joints on the basis of the numerical value matrix equation expressed by equation (6) to determine whether a manipulator constructed based on the simulated manipulator will operate as desired and is capable of implementing a desired position and orientation of the tip of the manipulator.

9. A manipulator simulation apparatus, embodied on a computer, adapted to evaluate angles of rotation of joints for coupling arm-to-arm to each other on the basis of information representative of position and orientation of a tip of a manipulator having six or less arms, which are sequentially coupled with each other, so as to implement the position and the orientation of the tip of the manipulator, wherein

when a homogeneous coordinates transformation matrix

A.sub.hand representative of the position and the orientation of the tip of the manipulator looking at an absolute coordinate system is expressed by ##EQU111## where C.sub.hand denotes an orientation, and

t.sub.hand denotes a position,

and the homogeneous coordinates transformation matrix A.sub.i representative of the position and the orientation of the (i+1)th coordinate system using the i-th coordinate system, of all the coordinate systems sequentially assigned from the rear end of the manipulator toward the top, is expressed by ##EQU112## where: i=1, 2, . . . , 6

C.sub.i =C.sub.z,i .multidot.C.sub.x,i ##EQU113## c.sub.i =cos.theta..sub.i, s.sub.i =sin.theta..sub.i .lambda..sub.i =cos.alpha..sub.i, .mu..sub.i =sin.alpha..sub.i

.theta..sub.i denotes an angle of rotation of i-th joint,

.alpha..sub.i denotes an angle of gradient of axis of rotation of i-th joint to an axis of rotation of i-1th joint,

t.sub.i denotes a three-dimensional parameter defining a configuration of i-th link, and

the homogeneous coordinates transformation matrix A.sub.i may be a unit matrix,

said apparatus comprises:

storage means for storing a matrix equation expressed by equation (5) in which respective components of said homogeneous coordinates transformation matrix A.sub.hand are given as variables, ##EQU114## where U(A.sub.hand) and V(A.sub.hand) stand for matrices in which the respective components of said homogenous coordinates transformation matrix A.sub.hand are given as variables, said matrices being produced by a numerical value substitution of respective components of the parameter t.sub.i into said matrices U and V, respectively,

wherein said matrix equation expressed by equation (5) is obtained by a numerical value substitution of respective components of the parameter t.sub.i into a matrix equation (4) evaluated based on said homogeneous coordinates transformation matrix A.sub.hand and said homogenous coordinates transformation matrix A.sub.i, ##EQU115## where U denotes 20.times.16 of real number matrix, V denotes 20.times.18 of real number matrix,

.xi.=�x.sub.3 s.sub.1 s.sub.2, x.sub.3 s.sub.1 c.sub.2, x.sub.3 c.sub.1 s.sub.2, x.sub.3 c.sub.1 c.sub.2, x.sub.3 s.sub.1, x.sub.3 c.sub.1, x.sub.3 s.sub.2, x.sub.3 c.sub.2, s.sub.1 s.sub.2, s.sub.1 c.sub.2, c.sub.1 s.sub.2, c.sub.1 c.sub.2, s.sub.1, c.sub.1, s.sub.2, c.sub.2 !.sup.T

.rho.=�x.sub.4.sup.2 x.sub.5.sup.2, x.sub.4.sup.2 x.sub.5, x.sub.4.sup.2, x.sub.4 x.sub.5.sup.2, x.sub.4 x.sub.5, x.sub.4, x.sub.5.sup.2, x.sub.5, 1!.sup.T

X.sub.i =tan (.theta..sub.i /2);

input means for inputting respective components of said homogeneous coordinates transformation matrix A.sub.hand ;

numerical value matrix equation derivation means for, by numerical value substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand entered through said input means into the matrix equation expressed by equation (5), deriving a numerical value matrix equation expressed by equation (6), ##EQU116## where U.sub.num and V.sub.num stand for numerical value matrices produced by numerical substitution of the respective components of said homogeneous coordinates transformation matrix A.sub.hand into said matrices U(A.sub.hand) and V(A.sub.hand), respectively; and

joint rotation angle computing means for evaluating angles .theta..sub.i.sup.(k) of rotation (k stands for positive integers to the maximum 16 for discriminating mutually between a plurality of angles .theta..sub.i) of the joints on the basis of the numerical value matrix equation expressed by equation (6) to determine whether a manipulator constructed based on the simulated manipulator will operate as desired and is capable of implementing a desired position and orientation of the tip of the manipulator.

10. A manipulator simulation apparatus according to claim 8, wherein said joint rotation angle computing means comprises;

means for evaluating equations, in which .theta..sub.1 and .theta..sub.2 are eliminated, on the basis of the numerical value matrix equation given by equation (6);

means for evaluating eigenvalue equations or generalized eigenvalue equations on the basis of the resultant equations in which .theta..sub.1 and .theta..sub.2 are eliminated; and

means for evaluating said angles .theta..sub.i.sup.(k) of rotation on the basis of the resultant eigenvalue equations or generalized eigenvalue equations.

11. A manipulator simulation apparatus according to claim 9, wherein said joint rotation angle computing means comprises;

means for evaluating equations, in which .theta..sub.1 and .theta..sub.2 are eliminated, on the basis of the numerical value matrix equation given by equation (6);

means for evaluating eigenvalue equations or generalized eigenvalue equations on the basis of the resultant equations in which .theta..sub.1 and .theta..sub.2 are eliminated; and

means for evaluating said angles .theta..sub.i.sup.(k) of rotation on the basis of the resultant eigenvalue equations or generalized eigenvalue equations.

12. A manipulator simulation apparatus according to claim 6, wherein said joint rotation angle computing means comprises;

means for evaluating equations, in which .theta..sub.4 and .theta..sub.5 are eliminated, on the basis of the numerical value matrix equation given by equation (3);

means for evaluating eigenvalue equations or generalized eigenvalue equations on the basis of the resultant equations in which .theta..sub.4 and .theta..sub.5 are eliminated; and

means for evaluating said angles .theta..sub.i.sup.(k) of rotation on the basis of the resultant eigenvalue equations or generalized eigenvalue equations.