COMPUTATION OF ROBOT INERTIA MATRIXUSING BLOCK DIAGRAM APPROACH

Narpat S. GehlotUniversidade Federal da Paraíba - Departamento de Engenharia Elétrica

58109-970 Campina Grande-PB-BrasilE-Mail: gehlot@dee.ufpb.br

Pablo J. AlsinaUniversidade Estadual da Paraíba - CCT - Departamento de Física

58100 Campina Grande - PB - Brasil

Abstract: In this paper, two new methods for computation of the inertia matrix of robotmanipulators are proposed. The new algorithms are based on a block diagram ofmanipulator dynamics, derived from the Newton-Euler formulation. The proposedmethods allows to compute the inertia matrix explicitly, in an efficient recursive manner,and can be applied for robot dynamics simulation.

r. INTRODUCTION

The inverse dynamics formulation is a c1assicproblem in robotics. The inverse dynamics equationallows to compute the vector of input generalizedtorques as a function of the N joint positions and itsderivatives,

where,

't = Nx I vector ofgeneralized torques.q = Nx I vector ofjoint positions .M(q) = NxN inertia matrix.'t m = Nx I vector of inertial torques.'t o = Nx I vector of coriolis, centrifugai,gravitational and disturbance torques.

The solution of Eq. (I) is a typical control problem :finding the adequate vector r needed to impose the.desired joint trajectory q. In fact, the computedtorque control technique (Yoshikawa , 1990).utilizes Eq. (I) to obtain the vector of generalizedtorques r from the measured values of position andvelocity and computed values of acceleration. The

Newton-Euler technique (Luh, 1980) allows tocompute the inverse dynamics Eq. (1) in an efficientrecursive way. The computational efIort of thismethod has a linear dependence with the number Nof degrees offreedom (DOF) ofthe manipulator.

The dynamic simulation of robotic manipulators is adual of the control problem. The robot simulationconsists on obtaining the arm position as a functionof time, given the vector of generalized torques andthe initial conditions. This is attained by integratingthe direct dynamics, obtained by inverting Eq. (1),

(2)

It can be noted that the inverse of the inertia matrixis needed in the evaluation of Eq. (2). For thisreason, the Newton-Euler method cannot be useddirectly to compute Eq. (2), since this method onlyevaluates numerically the vector of generalizedtorques, without computing explicitly the inertiamatrix M(q). In spite of this, ao artifice can be usedto compute the inertia matrix through the Newton-Euler method, as shown-by Walker (Walker & OrinI982).Several methods were developed to computethe forward dynamics (and -the inertia matrix) .Baharin (Baharin & Green(Í991) proposed a

447

2. NEWTON-EULER FORMULATION

Defining the 3x3 and 6x6 matrix operators for crossproduct of3xl and 6xl vectors respectively as:

where p, v and 00 are 3x1 vectors. Thus, the 6x6spatial rotation matrix, from link i to link i+1, is:

(5)o 1A:+,J

I O -pz 1I Py I

(px) = I pz O -Px I (3)

l-py Px O Jrr V1 1 I (cox) 2(vx)1

= l2(vx) (ox) J (4)

q i Position variable ofjoint i.v i 3x I Linear velocity vector of link i.co i 3x I Angular velocity vector of Iink i.fi 3x I Force vector applied on link i by

link i-I.n i 3xl Moment vector applied on link i by

linki-I.3x3 Rotation matrix frorn link i to i+I.

1 P;+ I 3xl Position vector oflink i+1 withrespect to link i.

P c; 3xl Center ofmass position vector oflink i.

m i Total mass of link i.h i 3x3 Inertia tensor of link i in its own

reference frame.'t ; Driving torque/force oflink i.

The Newton-Euler formulation allows to computethe manipulator dynamics in a computationallyefficient recursive manner. This formulation isrealized in two stages: Forward and Backwardrecursions. In the Forward recursion, the cinematicvariables are transformed frorn the base of themanipulator to the end-effector. The Backwardrecursion updates forces and torques starting fromthe tool to the base, obtain ing the driving torque ofeach joint. A 6x I spatial vector version of theNewton-Euler algorithm is summarized below. Thefollowing notation is adopted:

proposed recursive methods for computation of theinertia matrix are described in Section 4. Thecomputational effort of the proposed methods isdiscussed in Section 5. Final conclusions are givenin Section 6

In this paper, two novel methods for computing theinertia matrix of robot manipulator are proposed.The methods are based on recursive relationships,obtained from a block diagram representation of thedynamics equation. This block diagram is obtainedfrom the Newton-Euler formulation. In Section 2the Newton-Euler algorithm in a ôx l-vector form issummarized. The blqck diagram representation ofmanipulator dynamics is derived in Section 3. The

recursive Lagrangian formulation based ondifferential relationships. Also based on theLagrangian mechanics, Li (Li, 1988) proposed amethod for the evaluation of the terms of theDynamic Equation. Both above-mentionedhave a computational effort of order 0(N3) . Moreefficient methods (Lilly & Orin, 1991; Featherstone,1987) , with computational effort of order O(N2),have been proposed. Balafoutis (Balafoutis & Patel,1989) described a method, based on · the Newton-Euler algorithm and using the concept ofgeneralizedand augmented links, for computing the inertiamatrix with a computational effort of order 0(N2).Furthermore, an efficient method for computing ofthe inverse dynamics and the inertia matrixrearranging the equations for a minimum set ofdynamics parameters, was proposed by Kawasaki(Kawasaki, et ai., 1992). This method, oriented toparameter identification, has a computational effortof order 0(N2). Alsina (Alsina & Gehlot, 1994)proposed a formulation method of order 0(N2) basedon the Newton-Euler recursion expressed in term ofspatial vectors. This method allows the explicitcomputation of the inertia matrix. In order toimprove the real-time performance of the forwarddynamics computation, several paralleI algorithms(Fijani & Bejczy, 1989) and architectures have beenproposed. Arnim-Javaheri (Amim-Javaheri & Or in,1988) described several systolic architectures for theevaluation of the inertia matrix based on dedicatedVLSI robotic processors. Lee (Lee & Chang, 1988)proposed several parallel algorithms for real-timesimulation of robot forward dynamics based onsingle-instruction multiple-data-stream computer,with as many processor as the number of DOF of themanipulator. Beside the methods above, severalforward dynamics computation schemes, with noneed of inertia matrix inversion, were proposed. Ko(Ko, 1992) reformulated the Newton-Euler equationsin a Kalman Filter form for the solution of theforward dynarnics. Gogoussis (Gogoussis & Donath1990) proposed a method for the solution of forwarddynamics using the recursive properties of Newton-Euler formulation associated with algebraic loops(solved through analog hardware) for the digitalsimulation of robot dynamics, A good comparisonbetween several methods for inverse and forwarddynamics computation is found in (Zomaya, 1991).

448

--------

The 6x I vectors of spatial veIocity, spatial angularvelocity and spatial effort, and the 6x6 spatial inertiamatrix of link i, are respectively defined as:

[TV=v1 1

T]TCú;

T] TCú;

(6)

(7)

(8)

3. BLOCK DIAGRAM REPRESENTATION

By imposing zero joint accelerations cL ' (i =I, .... ,N), to (q, q) (vector of corioIis, centrifugai,gravitational and disturbance torques) can be easilycomputed through the Newton-Euler algorithm.Similarly, the inertial torque t m ' (t m = M(q)q ),can be updated through the Newton-Euler algorithrnby imposing zero joint velocities, initial spatialacceleration and initial spatial effort:

fi .(p .x)TlI c, Jhi

(9)- Forward Recursionfor inertial torques:

The flag of joint i is defined as: for i =Oto N-I

1; =[0 O O O O Ir for Rjoint(lO.a) Y· RiT y' 1 00rn;+1 = i+lo mi + i+lqi+l (17)

1;=[0 O O O Or forPjoint(IO.b) end i

where R and P stands for rotational and prismaticjoint respectively. Thus, the Newton-Euler algorithmis given by:

- Backward Recursion for inertial torques:

FmN+1 = O

for i = N to I- Forward Recursion:

v. = gravity acceleration

for i = I to N

F rn; = R;+IFrni+1 +H;Vrni

t m] = JT·Frni

(18)

(19)

(lI) end i

According to these recursions, a block diagramrepresentation of the . manipulator dynamics isobtained, as shown in Figure lo

(13).4. COMPUTATION OF INERTIA MATRIX

end i 401 The Inverse Path Method

- Backward Recursion:

FN+ 1 = Spatial effort on the tool.·

In Figure I, the block diagram inside the dashed linerepresents the transfer matrix between jointaccelerations and inertial torques, which is theinertia matrix. Thus, the inertia matrix representsthe relation between an input vector q and an

output vector t rn ' that is, t rn =M(q).q o • Thefollowing notation is adopted: [t rn , Fm] ij is the

transfer matrix of the path from Frnj to t m] and

[t ,V ].. is the transfer matrix of the path fromrn rn IJ

V . to 't . o Thus, from the block diagram of FigurernJ rn, .

I, the following recursive relationships are obtained:

(14)

(16)

(15)

F = (n.x)H .n .cj 1 I I

for i = N to I

end i449

Fig. 1 - Block Diagram: Inverse Dynamics and Inertia Matr ix.

r- .....

Inertia Matrix

Since the inertia matrix is symmetric, it is sufficientto compute Equation (20) for j varying from i to Nand Equations (21) and (22) for j varying from N toi. Thus, based on the above reIationships, a recursiveaIgorithm for the computation of the inertia matrixis proposed, as shown in Figure 2. Since the transfermatrices are computed recursiveIy in the directionfrom torques to joint accelerations, this algorithmwas named: Inverse Path Method .

(20)

(22)

The Inverse Path Method:

For i = I to N

[-rrn,Frn1i =f{If(i < N) Then [-rrn ,FrnL+, = [-rrn,Frn1iR;+1If(i < N-I) Then

ForG=i+2toN)

Endj

[-r rn 'Vrn 1N = [-r rn ,Frn 1N"HNMiN = [-rrn,Vrn1w l N

4.2 The Central Path Méthod

In the block diagram of Figure I, a central path[Frn ,VrnL can be divided in two parallel paths: one

through matrix H. ' and the other through

[Fm >Vrn l+l,i+1' R;+l ' On the other hand, thetransfer matrix [Frn ,qh is composed by matrix

R 1+I serially associated with transfer matrix

[Frn ,q]j+l,i' Thus, t?e following recursions areobtained:

450

If (i < N) Then

For (k = N-I to i)

End kEnd i

Fig. 2 - The Inverse Path Method.

Table 2-Computational Effort: Central Path Method.

(24) No.OfProducts(27N2+621N-648)/2

No. of Additions(21N2+531N-552)J2

From the recursions above, a new algorithm, namedthe Central Path Method, is derived for computationofthe inertia matrix, as shown in Figure 3..

The Central Path Method:

[Fm,Vm]NN = HN[Fm,q]NN = [Fm, Vm]NN·JNM NN =

For (i = N-I to I)

For a sixjoint manipulator, the lnverse Path Methodinvolves 1179 products and 993 sums, whi1e theCentral Path Method involves 2025 products and1695 sums for computation of the inertia matrix. Ascan be seen in Figure 4, for more than twe1ve joints(highly redundant manipulators), the Central PathMethod -becomes more efficient than the lnversePath Method. This is because, for high number ofDOF, the N2 term is predominant, and the CentralPath Method has the smallest N2coefficient.

No . o f P ro d u c ts

o Inverse Path• C entra I P a th10000

' 5 0 0 0

OO 1 O 20

No . o f J o in ts

15000 NO .ofSums

o Inve rsePath

10000. C entra I P a th

5000

OO 10 20

No . of Joints5. COMPUTATIONALEFFORT

Fig. 3 - The Central Path Method.

End i

Endj

[Fm,q]j,i+1 = R1+,·[Fm,q]j+l,i+1Mj,i+1 = Jy.[Fm,q]j ,i+]

. i ' iT[Fm ,VmL =Hj + Ri+I·[Fm,VmLI,i+l·Ri+1[Fm,q);; = [Fm,Vm]ii ·JiM ii =JT ·[Fm,q);;For G= i to 1)

Table 1 -Computational Effort: Inverse Path Method.

The proposed methods involve several 1x6 vector /6x6 matrix products and additions. Noting that the6x6 spatial inertia and spatial rotation matricesinclude some zero terms. Thus, the product of a lx6vector and a 6x6 spatial inertia matrix involves 24products and 18 additions. The product of a lx õvector and a 6x6 spatial rotation matrix involves 27products and 21 additions. In this way, bothalgorithms for the inertia matrix computationinvolve a computational effort· of order 0(N2), asshown in Figure 4. The computational efforts for theInverse Path Method and for the Central PathMethod are respectively summarized in Table 1 andTable2.

No.OfProducts39W - 42N +27

No. of Additions33W - 36N + 21

Fig. 4 - Computational Effort.

6. CONCLUSIONS

Two novel methods for computation of the inertia .matrix of robot manipulators were proposed: theInverse Path Méthod and the Central Path Method.The proposed methods were derived from a set ofrecursive relationships obtained from a blockdiagram representation of the dynamics equation.The methods update explicit1y the inertia matrix inan efficient recursive way, thus allowing to app1ythese formulations to the forward dynamics problemand the dynamics simulation of robot manipulators .The computational efforts of the proposed methodshave an 0(N2) dependence. The Inverse PathMethod is more efficient for small number of DOF,while the Central Path Method is more efficient forhighly redundant manipulators.

451

REFERENCES

Alsina, P. J. & N. S. Gehlot (1994). "An AlternateFonnulation of Manipulator Dynamics withRecursive Computation of Inertia Matrix forComputer Sirnulation". Proc. of the IEEEInternational Symposium on IndustrialElectronics, ISIE-94, Chile, Vol. 1, pp. 335-338.

Amirn-Javaheri, M. & D. E. Or in (1988). "SystolicArchitecture for the Manipulator Inertia Matrix".IEEE Trans., Vol. SMC-18, No . 6, pp. 939-951.

Baharin, I. B. & R. J. Green, (1991 )."Computationally-effective Recursive LagrangianFormulation of Manipulator Dynamics". Int.Journal ofControl, Vol. 54, No . 1, pp . 195-214.

Balafoutis, C. A. & R. V. Pate l (1989). "EfficientCom putation ofMan ipulator Inertia Matrices andthe Direct Dynamics Problem", IEEE Trans.,Vol. SMC-19, No .5, pp. 1313-1321.

Featherstone, R. (1987). "Robot DynamicsAIgorithms". Boston Kluwer AcademicPublishers.

Fijany, A. & A. K. Bejczy (1989). "A Class ofParallel AIgorithms for Computation of theManipulator Inertia Matrix", Trans. on Roboticsand Automation, Vol. 5, No . 5, pp. 600-615 .

Gogoussis, A. & M. Donath (1990) "A Method forthe Real Time Solution ofthe Forward DynamicsProb1em for Robots Incorporating Friction".Trans. ofthe ASME, Vol. 112, pp. 630-639.

Kawasaki, H., A. Murata & K. Kanzaki (1992). "AnEfficient Method for the Computation of InverseDynamics and Inertia Matrix based on theMinimum Set of Dynamics Parameters of

452

Manipulators". Proc. of ICARCV-92, Singapore,Vol. 3, pp. RO-1.2 .1 - RO-1.2 .5.

Ko, K. (1992). "Manipulator Dynamics based onStochastic System ". Proc . of ICARCV-92,Singapore, Vol. 3, pp. RO-2.5 .6 - RO-2 .5.6.

Lee, C. S. G. & P. R. Chang (1988). "EfficientParallel AIgorithms for Robot Forward DynamicsCornputation" . IEEE Trans., Vol. SMC-18, No.2, pp. 238-251 .

Li, C. J. (1988). "A New Method of Dynam ics forRobot Manipulators" . IEEE Trans., Vol. SMC-18, No . 1, pp. 105-114.

Lilly, K. W. & D. E. Orin (1991). "Altern ateFormulations for the Manipulator InertiaMatrix" . Int. Jour. of Robotic Research, Vol. 10,No. 1, pp. 64-74.

Luh , J. Y. S., M. W. Walker & R. P. C. Paul (1980)"on-line Computational Scheme for MechanicalManipulators". Journal of Dynamic Systems,Measure. & Control, Vol. 102, pp. 69-76.

Walker, M. W. and D. E. Orin (1982). "EfficientDynamic Computer Simulation of RobotMechanisrns". ASME Journal of DynamicSystems, Measure. and Control. Vol. 104, pp .205-211.

Yoshikawa, T. (1990). "Foundat ions of Robotics -Analysis and Control". The MIT Press.

Zomaya, A. Y. (1991). "Efficient Robot Dynamicsfor High Sampling Rate Motions: Case Studiesand Benchrnarks". Int. Journal of Control, Vol.54, No. 4, pp. 793-8 I4.