Method and apparatus for control of an articulated robot

A calculating apparatus for obtaining a Jacobian matrix to be used for transforming speed data in the Cartesian coordinate system in an articulated robot having a plural number N of degrees of freedom into speed data in the articular coordinate system, said calculating apparatus comprising:

• coordinate transformation matrix calculating means for obtaining by the number of degrees of freedom, coordinate transformation matrices M.sub.j in the articular coordinate system, corresponding to the motions of said degrees of freedom,
• differential matrix calculating means for obtaining differential matrices M.sub.j ' of said coordinate transformation matrices of each of said degrees of freedom concerning the motions of said degrees of freedom,
• a first group of a plurality of matrix registers connected with each other for maintaining respectively said coordinate transformation matrices, one matrix register of said first group of matrix registers being connected to said coordinate transformation matrix calculating means,
• a second group of a plurality of matrix registers connected with each other for maintaining respectively said differential matrices, one matrix register of said second group of matrix registers being connected to said differential matrix calculating means,
• a plurality of matrix multiplying means connected to determined matrix registers among said first and second groups of matrix registers for performing multiplication of two matrices, and
• control means connected to said first and second groups of matrix registers and said matrix multiplying means for controlling them so as to obtain a matrix b.sub.j.sup.i represented by the following equation in said matrix multiplying means, ##EQU54## where j is an integer from 0 to N, i is an integer from 0 to N and E is a unit matrix.

A calculating apparatus for obtaining a Jacobian matrix to be used for transforming speed data in the Cartesian coordinate system in an articulated robot having a plural number N of degrees of freedom into speed data in the articular coordinate system, said calculating apparatus comprising:

• coordinate transformation matrix calculating means for obtaining by the number of degrees of freedom, coordinate transformation matrices M.sub.j in the articular coordinate system corresponding to the motions of said degrees of freedom,
• differential matrix calculating means for obtaining a differential matrix M.sub.j ' of said coordinate transformation matrix of each of degree of freedom concerning the motion of each degree of freedom,
• a first matrix shift register connected to said coordinate transformation matrix calculating means for maintaining said coordinate transformation matrices of the number equal to the number of said degrees of freedom and outputting them successively by shifting them one by one,
• a second matrix shift register connected to said differential matrix calculating means for maintaining said differential matrices of the number equal to the number of said degrees of freedom and outputting them successively by shifting them one by one,
• switching means connected to said first and second matrix shift registers for selecting and outputting a matrix from either of them,
• matrix multiplying means having two input portions for obtaining a product of the matrices respectively inputted thereto so as to output a product matrix,
• a matrix register connected to said matrix multiplying means for maintaining and outputting said product matrix, said matrix register being able to be initialized to a unit matrix, one input portion of said matrix multiplying means being connected to the output portion of said switching means and the other input portion thereof being connected to the output portion of said matrix register, and
• control means connected to said first and second matrix shift registers, said switching means, said matrix multiplying means and said matrix register for controlling them so as to obtain a product matrix b.sub.j.sup.i represented by the following equation in said matrix multiplying means, ##EQU55## where j is an integer from 0 to N, i is an integer from 0 to N and E is a unit matrix.

A coordinate transforming apparatus for transforming speed data dX in the Cartesian coordinate system in an articulated robot having a plural number N of degrees of freedom into speed data d.alpha. in the articular coordinate system, said coordinate transforming apparatus comprising:

• coordinate transformation matrix calculating means for obtaining by the number of degrees of freedom, coordinate transformation matrices M.sub.j in the articular coordinate system, corresponding to the motions of said degrees of freedom,
• differential matrix calculating means for obtaining differential matrices M.sub.j ' of said coordinate transformation matrices for each of said degrees of freedom concerning the motions of each degrees of freedom,
• a first group of a plurality of matrix registers connected with each other for maintaining respectively said coordinate transformation matrices, one matrix register of said first group of matrix registers being connected to said coordinate transformation matrix calculating means,
• a second group of a plurality of matrix registers connected with each other for maintaining respectively said differential matrices, one matrix register of said second group of matrix registers being connected to said differential matrix calculating means,
• a plurality of matrix multiplying means connected to determined matrix registers among said first and second groups of matrix registers for performing multiplication of two matrices,
• control means connected to said first and second groups of matrix registers and said matrix multiplying means for controlling them so as to obtain a matrix b.sub.j.sup.i represented by the following equation in said matrix multiplying means, ##EQU56## where j is an integer from 0 to N, i is an integer from 0 to N and E is a unit matrix,
• inverse matrix calculating means connected to one matrix register of said first group of matrix registers and one matrix register of said second group of matrix registers for calculating an inverse matrix of a Jacobian matrix obtained from said product matrix b.sub.j.sup.i, and
• multiplying means connected to said inverse matrix calculating means for obtaining a product of said inverse matrix and said speed data dX in the Cartesian coordinate system, whereby said speed data dX is transformed into said speed data d.alpha. in the articular coordinate system.

A coordinate transforming apparatus for transforming speed data dX in the Cartesian coordinate system in an articulated robot having a plural number N of degrees of freedom into speed data d.alpha. in the articular coordinate system, said coordinate transforming apparatus comprising:

• coordinate transformation matrix calculating means for obtaining by the number of degrees of freedom, coordinate transformation matrices M.sub.j in the articular coordinate system corresponding to the motions of said degrees of freedom,
• differential matrix calculating means for obtaining a differential matrix M.sub.j ' of said coordinate transformation matrix of each of degree of freedom concerning the motion of each degree of freedom,
• a first matrix shift register connected to said coordinate transformation matrix calculating means for maintaining said coordinate transformation matrices of the number equal to the number of said degrees of freedom and outputting them successively by shifting them one by one,
• a second matrix shift register connected to said differential matrix calculating means for maintaining said differential matrices of the number equal to the number of said degrees of freedom and outputting them successively by shifting them one by one,
• switching means connected to said first and second matrix shift registers for selecting and outputting a matrix from either of them,
• a first matrix multiplying means having two input portions for obtaining a product of the matrices respectively inputted thereto so as to output a product matrix,
• a first matrix register connected to said first matrix multiplying means for maintaining and outputting said product matrix, said first matrix register being able to be initialized to a unit matrix, one input portion of said first matrix multiplying means being connected to the output portion of said switching means and the other input portion thereof being connected to the output portion of said matrix register,
• control means connected to said first and second matrix shift registers, said switching means, said first matrix multiplying means and said first matrix register for controlling them so as to obtain a product matrix b.sub.j.sup.i represented by the following equation in said first matrix multiplying means, ##EQU57## where j is an integer from 0 to N, i is an integer from 0 to N and E is a unit matrix,
• a second matrix register connected to said first matrix register for maintaining the s-th Jacobian matrix A.sup.(s) obtained from said product matrix b.sub.j.sup.i, where s is an arbitrary integer,
• a third matrix register for maintaining an inverse matrix B.sup.(s) which is an approximate solution of said matrix A.sup.(s),
• second matrix multiplying means for obtaining a matrix [A.sup.(s) B.sup.(s) ] using said Jacobian matrix A.sup.(s) and said inverse matrix B.sup.(s),
• initializing means for applying a unit matrix E,
• first matrix subtracting means for obtaining a matrix [E-A.sup.(s) B.sup.(s) ] using said matrix [A.sup.(s) B.sup.(s) ] and said unit matrix E,
• first multiplying-adding means for obtaining a scalar value V.sup.(s) by calculating the square summation of the respective elements of said matrix [E-A.sup.(s) B.sup.(s) ],
• third matrix multiplying means for obtaining a matrix [A.sup.T(s) (E-A.sup.(s) B.sup.(s))] using a transposed matrix A.sup.T(s) of said matrix A.sup.(s) and said matrix [E-A.sup.(s) B.sup.(s) ],
• second multiplying-adding means for obtaining a scalar value U.sup.(s) by calculating the square summation of the respective elements of said matrix [A.sup.T(s) (E-A.sup.(s) B.sup.(s))],
• calculating means for obtaining a scalar value .lambda..sup.(s) =V.sup.(s) /U.sup.(s) using said scalar values V.sup.(s) and U.sup.(s),
• fourth matrix multiplying means obtaining a matrix .lambda..sup.(s) [A.sup.T(s) (E-A.sup.(s) B.sup.(s))] using said matrix [A.sup.T(s) (E-A.sup.(s) B.sup.(s))] and said scalar value .lambda..sup.(s),
• second matrix subtracting means for obtaining, using said matrix B.sup.(s) and said matrix .lambda..sup.(s) [A.sup.T(s) (E-A.sup.(s) B.sup.(s))], the (s+1)th approximate matrix [B.sup.(s+1) ] represented by the following equation:
• [B.sup.(s+1) ]=[B.sup.(s) ]-.lambda..sup.(s) [A.sup.T(s) (E-A.sup.(s) B.sup.(s))]
• and
• fifth matrix multiplying means connected to the output portion of said second matrix subtracting means for obtaining a product of said inverse matrix B and said speed data dX in the Cartesian coordinate system, whereby said speed data dX is transformed into said speed data d.alpha. in the articular coordinate system.