THE PUBLISHING HOUSE OF THE ROMANIAN ACADEMY

PROCEEDINGS OF THE ROMANIAN ACADEMY, Series A, Volume 9, Number 3/2008, pp. 000000

DYNAMIC MODELING OF MANIPULATORS WITH SYMBOLIC COMPUTATIONAL METHOD

Viviana FILIP VALAHIA University of Targoviste, e-mail: [email protected]

This paper presents a symbolic computational method for manipulators dynamic equations. For a certain trajectory imposed to characteristic point, the necessary motor torque is calculated in the driving joint. The dynamic model Lagrange - Euler is considered for a manipulator with n degree of freedom, for which a symbolic computational algorithm is elaborated depending on input and output data. For example, this symbolic calculation is applied to a trimobil planar manipulator, as an open kinematical chain. Key words: manipulator; dynamic model; symbolic calculation; generalized force; Coriolis force.

1. INTRODUCTION The dynamic modeling of robots [5,6] represents the determination of the dynamic equations, which is the first information necessary for robots control [1,4]. These equations are useful for computational simulation of the robots motion and for the evaluation of kinematical structure of robots [1]. In the dynamic formulation of manipulators the following methods are used: Lagrange-Euler, Newton-Euler, DAlembert. In [5,6] reference books there are discussed only plan manipulators with 2 degree of freedom. For the manipulators with more than 2 degrees of freedom (DOF), a very laborious calculation is necessary. The Lagrange-Euler method is relatively simple and systematical. As a rule, the dynamic for a device of electronic control and the frictions of gearing are not considering. Thus there is obtained a 2nd degree equation system. For the robots with 6 DOF, the dynamic equations are nonlinear and very laborious. Generally, each term of the inertial force and gravitational force depend of the instantaneous position of the kinematical links; the terms moment and force depend on the velocity and the position of kinematical links. The dynamic equations are obtained by the Lagrange-Euler method for the non-conservatives systems. If the non-dimensional method is used, the dynamic formulation is more efficient. The new method of kinematics and dynamics modeling use the homogenous matrix and the lagrangean formulation. The objective of the paper is the determination of the robots dynamic equations, when the mechanic characteristics are known. In this paper there is proposed a program created in Mathematica programming language. This program is developed for the manipulator-robots with open kinematical chains, using only revolute and prismatic pairs. Through this program, we obtained: the Denavit-Hartenberg (D-H) parameters for robot, the geometric and kinematical model, the effective inertia of each joint, the coupling inertia, the Coriolis forces, the gravitational loads and the dynamic equations. 2. DYNAMIC MODEL LAGRANGE-EULER Is considered the differential equation of Lagrange:
d L L = Qi i qi dt q (i = 1, 2,, n)

(1)

Viviana FILIP

where: L = E c E p ; n number of DOF; qi coordinate of joint j; generalized force of joint i. The kinematics energy of kinematical link i is:
0 0 T i [T ] 1 i[ ] Ec = D [ Ji ] q j qk 2 j =1 k =1 i i

i - velocity of joint i; Qi q

q j qk

(2)

where: ri is the position vector of mass 0 i [T ] is the transfer matrix from the i link attached system to the fixed reference system; T J i = r i r i dm is the 4x4 inertial matrix of kinematical link i, with the planar, centrifugal and static
i

moments. The potential energy of kinematical link i in gravitational field is:

E p = mi [ g ]
i =1

T0 i

[T ] [ ri ]
i

(3)

The Lagrange function of one manipulator-robot with n kinematical links is:

n T 1 n i2 + mi [ g ] q j qk + 2 J mi q i =1 i =1

0 i

[T ] [ ri ]
i

(4)

Replacing (2) in (1), we obtain:

j + J mi q i + Dijk q jq k + Di Qi = Dij q

(5)

where:
0 0 T p [T ] p[ ] J p Dij = D q j qi p = max( i , j ) n

(6)

0 0 T p [T ] p[ ] J p Dijk = D q j qk qi p = max( i , j , k ) n

(7)

Di = m p [ g ]
p =i

0 p

[T ]) i

qi

[ ri ]

(8)

The coefficients (6), (7) and (8) are: Dii - effective inertia of joint i; Dij = D ji - coupling inertia between joints i and j; Dijk = Dikj - Coriolis inertia of joint i due to speeds of joint j and k; Di - gravitational loads of joint i; J mi - motor inertia of joint i; J i - pseudo inertia matrix.

Dynamic modeling of manipulators with symbolic computational method

3. GEOMETRICAL TRANSFORMATION D-H

The operator which describes the position and orientation of reference system i relative to system i-1 is defined through D-H matrix:

c i s i 1 i [ T ] = i 0 0

si c i ci c i s i 0

s i s i c i s i c i 0

d i ci d i si ai 1

(9)

where: c = cos and s = sin . In an open kinematical chain with revolute joints, the angular parameters i , i and the linear parameters

ai , di have the following signification (Fig. 1): i = ( zi 1 , zi ) , ai = Oi 1 A , di = AOi .

z i 1

i+1

z i 1
zi

A
i 1

yi i Oi 1
i

A
i 1

yi
Oi

i +1

y i 1

Oi

si
O i 1

zi
xi

xi 1

|| xi

xi

Fig.1 Kinematical chain ( i 1), i , (i + 1)

xi 1
Fig.2. Kinematical chain with prismatic joints

In the case of the revolute joint (i-1,i) in the point Oi-1, the angular displacement i is variable (positive or negative). The others three parameters ai , di , i have only constant values. The distance di is the length of the common normal of the axes zi 1 and zi [2]. In a particular case of the kinematical chain, the axes z i 1 and z i are parallels (planar kinematical chain) or perpendiculars (spatial kinematical chain): i = 0 0 , 90 0 . In the case of the prismatic joint between the kinematical link ( i 1 ) and i , x i 1 || x i , that means the angular displacement is null ( i = 0 ) , while the linear displacement s i = a i is variable (Fig.2). For i = 0 , the general matrix D-H (9) has this form:

1 0 0 1 i 1 i [T ] = 0 s i 0 0

0 0 c i 0

di 0 . si 1

(10)

In the particular case i = 0 , the matrix (10) is reduced at a translation matrix in the plan [x i 1 , z i 1 ] :

Viviana FILIP

1 0 i 1 i [T ] = 0 0

0 1 0 0

0 di 0 0 . 1 si 0 1

(11)

For a planar kinematical chain, the general form of the homogeneous matrix results on the basis of the matrix (9), where i = 0 ( z i 1 || z i ) and a i = 0 (Fig.3).

yi Oi y i 1
i 1

xi
i+1

Fig.3. Planar kinematical chain with revolute joints

The matrix D-H of the planar kinematical chain (Fig.3) is:

c i s i 1 i i [T ] = 0 0
where d i = Oi 1 Oi and i = ( x i 1 , x i ) .

s i c i 0

0 d i c i 0 d i s i , 1 0 0 1

(12)

4. ALGORITHM AND PROGRAM OF DYNAMIC CALCULATION. APLICATION

The D-H method was used buy many researchers for the study of kinematics and dynamics of manipulator robots. It forms the basis for the following programs GESIMA (GEometric SImulation of the MAnipulator), SPHEMA (SPHEres Manipulator), IKIREM (Inverse KInematics REdundant Manipulator) which solves the inverse kinematics for a redundant manipulator by utilizing the optimum simulation of the system manipulator obstacle [3]. Also based on the D-H method the SUMT program was developed [4] which computes the penalty function method for inverse kinematics. The upper mentioned programs where developed using FORTRAN programming language because this language also allowed the graphical representation of the robots position based on calculated solutions and through the communication with other commercial programs which contain subroutines for graphical communication. The program proposed in this paper is similar to the upper mentioned ones, except its development in the Mathematica programming language, thus because the paper aims to establish the dynamic equations and the Mathematica already contains specific functions for this purpose. The proposed algorithm and program for the dynamics calculation of the manipulator-robots has input and output data. The input data are: D-H parameters; the coordinates of mass center for each kinematical link; the inertial couples axial and centrifugal of each kinematical link; the inertial motor (actuator) couple for each joint; the vector of gravitational acceleration. The output data are: the geometric - kinematical model; the effective inertial force or couple of each joint; the inertia of coupling between the joints i and j ; the centrifugal force of link i due to speed of joint

Dynamic modeling of manipulators with symbolic computational method

j ; the Coriolis force of the joint i due to the speeds of joint j and k ; the gravitational loads of each joint;
the manipulator robots dynamic equations.; The notations used in computational program are [5]: m[i ] - the mass of i kinematical link; g - the gravitational acceleration vector, with respect to the fixed-reference system;

q[i ][t ] - the generalized coordinate of kinematical link as function of time; Q[i ][t ] - the generalized force of joint i corresponding the independent parameter q[i ][t ] .

It is consider, as example, a planar manipulator 3R (Fig. 4) actuating in the vertical plan.

P
3

y2

q3
x2 O2

y1

y0 1
q1
0

2
O1

q2 x1

O0

x0
Fig.4. Trimobil planar manipulator 3R

In the particular case, the three links have equal lengths:

O0 O1 = O1 O 2 = O 2 P = l .
The numerical value is: l = 0 ,2 m = 2 dm . Also, all kinematical links are homogeneous bars, having the same density = 8000 kg / m 3 . For each kinematical link, the transversal section is constant, having the same diameter (3cm). The actuating systems are represented by the motoreductors placed in the 3 kinematical joint O0 , O1 and O 2 (Fig.4). The actuators have same inertial mechanical couple: J m1 = J m 2 = J m 3 = 0,11 kg dm 2 The generalized coordinates are represented through the angular displacements: q 1 = 10 (absolute position) and q 2 = 21 , q 3 = 32 (relative position). In the computational program, the axial inertial couples J xx , J yy , J zz , and the centrifugal inertial couples are functions of mass m[i ] for each kinematical link i:
2 1 x dm = 2 ( J xx + J yy + J zz ); 2 1 y dm = 2 ( J xx J yy + J zz ); 2 1 z dm = 2 ( J xx + J yy J zz ).

(13)

The centrifugal couples are:

Viviana FILIP

J xy = xydm; J xz = xzdm; J yz = yzdm .

(14)

S x = xdm; S y = ydm; S z = zdm .

(15)

Q[1][t] g Cos[q[1][t]] m[1] + (2g Cos[q[1][t]] + g Cos [q[1][t]+q[2][t]]) m[2]+(2 g Cos[q[1][t] + 2 g Cos[q[1][t] +q[2][t]]+g Cos[q[1][t]+q[2][t]+q[3][t]]) m[3]+2(-2 m[2] Sin[q[2][t]]+m[3](-4 Sin[q[2][t]]-2 Sin[q[2][t]+ q[3][t]])) (q[1])[t](q[2])[t] + (-2m[2]Sin[q[2][t]+m[3](-4Sin[q[2] [t]]2Sin[q[2][t]+q[3][t]]))(q[2])[t]2+2m[3](-2Sin[q[3][t ]]2 Sin[q[2][t]+q[3][t]]) (q[1])[t] (q[3])[t]+2 m[3] (-2 Sin[q[3][t] 2 Sin[q[2][t] + q[3][t]]) (q[2])[t] (q[3])[t] + m[3] (-2 Sin[q[3][t] 2 Sin[q[2][t] + q[3][t]]) (q[3])[t]2 +Jm1 (q[1])[t] + (m[1]+(5+4Cos[q[2][t]])m[2] + (9+8 Cos[q[2][t]+4Cos[q[3][t]]+4Cos[q[2][t] + q[3][t]]) m[3]) (q[1])[t] +((1+2 Cos[q[2][t]]) m[2] + (5 + 4 Cos[q[2][t]] +4Cos[q[3][t]]+2 Cos[q[2][t]+q[3][t]]) m[3]) (q[2])[t] + (1+2Cos[q[3][t]]+2Cos[q[2][t]+ q[3][t]]) m[3] (q[3])[t] Q[2][t] g Cos[q[1][t] + q[2][t]]m[2] + (2g Cos[q[1][t] +q[2][t]+g Cos[q[1][t]+q[2][t]+q[3][t]]) m[3] + (2m[2] Sin[q[2][t] + m[3](4Sin[q[2][t]] +2Sin[q[2][t] +q[3][t]])) (q[1])[t]2- 4m[3] Sin[q[3][t]](q[1])[t] (q[3])[t] - 4m[3] Sin[q[3][t]] (q[2])[t] (q[3])[t] - 2m[3]Sin[q[3][t]] (q[3]) [t]2+ ((1+2Cos[q[2][t]]) m[2] + (5+4Cos[q[2][t]] + 4Cos [q[3][t]] + 2Cos[q[2][t] + q[3][t]]) + m[3]) (q[1])[t] + Jm2 (q[2])[t] + (m[2] + (5+4Cos[q[3][t]]) m[3]) (q[2])[t] + (1+2Cos[q[3][t]]) m[3] (q[3])[t] Q[3][t] g Cos[q[1][t]+q[2][t]+q[3][t]] m[3] + m[3] (2Sin[q[3][t]] + 2Sin[q[2][t] + q[3][t]]) (q[1])[t]2 + 4m[3]Sin[q[3][t]](q[1])[t] (q[2])[t] + 2m[3] Sin[q[3][t]] (q[2])[t]2 + (1+2Cos[q[3][t]] + 2Cos[q[2][t] + q[3][t]]) + m[3] (q[1])[t] + (1+2Cos[q[3][t]]) m[3] (q[2])[t] + Jm3 (q[3])[t] + m[3] (q[3] (q[3])[t]

5. CONCLUSIONS

The program was developed in Mathematica, which is an advanced mathematic computations programming language. By using Mathematica the coding of needed subroutines is not necessary for equations and systems of equations solving. These issues represent a drawback in older/classical programming languages like FORTRAN, Pascal, C+ because of prolonged computational periods needed. The efficiency of the program proposed through this paper can be observed especially in the modelling of inverse geometry of redundant structures, which needs non-linear equations systems solving through iterative methods. Another efficiency feature of this program can be observed in the simulation of manipulator robots through the integration of differential non-homogenous equations with given initial parameters.
REFERENCES
1. FILIP V., NEAGU A. - A Symbolic Computational Method for Dynamic Simulation of Multibody Systems, Proceeding of International Conference on Innovative Computing, Information and Control, Volume I, China, pp.130-133, 2006. 2. MANFRED L. HUSTY , MARTIN PFURNER, HANS-PETER SCHROCKER - A new and efficient algorithm for the inverse kinematics of a general serial 6R manipulator, Mechanism and Machine Theory 42, Elsevier, pp. 6681, 2007 3. MITSI, S., BOUZAKIS K. D., - Simulation of redundant manipulators for collision avoidance in manufacturing and assembly environments, Mechanism and Machine Theory Vol. 28, No. 1, Elsevier, pp. 13 -21, 1993. 4. MITSI, S., BOUZAKIS K. D., MANSOUR G., - Optimization of robot links motion in inverse kinematics solution considering collision avoidance and joint limits, Mechanism and Machine Theory Vol. 30, No. 5, Elsevier, pp. 653 - 663, 1995. 5. PAUL R., - Robot Manipulators - Mathematics, Programming and Control, MIT Press Cambridge, 1981. 6. ZAHARIA S., FILIP V. - A Symbolic Computational Method for a Dynamic Model of Robot Manipulators, Proceeding of 2nd International Conference on Engineering Computational Technology Leuven, Belgium, pp. 51-56, 2000.

Received July 09, 2008