ENGINEERING SOCIETIES SEP 3 0 1982.

LIBRARY
M.W.Walker
Staff Engineer, Corporate Robot Design Engineering, Nordson Corporation, Amherst, Ohio

Efficient Dynamic Computer Simulation of Robotic Mechanisms

The Newton-Euler formulation of the equations of motion of an open-loop kinematic chain is used within a dynamic computer simulation of a robotic mechanism. Four different methods are presented for obtaining the joint variable accelerations given the joint positions, velocities, and input torques or forces. Each of these methods was programmed in FORTRAN and then executed on a CYBER 175 digital computer. This paper contains a comparison of the computational complexity of these methods and the execution time of the programs which implement them.

D. E. Orin
Assistant Professor, Department of Electrical Engineering, The Ohio State University, Columbus, Ohio

Introduction In the development of a dynamic computer simulation for robotic mechanisms such as an industrial manipulator, the problem of formulating the rigid body equations of motion must be addressed. For mechanisms with only two or three degrees-of-freedom, these equations can usually be derived manually using one of several approaches; for example, Lagrangian [1-3], virtual work [4], and Newton-Euler [5-7]. For mechanisms with more than three degrees-of-freedom, the equations become so complex that it requires yet another computer program to symbolically derive these equations [8, 9]. Because of the combinatorial complexity of the equations, researchers in the past have applied simplifying assumptions to the model for the mechanism under consideration so that a solution could be obtained. These assumptions have included ignoring coriolis effects, considering some system elements to be massless, and neglecting joint offsets. However, the results thus obtained may only be valid in a limited range of operation. (For instance, coriolis forces are non-negligible in high-speed motion.) Recently, more general solutions have been obtained for the dynamic equations of motion in which most of the simplifying assumptions have been removed. The present emphasis is specifically placed on computational efficiency. This has resulted since it is desirable to obtain a solution to the equation in real-time for purposes of control ("inverse plant" problem [10]). Also, as both control designers and mechanical designers begin to routinely use dynamic analysis as a key component in the development process, optimizing the cost of simulation becomes increasingly important. A block diagram showing the component parts of a typical dynamic computer simulation for control law development is given in Fig. 1. As trained or commanded by a human operator or as automatically generated through programmed algorithms, the complete trajectory is furnished by the Motion Design section. Through a control law, torques are applied by the actuators to the mechanism. Through solution of the dynamic equations, the joint accelerations may be
Contributed by the Dynamic Systems and Control Division for publication in
the JOURNAL OF DYNAMICS SYSTEMS, MEASUREMENT, AND CONTROL. Manuscript

Motion Design

_V
Feedback Control

9d

_&
Dynamic Equations

V
Motion Integration

a>g.

^/
Fig. 1 Block diagram of a typical computer simulation for control law development (q, q, q relative joint position, rate, and acceleration; r torque; d desired values

received by the Dynamic Systems and Control Division, September 29, 1981.

obtained, and through integration, the actual trajectory is determined. In this paper, efficient solution of the dynamic equations for a general T degree-of-freedom, single open-chain robotic V mechanism will be considered. The basic problem as shown in Fig. 1 is to solve for the relative joint accelerations given the
SEPTEMBER 1982, Vol. 104/205

Journal of Dynamic Systems, Measurement, and Control

Downloaded 12 Sep 2011 to 141.213.169.57. Redistribution subject to ASME license or copyright; see http://www.asme.org/terms/Terms_Use.cfm

Copyright 1982 by ASME

Link j

associated link coordinate system, are determined. Finally, the dynamic equations of motion for the manipulator, in terms of the kinematics and mass properties of each link, are formulated. Kinematics. In assigning link coordinates to each link, the Denavit and Hartenberg convention is used [12] (Fig. 2). This convention has been used in a number of previous references which describe how the four parameters, ah dh a-,, 0, are defined. Joint / can either be rotational or translational. If it is rotational, ah d, and a, are constants and 0, is variable. If it is translational, a,-, a, and 0,- are contstants and 0, is variable. The orientation of link i coordinates with respect to link / 1 coordinates is defined in terms of a,- and 0,. The direction cosine matrix, A;_ 1 ; of link ; coordinates referenced to link / - 1 coordinates indicates the orientation: cos 0,- cos a,- sin 0, sin 0,sin a,- sin 0,(1)

Axis of r o t a t i o n (or translation) of j o i n t

j * 1

cos a, cos 6, - sin a,- cos 0, sin a, cos a.

Link j - 1

Fig. 2 Parameters relating adjacent link coordinate systems

input torques at a particular trajectory point (specified joint positions and velocities). Four methods for solving the equations will be discussed and compared with respect to their relative computational efficiency. Three of these methods are based on recently developed techniques for solving the inverse problem; that is, given relative joint positions, rates, and accelerations, solve for the relative joint torques. The motivation for basing the present work on these techniques is their relative computational efficiency [11]. In particular, the best techniques are based on the Newton-Euler approach [5-7] in which the computation time grows linearly with the number of degreesof-freedom. A fourth method is discussed and is based upon a recursive technique for constructing the moment of inertia matrix. The concept of composite center of mass and moment of inertia matrix of a partial set of links at the open end of the chain is used for this purpose. For each of the four methods, the relative computational complexity will be enumerated in terms of the number of floating point multiplications (divisions) and additions (subtractions) required as a function of the number of degrees-of-freedom. These methods will be further compared by using each of them in a dynamic computer simulation of the Nordson Corporation industrial robot. This is a spraypainting robot with six degrees-of-freedom. The execution times for each of the simulation programs will be given. General Form of Kinematic and Dynamic Equations of Motion for a Manipulator The manipulator to be simulated is an open-chain mechanisn consisting of a sequence of N+ 1 rigid links. Each link is numbered assigning the base of the manipulator as link O and increasing outward to the final link, link N. Between link / and link i 1 is a joint numbered i which has only one degree-of-freedom. Thus, the manipulator has iVjoints giving it N degrees-of-freedom. If a joint is rotational, the actuator at that joint produces a torque; and, if it is translational, the actuator at that joint produces a force. The remainder of this section gives the procedure used in describing the geometry of the manipulator and its dynamics. First, the kinematics of the manipulator are defined. That is, a coordinate system for each link is defined whose position and orientation with respect to that link is fixed; the relationship between successive link coordinate systems is then described. Next, for each link, its mass, its center of mass, and its moment of inertia matrix, all referenced to the 206/ Vol. 104, SEPTEMBER 1982

By successive multiplication of these link coordinate matrices, the direction cosine matrix of link ;' coordinates referenced to link O coordinates, Ad may be obtained: Aj, = AjA? (2) If p, denotes the position of the origin of link i coordinates with respect to the origin of link O coordinates and p* denotes its position with respect to link i - 1 coordinates, then: P*=P,--P/-i(3) The components of p* may be determined with respect to any of the coordinate systems associated with the manipulator. A leading superscript will be used to make reference to any particular coordinate system. The relationship between the components of p* referenced to link / coordinates ('pf) and base coordinates (pf) and further with the parameters of link / are important ones and are given as follows: a; ip!=(A 0) p*=Afpt=
i T

d, sin a,

(4)

d, cos a.-. (Note that A? is just the transpose of A'0.) These equations define the kinematics of the manipulator. Newton-Euler Formulation of Dynamic Equations of Motion. Once a link coordinate system has been defined for each link, the mass properties of each link can be specified. Let rrij denote the mass of link /, s, denote the location of the center of mass of link / with respect to the origin of link i coordinates, and J, denote the inertia matrix of link;' about its center of mass. Note that s, and J, are constants when referred to their own link coordinates; that is 's, and 'J, are constants, and it is these quantities along with the m, that are explicitly used as input parameters to the dynamic equations of motion. The procedure in the Newton-Euler formulation of the dynamic equations of motion is to first write the equations which define the angular and linear velocities and accelerations of each link and then write the equations which relate the forces and moments exerted on successive links while under this motion. The equations are [6]: *, = m,r ; N,=J ; co ; + a>,.x(J,-.&>,-) f/=F,+f/+1 n,=n/+1+N, + (pf+s/)xF,-f-p,xf/+1 (5) (6) (7) (8) (9)

f; = CO; X S; + CO; X ( 40, X S; ) + j);

Transactions of the ASME

Downloaded 12 Sep 2011 to 141.213.169.57. Redistribution subject to ASME license or copyright; see http://www.asme.org/terms/Terms_Use.cfm

For joint i rotational: w/ = w,_ 1 +z / _,9 / to, = o);_i +z,_,<7, + a)/i XZ,_ig,
p, = a ) , X p f + w , X ( c o , X p f ) + p , _ ,

(10) (11)
(12)

These two subroutines, SUB1 and SUB2, form the basic programs used for simulating the rigid body dynamics of the manipulator. Equations of Motion With Constraint Forces and Moments Eliminated. The equations in the previous section contain within the f, and n, the forces and moments due to physical constraints at each joint. To simulate the manipulator, these forces and moments are eliminated from the equations. Hooker and Margulies have shown how this can be done for the Newton-Euler formulation [13, 14]. In some of the more classical formulations, the forces and moments due to physical constraints never appear. What is required for the simulation is not an explicit analytic expression for each of the terms in this equation but merely the general form of this equation. It is: H(q)q + C(q,q)q + G(q) + K(q) r k=T where H(q) C(q,q) G(q) K(q) TVx T symmetric, nonsingular moment of V inertia matrix. TVX T matrix specifying centrifugal and coriolis V effects. TVx 1 vector specifying the effects due to gravity. 6xTV Jacobian matrix specifying the torques (forces) created at each joint due to external forces and moments exerted on link TV. K(q) T indicates the transpose of K(q). 6 x 1 vector of external moments and forces exerted on link TV. The first three components are n e and the last three components are f e. TVx 1 vector of torques (forces) of each joint actuator. The /th component is 77. TVx 1 vector of joint variables. The /th component is <7,. (22)

7, = z,_,.n, For joint / translational: ,- = w/-i

ft); = ft),-l-

(13)

(14)
(15)

P/=Z/-i4f; + w,-xpf
+ 2ft), X (Z;_ ,<?;) +ft),X ( 0), X p?) + P;_ , T,=*,-l-t,. (16) (17)

Previously undefined terms are given as follows: ft), angular velocity of link / < ) angular acceleration of link/ <, f, linear acceleration of link / center of mass F, total force exerted on link / N, total moment exerted on link / f, force exerted on link i by link i - 1 n, moment exerted on link / by link / - 1 7, torque exerted by actuator at joint / if rotational, force if translational <7, joint variable at joint i (0, if rotational, rf, if translational). These equations can be used to compute joint torques (forces) efficiently by referencing the /th link velocities, accelerations, forces and moments to the /th link coordinate system [6]. First, the following are initialized to indicate a stationary base:
ft>o=ft>o=0

k 7 q

(18) (19)

p 0 = [0,0,9.80621 m / s 2 ] r .
2

(The 9.80621 m / s is the acceleration due to gravity and the base coordinates are set up so that gravity acts in the positive Zo direction. Also, note that a leading subscript of O, to indicate base coordinates, is usually eliminated for simplicity.) From this, all of the to,, ft),, r,, p,, N, and F, are computed. Next, if there is an external force fe and moment ne exerted on link TV, then: tN = te nA' = n c . (20) (21)

In the following section, several different methods for solving the joint accelerations based upon the form of equation 22 will be described. Each of these methods is compared with respect to the number of multiplications and additions required to implement them. Reference to the total multiplications includes both multiplications and divisions. Similarly, total additions refers to the total number of additions and subtractions. Techniques for Solving for the Joint Accelerations Method 1. From equation 22, note that the joint torques (forces) are linear functions of the joint accelerations. Therefore, if b is defined to be a bias .vector which is equal to the torques (forces) due to gravity, centrifugal and coriolis accelerations, and external forces and moments on link TV, then b = C(q,q)q + G(q) + K(q) r k. (23) Thus, the accelerations of the joint variables can be obtained by solving the linear equation H(q)q = ( 7 - b ) . (24) This bias vector b can easily be computed by setting q, q and k to their current state, but letting q = 0, and calling SUB1 (q, q, 0, k, b). That is, the torque computed by SUB1 with these input values is equal to the bias vector b. The difficult part in solving equation 24 is in evaluating the elements of the matrix H. In this first method, this is accomplished by setting q to its current state, but letting q = e,-, and calling SUB2 (q, ey, h,), where ey- is an TVx 1 vector with the y'th element equal to 1 and 0 everywhere else, and h, is the y'th column of H. That is, h, is the torque (force) on the joint actuators when: the joint velocities are zero; there are no SEPTEMBER 1982, Vol. 104/207

Otherwise, these are set equal to 0. All of the n,, f,, are 7, are then computed. A FORTRAN subroutine, called SUB1 (q, q, q, k, 7) was written to compute the 7 given q, q, q and the forces and moments exerted on link TV, k. For a manipulator with T V links, the computational complexity of this program measured in terms of the number of multiplications and additions is: 137TV- 22 multiplications 1017V-11 additions. Another FORTRAN subroutine, called SUB2 (q, q, 7) was written which is identical to SUB1 (q, q, q, k, 7) except that all of the code for including the velocity terms, the gravitational effects and the effects due to external forces and moments is eliminated. This reduces the computational complexity of SUB2 (q, q, 7) to: IAN-22 multiplications 547V- 17 additions. Journal of Dynamic Systems, Measurement, and Control

Downloaded 12 Sep 2011 to 141.213.169.57. Redistribution subject to ASME license or copyright; see http://www.asme.org/terms/Terms_Use.cfm

external forces; there are no gravitational effects; and the joint accelerations, q, are equal to e,. (See equation 22.) Once the elements of H are determined, then the joint accelerations are obtained by solving equation 24. The exact procedure with the computational complexity at each step is; STEP 1: Compute the bias vector b, using SUB1. 1377V-22 multiplications 101 TV-11 additions STEP 2: Compute the matrix H one column at a time, using SUB2. For T columns: V 74N2-22N multiplications 547V2 - 177V additions. STEP 3: Solve the linear equation 24 for the joint accelerations. (Triangular Decomposition is used [15].) (1 /6)7V3 + (1 1 /2)7V2 - (2/3)7V multiplications (1/6)7^ +7V2 - (1 1/6)7V additions The total number of multiplications and additions in this method are: (1/6)7^ + (75 1/2)7^ + (114 1/3)7V- 22 (1/6)7^ + 557V2 + (82 5/6)7V- 11 multiplications additions.

rigid body. Given this composite system of links with its total mass, the location of its center of mass and its moment of inertia, then since joint j is the only joint in motion, the total force F,, and moment Ny, exerted on the composite system of linksy through T would be (for ijj = 1): V Fj=Mj\j=Mj(Zj_ixcj)=Zj_lx(MjCj) "| (25) joint y rotational (26) (27) joint y translational (28)

N, = Ejz,_,
VJ=MJZJ.

Nj = 0 where previously undefined symbols are:

Method 2. This method is the same as Method 1 except that a different algorithm is used for computing the matrix H. Since this matrix is symmetric, only the diagonal and the bottom half of the off-diagonal terms are computed. Thus, the components of H are computed in the following order:
HN,N\ HN,N-I'> HN-I,N-I\ HNIN-Z', HN-I,N-2', HN^2,N-I\ etc.

Mj total mass of links j through TV vy linear acceleration of the center of mass of the composite system of linksy through TV cy the location of the composite center of mass of links j through T with respect to the origin of link y ' - l V coordinates Ey the moment of inertia matrix of the composite system of linksy through TV With this acceleration, F, and N, are all zero for i < j . Using this in equations (7) and (8) gives: for/ = l , . . . . , y - l (29) (30)

The reason for computing them in this order is that SUB2 may f = f/+i / be used by thinking of the last Nj + 2 links as a separate n,=n,+ 1+pfxf,+1 manipulator with TVy + 1 degrees-of-freedom. Starting at l i n k y ' - l as the base link, set <?y = l, <z,=0, i=j+l, . . . ,7V and and solve for the resulting joint torques (forces) at joints j through7VusingSUB2(q, e,, h,-, j-l). (Note that an additional parameter, y - 1, is being passed to SUB2 to indicate that the forward computations of the link accelerations are to start at link./- 1, and the reverse computations of the forces and moments are to end at link y ' - l . ) The last TVy'+l elements of hy give us Hj y, H,j+ij> HNJ and HjJ+l; H, " -LJJ+2 -Hjjy. ~ Each call to SUB 2 requires: 74(7V-y'+ 1) - 22 multiplications 54(7V-y'+ 1 ) - 17 additions Therefore, the total number of computations required to build the entire matrix H is:
N

Center of Mass

J (74(7V-y'+ 1) - 22) = 377V2 + 157V multiplications

7=1

Fig. 3
N

Position of center of mass G with respect to coordinate 0

]j (54(TV-y + 1) - 17) = 277V + 107V additions.

7=1

The remainder of the procedure for computing the joint accelerations is exactly the same as in Method 1. Thus, the total number of multiplications and additions for this method is: (1/6)TV3 +(38 1 / 2 ) ^ +(151 l/3)7V-22 multiplications (1/6)7V3 + 287V2 + (109 5/6)7V- 11 additions. Method 3. This method is the same as in Methods 1 and 2 except that a different procedure is used for computing the inertia matrix, H. In this method only the diagonal and top half of the off-diagonal terms are computed. The components of the inertia matrix are computed in the following order:
^WJVI HN-I,N> HN-2,N HlN; HN_[N_{, HN_1N_i,

. . . Hl./v-i; etc. To do this, first note that when solving for the y'-th column of the inertia matrix in Method 1, q, = O for i > j . That is, all of the last TVy + 1 links appear as a single 208/ Vol. 104, SEPTEMBER 1982

Fig. 4

illustration of composite center of mass of links / through N

Transactions of the ASME

Downloaded 12 Sep 2011 to 141.213.169.57. Redistribution subject to ASME license or copyright; see http://www.asme.org/terms/Terms_Use.cfm

tj = j

(31)

My=My+1 y'-i

+mj

(44)

n, = N, + c , + F , .

(32)

Thus, starting with i=j, equations (29) through (32) may be used to obtain all of the n, and f,, for / < j . The components of the moment of inertia matrix along column j are then equal to the torques or forces generated at the joints. Thus, for / < j : _ i -n, H<i = ^z,_! f, joint/ rotational (33) joint / translational

=Ay^=Aj_ 1 [~[my(; S y+; p ;) +A^+ie'cy+1+;P;)]]

(45)

~ ' Ey = Ay^ , ( ^ , + My+ , [(JCj+ , +^pj-JCj ) . (JCj + , +

J

9f-Jcj)I +'pj-jcj)l }Aj-'. (46) (47) (48) >-'Ny=^'EyZ0 (49) (50)

| ^ 1 ny=^ 1 Ny+J-' fCyX- / '-'Fy f, i ;+i

+j3j + mjl(->sj +lpj-JCj). (34) (35) (36)

(hj

To compute the My, Cy, and Ey, start at link N: MN = mN

C/V SN + pjv

- (;sy + ; P / - ; C y ) (>sy +Jp}-Jcj)T]

J 1

The equations used for computing Hy i < j are:

- J = Z0XMJJ-,CJ

where m.j = mass of link j Sj = location of the center of mass of link j from link j coordinates J, = the moment of inertia matrix of linky. To solve for the My, C and Ey, the following recursive y equations may be used:
Mj=MJ+l +rrij

(51) (52) (53)

=Af_,('n, + , + ' p ? x ' f / + 1 ) z component of'" n,

"u =
1

joint/rotational joint/translational

(37) + pj)) (38)

z component of'"

f,

c> = (mj (Sj + pj) +Mj+,(cy+,

E

where:

; = Ey+i +Mj+l[(cj+i

+pf-Cj)-(cJ+i+pJ-Cj)l
C ) T] y

- (Cj+ 1 + P * ~ Cj ) ( Cy+ i + Pj-

(54)
(39)

+ Jy +/y[(Sy + P ; - C y ) (Sy + P ; - C y ) I - ( S y + P j - C y ) ( S y + P ; - C y ) T]

where I is the 3 x 3 identity matrix. From equation (37), My is just the sum of the mass of linky and the total composite mass of links j+l through TV. From equation (38), the composite center of mass of links j through TV, cy, can be obtained from a knowledge of the location of the center of mass of link j , Sy, and the location of the composite center of mass of links j + 1 through TV, cj+1. In an analogous manner, once Cy+1, My +1 , and Ey+1 have been determined, the parallel axis theorem for a moment of inertia matrix may be used to determine the moment of inertia matrix of the composite system of links j through T relative to V Cy. The parallel axis theorem is a method for obtaining the moment of inertia matrix J 0 of a rigid body about an arbitrary origin of coordinates O in terms of the inertia matrix J G relative to the center of mass [16]. Let r be the coordinate of G relative to 0 (Fig. 3) and Mbe the mass, then J0=JG+M(r.r)I-rr7') (40) Thus, the first two terms in equation (39) are the contribution to Ey by the composite of links j+1 through N, and the last two terms are the contributions to Ey by link./' (Fig. 4). In equations (25) through (39) it has been implied that all vectors are referenced to link O coordinates and because of this, they are quite difficult to compute. As in [6], to ease the computational burden, all mass properties of the composite system of links j through N are referenced to link j -1 coordinates. Thus, the equations used for computing the mass properties of the composite system of links j through N, for j= 1 . . . TVbecome MN~mN
V - l - _At N (N J - N n * 1 l - I V f J V - K S N ^ VN) N - l l f ILN N r J i V - 1

The procedure for computing all of the Hy is as follows: STEP1: ComputeMy.J-'cy^-'Ey j=N. . .1 One iteration of equation (44) through equation (46) requires 85 multiplications and 84 additions. Getting the procedure starting with equations (41) through (43) requires: 58 multiplications and 31 additions. Thus, the step requires a total of 85/V-27 847V- 35 STEP2: multiplications additions.

Compute J-%, >-'Nyj=l...N using equation (47) and equation (48). This requires a total of: 77V AN multiplications additions.

STEP 3:

F o r y = i V . . . 1 compute 'f,, ' " ' n , , i=j . . . 1 , using equations (49) through (52). For a particular j , computing the /_1f, and the '~'n,, i=j . . . 1 requires a total of 24(/'-l) multiplications and 1 4 0 - 1 ) additions. Repeating this for j=N . , . 1 requires a total of: 12N2-36N 7A^-21N multiplications additions

(41) (42) (43)

Therefore, the total number of computations required to compute the moment of inertia matrix is: 12/V2 + 567V- 27 77V2 + 677V-53 multiplications additions

Using this procedure for computing the moment of inertia SEPTEMBER 1982, Vol. 104/209

Journal of Dynamic Systems, Measurement, and Control

Downloaded 12 Sep 2011 to 141.213.169.57. Redistribution subject to ASME license or copyright; see http://www.asme.org/terms/Terms_Use.cfm

matrix H and the algorithm described in Method 1 for solving for the joint accelerations requires a total of: (1/6)7^ + (13 1/2)7^ + (192 1/3)7V- 49 (1/6)7^ + 87V2 + (166 5/6)N- 64 multiplications additions.

Method 4. An iterative procedure is used in this method to solve for the joint accelerations. That is, an initial estimate for the joint accelerations is made and followed by successive adjustments to these variables until they converge to the correct solution. The procedure used is the conjugate gradient technique which may be used for solving a set of linear equations when the coefficient matrix is symmetric and nonsingular [17]. The conjugate gradient technique for solving a set of linear equations may be briefly stated as follows: Problem: Solution: where Solve the equation B x = y when B is a symmetric nonsingular matrix. Successively compute u,, w,, and x,x = initial estimate 0=w0=y-Bx0 7, = (w J ) w,)/(u,,Bu,)
;

Fig. 5 Nordson manipulator showing assignment of link coordinates

(55) (56) (57) (58) (59)

1 XR)rrz> trt
A Afflpllfil

=5

Rigid Body Dynamics

/+i=x,- + 7/U; 0/=(w / + 1 > w ; + 1 )/(w ; ,w ( )

= w,

ft
Fig. 6 Block diagram of analog position control system being simulated

'/+i=W/-7,-Bu,-

(60) (c, d) indicates the inner product of two vectors c and d, or

+ ftu,-.

(c,d) = c r d

(61)

Table 1 Execution times of four different methods of a five second trajectory

Method 1 2 3 4 Execution time 65.663 s 50.712s 37.395 s 61.910s

If there are no round-off errors, x, will be the solution for m < TV, where T is the number of equations. The w, are V residual vectors; that is: w,=y-Bx, (62) Therefore, the solution has converged when y,- = 0. Inspection of equation (24) reveals that the conjugate gradient technique can be used for solving the joint accelerations by letting: y= r-b B = H(q) x = q. (63) (64) (65) STEP 6:

STEP 4: STEP 5:

If (p, = 0) stop; else continue. Sett = H(q)u,. 747V-22 547V-17 7V+1 TV-1 multiplications additions multiplications additions multiplications additions

Evaluating equations (63) and (64) and substituting the results into equations (55) through (60) is the same basic procedure as in Methods 1 and 2, except that the conjugate gradient technique is used to solve the linear equation (24), which is a less efficient technique for solving linear equations than the triangular decomposition method used in Methods 1 and 2. The efficiency of using the conjugate gradient technique is obtained by noting that the moment of inertia matrix H (B) is only used when being multiplied by a vector in equations (55), (56), and (58). Therefore, SUB2 may be used to evaluate this product without ever explicitly solving for the elements of the matrix H. The complete procedure for solving for the joint accelerations is as follows: STEP 1: STEP 2: Estimate solution x 0 . Set i = 0. u 0 = w 0 = T-H(q)x0-C(q,q)q-G(q)-K(q)7'k 1377V= 22 1017V- 5 STEP 3: TV 7V-1 multiplications additions multiplications additions

Set7,=p,/(u,,t).

STEP7:

SetX/ + ,=x / + 7/u / . TV TV

STEP 8: STEP 9:

If ( i = / V - 1) stop; else continue. Setw, + 1 = w , - 7 , t . TV TV multiplications additions multiplications additions pi+l/Pi. multiplication

STEP 10:

Setp, + 1 =(w, + 1 ,w, + 1 ). TV TV-1

STEP 11: STEP 12: STEP 13:

If (pi+1 = 0) stop; else continue. Setp, = 1

Setp, = ( w , , w , ) .

Setu / + 1 =w / + 1 +|8/U;. T V additions TV multiplications Transactions of the ASME

210/ Vol. 104, SEPTEMBER 1982

Downloaded 12 Sep 2011 to 141.213.169.57. Redistribution subject to ASME license or copyright; see http://www.asme.org/terms/Terms_Use.cfm

Table 2 Number of computations to solve for joint accelerations

# Mul. Method 1 2 3 4 # Multiplications l / 6 W 3 + 75 1/2N 2 + 114 1/37V-22 1/67V3 + 3 8 l/2/V 2 + 151 l / 3 A r - 2 2 1/6/V3 + 1 3 l/2/V 2 + 192 1 / 3 / V - 4 9 76TV + 1 2 0 / V - 2 1
2

# Add. (N=6) 2502 1692 1261 2532

# Additions 1/6/V3 +55/V 2 + 8 2 5 / 6 / V - 11 1/6/V3 +28/V 2 + 109 5/6TV- 11 1/6/V 3 + 8/V2 + 1 6 6 5 / 6 / V - 6 4 56/V + 8 7 / V - 6
2

(N=6) 3418 2308 1627 3435

Set/ = /+l;gotostep5. 1 addition Adding all the multiplications and additions and assuming the iterative procedure is repeated the maximum of times (TV), then the total number of computations are: 76TV2 + 120TV-21 multiplications 56TV2 + 87TV- 6 additions Simulations Results. Each of the four methods of computing the joint accelerations were programmed in FORTRAN and used in a simulation of the Nordson robot (Fig. 5). The manipulator has six degrees-of-freedom with hydraulically driven rotary actuators. A block diagram of an analog position controller also included in the simulation is shown in Fig. 6. Each current amplifier converts a position error signal into a current which drives the servo valve. The hydraulics section includes a second order model for the flow control valve, and an integrator to model the actuator. The output of the hydraulics section is the torque generated by the actuators. This is the input to the rigid body section where one of the four methods described above is used to solve for the joint accelerations. Integrating this acceleration once gives the velocity and once again gives the position of the joint variables. Therefore, for each joint there exist five state variables giving a total of 30 state variables that must be integrated in the manipulator simulation. Table 1 gives the execution time of this system simulation using a Cyber 175. The integration technique used in the simulation is the fifth order Runga-Kutta with a fixed step size. A step size of four milliseconds was chosen which implies that the derivative of the state variables were evaluted 1,250 times for every second of simulation time. Conclusion In this paper, four different methods for solving for the joint accelerations have been described. The problem has been solved utilizing efficient computational schemes for solving the inverse problem. That is, given joint variable accelerations, velocities and positions solve for the joint actuator torques (forces). Table 2 lists a comparison of the computational complexity of each of the four methods. First note that Method 4 has an T 2 dependency while the V others have an TV3 dependency. However, Method 4 requires the largest number of computations. Method 1 and Method 4 are similar in that they both use the inverse plant programs, SUB1 and SUB2, the same number of times. The reason Method 1 is more efficient is that for T = 6, it requires less V computations to solve a set of linear equations by triangular decomposition than it does to compute the magnitudes yh /3, and directions u,, w, in the conjugate gradient technique. Referring to Table 1, it is seen that Method 4 actually required less time to execute. This is due to the occasional convergence of the iterative procedure in Method 4 in less than T V iterations. Method 2 provides a large improvement over Method 1 by taking advantage of the fact that H is symmetric and only computing the off-diagonal terms once. The result was that the TV2 dependency decreased and the T or linear dependency V increased.
Journal of Dynamic Systems, Measurement, and Control

STEP 14:

Method 3 was the most efficient. This method takes into account the fact that the moment of inertia matrix H is symmetric as in Method 2, but it also utilizes a recursive procedure for computing the mass, the center of mass, and the moment of inertia matrix of the composite system of links j through TV. Again, the T 2 dependency decreased and the V linear dependency increased over that of using Method 2. Thus, Method 3 provides an efficient method of simulating a robotic mechanism, as well as providing some insight into the form of the moment of inertia matrix H. This insight could prove useful in the future design of robotic mechanisms. Acknowledgment This work was supported by Nordson Corporation and by the National Science Foundation under Grant ENG-7818957 to the Ohio State University. References
1 Paul, R. P. C , "Modeling, Trajectory Calculation and Servoing of a Computer Controlled Arm," A.I. Memo 177, Stanford Artificial Intelligence Laboratory, Stanford University, Stanford, Calif., Sept. 1972. 2 Uicker, J. J., "On the Dynamic Analysis of Spatial Linkages Using 4 x 4 Matrices," doctoral dissertation, Northwestern University, Evanston, 111., 1965. 3 Lewis, R. A., "Autonomous Manipulation of a Robot: Summary of Manipulator Software Functions," Technical Memorandum 33-679, Jet Propulsion Laboratory, Pasadena, Calif., Mar. 1974. 4 Williams, R. J., and Seireg, A., "Interactive Modeling and Analysis of Open or Closed Loop Dynamic Systems with Redundant Actuators," ASME Journal of Mechanical Design, Vol. 101, No. 3, July 1979, pp. 407-416. 5 Orin, D. E., McGhee, R. B., Vukobratovic, M., and Hartoch, G., "Kinematic and Kinetic Analysis of Open-Chain Linkages Utilizing NewtonEuler Methods," MathematicalBiosciences, Vol. 43, No. 1/2, Feb. 1979, pp. 107-130. 6 Luh, J. Y. S., Walker, M. W., and Paul, R. P. C , "On-Line Computational Scheme for Mechanical Manipulators," ASME JOURNAL OF
DYNAMIC SYSTEMS, MEASUREMENT, AND CONTROL, Vol. 102, June 1980, pp.

69-76. 7 Stepanenko, Y., and Vukobratovic, M., "Dynamics of Articulated OpenChain Active Mechanisms," Mathematical Biosciences, Vol. 28, No. 1/2,1976. 8 Dillon, S. R., "Computer Assisted Equation Generation in Linkage Dynamics," doctoral dissertation, Ohio State University, Columbus, Ohio, Aug. 1973. 9 Liegeois, A., Khalil, W., Dumas, J. M., and Renaud, M., "Mathematical and Computer Models of Interconnected Mechanical Systems," Proc. of the Symposium on Theory and Practice of Robots and Manipulators, Warsaw, Poland, Sept. 1976. 10 Hemami, H., and Jaswa, V. C , "On a Three-Link Model of the Dynamics of Standing Up and Sitting Down," IEEE Trans, on Systems, Man, and Cybernetics, Vol. SMC-8, No. 2, Feb. 1978, pp. 115-120. 11 Hollerbach, J. M., "A Recursive Lagrangian Formulation of Manipulator Dynamics and a Comparative Study of Dynamics Formulation Complexity," IEEE Trans, on Systems, Man, and Cybernetics, Vol. SMC-10, No. 11, Nov. 1980, pp. 730-736. 12 Denavit, J., and Hartenberg, R. S., "A Kinematic Notation for LowerPair Mechanisms Based on Matrices," ASME Journal of Applied Mechanics, June 1955, pp. 215-221. 13 Hooker, W. W., and Margulies, G., "The Dynamical Attitude Equations for an n-Body Satellite," Journal Astronaut Science, XII (4), 123,Winter, 1965. 14 Hooker, W. W., "A Set of r Dynamical Attitude Equations for an Arbitrary n-Body Satellite Having r Rotational Degrees of Freedom," AIAA J., Vol. 8, No. 7, 1970, pp. 1205-1207. 15 Nobel, B., Applied Linear Algebra, Prentice-Hall, New Jersey, 1969. 16 Symon, K. R., Mechanics, 3rded., Addison-Wesley, 1971. 17 Beckman, F. S., "The Solution of Linear Equations by the Conjugate Gradient Method," Mathematical Methods for Digital Computers, Wiley, 1960.

SEPTEMBER 1982, Vol. 104/211

Downloaded 12 Sep 2011 to 141.213.169.57. Redistribution subject to ASME license or copyright; see http://www.asme.org/terms/Terms_Use.cfm