Efficient recursive algorithm for inverse dynamics

Mechatronics,Vol. 3, No. 2, pp. 205-214, 1993

09574158/93 $6.00+0.00 Pergamon Press Ltd

Printed in Great Britain

EFFICIENT RECURSIVE A L G O R I T H M FOR INVERSE DYNAMICS

IMRE J . R U D A S - A K o s T 0 ~ n

B~tki Don~tPolytechnic, Department of Computer Science H-1081 Budapest, N6pszfnhfizu. 8. Hungary Tel.: (361) 133-4513. Fax: (361) 133-9183 Abstract - An efficient recursive computational scheme for the robot manipulator inverse dynamics is outlined. The algorithm is a new computational approach and is based on the application of the Gibbs-function. The solution of the inverse dynamic problem is formulated as a problem in quadratic programming, and the recursive equations for the computation of the driving torques are derived from the necessary conditions of minimum usage of the Lagrange method of undetermined multipliers. The algorithm involves the recursive computations of links' angular velocities, angular accelerations and linear accelerations expressed in moving coordinate systems. Further simplification can be performed by writing the vector equations as equations of coordinates. The reduced computational scheme is presented by a Pascal program detail. The computational complexity of the 'algorithm is analyzed.

1. I N T R O D U C T I O N The problem of robot control is mainly due to the nonlinear system dynamics. In order to achieve good tracking of the desired trajectory, it is necessary to take care of the dynamics of the robot manipulator. This means that in the control synthesis the dynamic of the robot should be included. In the available literature, a wide class of control schemes and methodologies was presented which is based on the inverse dynamic approach [4], [5], [6]. Inverse dynamics of robot manipulators is defined as the on-line computation of driving torques using the joint coordinates, velocities and accelerations. The numerical calculations necessary for the dynamic control synthesis cannot be carried out in real-time unless the number of mathematical operations is reduced. The aim of this paper is to present an efficient computational scheme for the computation of the driving torques. For simplicity only rotary joints are considered. Similar discussions hold for prismatic joints. 2, K I N E M A T I C S The robot manipulator is considered as an articulated chain of N rigid bodies which are serially connected by revolutejoints. There is one coordinate system attached to each link of the manipulator according to the Denavit and Hartenberg convention [ 1]. The base coordinate system (x 0, Y0, z o ) attached to link 0. Let r~i_ t be a vector from coordinate system i - 1 to the ktn point fixed in link i expressed in link i - 1 coordinates. Then adjacent coordinate systems are related by the relation: rki,i-1 "~ Ai,i-lrik, i + Pi-l,i,

(1)

where Ai,i_ t is the rotation matrix [cos0i Ai,i_ 1 = | s i n 0 i

ko

- s i n O i c o s c t i sin Oi s i n c t i ] c o s 0 i c o s a i - c o s 0 i sin a i ,J sin cti cos a i 205

(2)

206

I . J . RUDAS and A. T 6 T H

and Pi-l,i is the vector from coordinate origin i - 1 coordinate origin i expressed in link i coordinates

Pi

l.i

=

I'] S i sin

a,

.

(3)

S i COS O~i

We define Ao._ 1 = I the identity matrix. The coordinate system i and the base coordinate system can be related by cascading the transformations: r~o = Ai.o r~i + P/.o'

(4)

where Ai. 0 = AI,oA2.1...Ai,i_ 1

(5)

Pi.0

(6)

and = Pi-I.0

+ Ai.oPi-l.i"

Let (hi. 0 and ai. o be, respectively, the angular and linear acceleration of the center of mass of link i with reference to base coordinate system. The kinematic equations are [1 ]: 0),.0

(7)

= 0)i 1.0 + Z i - l C l i •

(-Oi,0 = (-0i 1.0 + 0),-1,0 X Z i _ l q i + Zi_lgli,

(8)

a,,o = ai-l.o + &,.o x Pi-l.i + wi-l.o × (0)i-l.o x Pi-,),

(9)

~',.o = 0),0 x ( 0)i.o × s, , ) + 60,o × s,.~ + p,.o.

(1o)

where previously undefined terms are: si. i is the vector from coordinate origin to the link i center of mass, i:i. o is the acceleration of the center of mass of link i. The matrix Aro transforms these vectors to a new coordinate system whose axes are parallel to (Xo, Yo, Zo ), but its origin is the same as that of ( x i , Yi, zi ). Denoting these transformed vectors by cb~,,, a~., and i:,., we obtain

(°i,i = A~iotbi,o,

ai.i = A/ioai,o,

f'i.i = A~ioi:i.o,

(11)

Define the operator ~,,, as follows

ffZ,,, = A/IoA,. o.

(12)

As is known ~i,, is skew-symmetricoperator, which can be represented in the ( i i , Ji, k, ) coordinate system by the following matrix 0 0)( 3 ) i .(i2 )

L- 0 ) i , ~ where 0),,i = 0)l)]ii + 0)i.i Ji + 0)l},'k,

_ 0 ) ~ 3 ~ 0)!2) Li

0 0)(1, i.i

t.t

_0)0~ i.i

0

(13)

Efficient recursive algorithm for inverse dynamics

207

In general, if a skew-symmetric operator defined on the normal three dimensional Euclidean space and represented by a skew-symmetric matrix, then

X = (1)i, i X St. i -~ ~i,iSi,i .

(14)

Using this equality Eq. (7), Eq. (8), Eq. (9) and Eq. (10) can be written in the form T

OJi, i = Ai,i_lO)i_l,i_l

+ wiqi,

(J)i, i = ATi-l(Oi-l,i-1 q- fikTi,i_lfOi_l,i_l -[- W i l t , T 2 at. i = Ai,i_lai_l,i_ 1 + Xi(~oi. i + ff2i.iPi_l,i, ri,i = ~22isi,i + •i.isi,i where

w

i

=

0 X i ---- --Si COS O'i si sin ai

3.THE G I B B S - F U N C T I O N

rol sin a i LCOS a i

Si COS /~i 0 -ai

+

ai,0,

(15) (16) (17) (18)

(19)

,

--Si sin a i ] a~ J 0

(20)

OF A ROBOT MANIPULATOR

The Gibbs-function of a robot manipulator can be written in the form N

G=12mi(ri,o) 2 i=1

..

2

,

(21)

where r/. 0 is the vector of the center of mass of link i with reference to the base coordinate system, and ml is the mass of link i. Using the identity

(ri, o) = tr

= tr _ / , _ i ,

(22)

and substituting Eq. (18) into Eq. (22) the Gibbs-function of the robot manipulator after suitable transformation will be the form:

, , , 2ff2~,ini,iai,i+ , 1 2ff2i,ini.iaTi + miai,iaTi}, G = l ~ t r _ {ff2i,iJi,i~2[i+ 2.Q2iJii.(2r,.i+

(23)

where

Jxixi Ji.i = Jyixi Jzix i

Jxiyi Jyiyi Jziy i

Jxiz, 7 Jyizi Jzjzi

is the inertia tensor with respect to the proximal joint of link i expressed in i's body coordinates,

(24)

I. J. RUDAS

208

and A. T6TH

n. = n”‘i_ + nc2)* ,,, Ji + nj,3,‘ki 1.1 1.1’

(25)

is the vector of statical moments of link i expressed in i’s body coordinates. It is easy to verify by simple computation that Eq. (23) can be transformed into the following form: G = k$

W[iFiW, i + 2 c$~H~w, i + 2 &?,Tiai,i + 2 O$;G,ai.i + m,aEiai,,,

(26)

I-1

where the matrices are: J Fi

=

Jw

Y&Y, +

- Jw,

-Jy,x‘

I

Jx,xt

+

-J,,,,

Jziz,

-Jm

-J,&

0 G, =

n(2’ l.1

0

-n(1) (,

,(I) (.I

0

-,IZ) 1.1

1,

(27)

J&X,+ Jhh 1

-d3.’ 1.1

$’

3

-Jy,z,

,

(28)

(29)

-W!“J

x,2,

I,,

0

-u!?J. 1.1

l,i

-

),I,

_w'~'J w!?J 2,x

-

Z,I.

tw!“J 1.‘

28)

0

1,‘

-&'J

X,Y, -

-d2'J )eY 1.1

-

i.l j,Z8

1.1

+d2'J L,L

),X

+ +

+d3’J 1.1 2,x,

x,x -

X,Y&G

‘,l

u!'!J 11x2

+

+w!?J 1.1 Y,Y,+ +d3'J 1.1 y,*, I,,

+

1.1

-Q)‘“‘J

*,z,

-QJ’“J

+cj~!~'J 224 -o!?J 1.1

1.1

-W!"J

+

1.i

Hi =

u!‘!J

0

(30)

Efficient recursive algorithm for inverse dynamics

209

4. T H E S O L U T I O N O F T H E I N V E R S E D Y N A M I C P R O B L E M Consider the robot manipulator with a prescribed configuration and velocity at time t. The acceleration is such that N

(31)

J = G - Y~ Tiq i i=l

has a minimum [3], and the accelerations have to satisfy the following equations obtained from Eq. (16) and Eq. (17): = COi.i - A i .Ti - l o g i•- l , i - 1

bi

T Ci = a i , i - A i , i - l a i - l , i - i

at- A T i _ 1 o g i - l , i - 1 q- W i # i : 0 ,

(32)

2 + X i (J)i.i -b ~'-2i.iPi_l, i -- 0 .

(33)

Thus, the calculation of the accelerations is reduced to the task of quadratic programming, i.e. to the minimalization of the function J defined by Eq. (31), along with constraints given by Eq. (32) and Eq. (33). This is achieved by the method of L a g r a n g e multipliers. Let us introduce the L a g r a n g e - f u n c t i o n N

L ---- J + i=l ~ ' . { .~Ti b i

+fl/ci},

(34)

where gi and fli, i = 1, 2 . . . . . N, are the L a g r a n g e ' s undetermined multipliers. The function L can be minimalized without constraints and has a minimum for the same value of the accelerations. Using the necessary condition for the minimum, ~L = 0, the following equations can be derived: ~/T =

T T j~i+iAi+1.i

- 69~iiG i -

og/iiTi

A/~ = A~÷lA/r+l,i+ [3irXi - doT.iF i - og~iliHi - a~iG i r i = -~,rw i

T

- miai, i ,

i = N, N - 1. . . . . 1,

i = 1, 2 . . . . . N,

(35) (36) (37)

with the boundary conditions: Ax+1 = 0,

flx+l = 0,

AT i+l,i = I.

(38)

5. C O M P U T A T I O N A L A L G O R I T H M AND C O N S I D E R A T I O N S The computational algorithm can be stated as follows: Input data: N - number of joints Denavit-Hartenberg mi,

ni,

parameters ( a i , a i , 0 i , si ),

Ji,i,

0)0. 0 = 6~0.0 = o,

ao. 0 = [0 0 9.81] r , q i , eli, q i .

Step 1. Compute ogi.i, 69i.i, ai.i by Eq. (15), Eq. (16), Eq. (17). Step 2. Compute fli and A i by Eq. (35), Eq. (36), i = N, N - 1. . . . . 1. Step 3. Compute r i by Eq. (37), i = 1, 2 . . . . . N. By writing the vector equations as equations of coordinates further simplifications can be performed. The reduced computational scheme is presented in the Appendix by a P a s c a l program detail. The required additions and multiplications based on this program are:

210

1. J. R U D A S and A. T 6 T H

Additions: Multiplications:

General case

N=6

82N-39 94N-34

453 530

6. C O N C L U S I O N S The involvement of on-line calculation of the inverse dynamics naturally requires that the number of arithmetic operations should be reduced. One of the most efficient computations can be achieved by the recursive Euler-Newton formulation proposed by Luh, Walker and Paul [1]. This method requires 131 N - 48 additions and 1 5 0 N - 4 8 multiplications [2]. Comparing the computational scheme proposed in this paper and the algorithm of Luh, Walker and Paul it is concluded that the number of arithmetic operations is greatly reduced. A Pascal program was written for the computational scheme presented in this paper and for the purpose of evaluation of efficiency a P U M A 600 like robot manipulator was chosen as a working sample. The general algorithm was used, i.e. the further possible simplifications of the algorithm from the selected configuration of the robot have not been taken into account. The joint torques were computed on an A T 4 8 6 type computer and the computational time was within 2 ms.

7. R E F E R E N C E S

[1] J. Y. S. Luh-M. W. Walker-R. P. C. Paul: On-lineComputationalScheme for MechanicalManipulators.Trans os ASME. Journal of DynamicSystems,Measurementand Control,June 1980, Vol. 102,pp. 69-76. [2] J. M. Hollerbach:A RecursiveFormulationof LagrangianManipulatorDynamics.Proc. JACC/1980, San Francisco. [3] L. A. Pars: A Treatiseon AnaliticalDynamic.Heinemann,London, 1985. [4] R. C. Paul:Modelling,TrajectoryCalculationand Servoingof ComputerControlledArm. A. I. Memo 177, StanfordArtificalIntelligence Laboratory,StanfordUniversity,Sept. 1972. [5] A. K. Bejczy:Robot Ann Dynamicsand ControlTechnicalMemorandum;Jet PropulsionLaboratory,Feb. 1974. [6] M. Vukobratovic-D.Stokic-N. Kircanski:Non-Adaptiveand AdaptiveControlof ManipulationRobots. Springer-Verlag,1985.

8. A P P E N D I X

for i::l to 3 do j-:l t o N d o begin AngVel [i, j ] :=0.0; AngAccel [i, j ] ::0.0; Beta[i, j ] :=0.0; end {for} ; for i:=l to 3 do for j:=0 r o n d o begin LinAccel [i, j ] :--0.0; Lambda[i, j ] :=0.0; end {for}; for i: =I to N do JointPos [i] :=0.0; for i:=l to 18 do for j:=l toNdoW~[i,j]:=0.0;

Efficient recursive algorithm for inverse dynamics w i t h r o b do begin for i: =i to N do JointPos [i ] ::Theta [i ] ; LinAccel [ [3,0 ] :=9.81; for i :=i to N do begin F[l,i] :--Sin(Aiz~a [i] ); F[2,i] :=Cos (Al~ha [i] ) ; F[3,i] ::Sin (JointPos [i] ) ; F[4,i] :-~Sin (JointPos [i] ) ; F[5,i] :--S[i] * F[l,i] ; F[6,i] :--S[i] * F[2,i]; JJ[l,i] :--J Inert [i, i, i] +J Inert [3,3, i] ; JJ[2, i] :=J Inert [2,2, i] +J Inert [3,3, i] ; JJ[3, i] :=J Inert [i, i, i] +J Inert [2,2, i] ; JJ[4, i] :=O Inert [3,3, i] +J Inert [2,2, i] ; JJ[5, i] :=J Inert [I, i, i] +J Inert [3,3, i] ; JJ[6, i] :--J Inert [2,2, i] +J Inert [i, i, i] ; {for} ; AngVel [2,1 ] ::F [i, 1 ] * JointVel [1 ] ; AngVel [3,1] ::F[2,1] * JointVel [i] ; AngAccel [2,1 ] :=F [i, 1 ] * JointAccel [1 ] ; AngAccel [3, I] :=F[2, I] * JointAccel [i] ; for i:=2 r o n d o begin ii: =Pred(i) ; G[l,i]:=F[4,i] * AngVel[2,ii] - F[3,i] * AngVel[l,ii]; G[2,i] :=AngVel [3,ii] + JointVel [i] ; AngVel[l,i] :=F[4,i] * AngVel [l,ii] + F[3,i] * AngVel [2,ii] ; AngVel[2,i] :=F[2,i] * G[l,i] . F[l,i] * G[2,i] ; AngVel [2,i] :=F[2,i] * G[l,i] + F[l,i] * G[2,i] ; G[3,i]::F[4,i] *AngAccel[l,ii] + F[3,i] * AngAccel[2,ii]; G[4, i] :=G[I, i] * JointVel [i] ; G[5,i] :=F[4,i] * AngAccel[2,ii] + F[3,i] * AngAccel[l,ii] ; G[6, i] :=AngVel [i, i] * Jointvel [i] ; G[7, i] ::AngAccel [3, ii] + JointAccel [i] ; G[8,i] :=G[5,i] - G[6,i] ; AngAccel [l,i] :=G[3,i] + G[4,i] ; ~cel[2,i]:=F[2,i] *G[8,i] +F[l,i] *G[7,i];

211

1. J. R U D A S and A. TOTH

212 AngAccel[3,i]:=-F[l,i] end

* G[8,i]

+ F[2,i]

* G[7,i];

{ for }

WW[2,1] :=Sqr(AngVel[2,1]) ; WW[3,1] :=Sqr(AngVel[3,1]) ; WW[6,1] :=AngVel[2,1] * AngVel[3,1] W W [ 7 , 1 ] :=WW[2,1] - W W [ 3 , 1 ] WW[8,1] :=WW[3,1]; W W [ 9 , 1 ] :=-WW[2, i] WW[10,1] :=AngAccel[3,1] ; W W [ii, i] : = A n g A c c e l [2, i ] ; WW[12, i] : = A n g A c c e l [3, i] ; WW[13, i] :=WW[6,1] ; WW[14, i] :=-WW[6, i] ; W W [15, i] := A n g A c c e l [2,1 ]

;

for i:=2 to N do begin WW[I,i] :=Sqr(AngVel[l,i] WW[2,i] :=Sqr(AngVel[2,i] W W [ 3 , i ] : = S q r ( i n g V e l [3,i] WW[4,i] :=AngVel[l,i] * An Vel[2,i]; WW[5,i]:=AngVel[l,i] * An FVel[3,i]; W W [ 6 , i ] : = A n g V e l [ 2 , i ] * An [Vel[3,i]; W W [ 7 , i ] :=WW[2,i] WW[3,i W W [ 8 , i ] :=WW[3,i] WW[I, ]; WW[9, i] :=WW[I,i] WW[2,i WW[10,i] :=AngAccel[3,i] - b~W[4,i] ; WW[II,i] :=AngAccel[2,i] - W~W[5,i] ; W W [ 1 2 , i ] :=WW[4,i] + i n g h c c e i [3,i] ; WW[13,i] :=ingiccel[l,i] + WW[6,i]; WW[14,i] :=ingiccel[!,i] - WW[6,i]; WW[15,i] :=ingAccel[2,i] - ~,~{[5,i] ; WW[16,i]:=WW[3,i] + WW[2,i]; WW[17,i]:=WW[2,i] + WW[I,i]; WW[18,i]:=WW[I,i] + WW[3,i]; e n d { for} ; LinAccel[l,l] LinAccel[2,1] LinAccel[3,1]

for i::l begin

::-F[5,1] * W W [ 1 0 , 1 ] + F[6,I] * W W [ 1 5 , 1 ] AS[l] * W W [ 1 6 , i ] ; :=Ell,l] * L i n A c c e l [ 3 , O ] -F[6,1] * WW[14,1] + AS[l] * W W [ 1 2 , 1 ] - F[5,1] * W W [ 1 8 , 1 ] ; :=F[2,1] * L i n A c c e l [ 3 , 0 ] - AS[I] * W W [ I I , I ] + F[5,1] * W W [ 1 3 , 1 ] F[6,1] * W W [ 1 7 , 1 ] ;

to N do

)

(I) M ct

c4

co

+

c~

m

~

tQ

2:

co~Z

c?

+

H

~

oa

+

~

~

.° II

~

i

2

+

N

3

cr

c4 I

Go ~

ct

-p£

c-4

0~

m

~

ca

.

® b~

Cl

I

q

>

¢.,,4

-°

1-1

.°

+

fl)

°° II

~u

[~

~o

0 II

~

X

r ~

~"

m

+

~"

m

I

k~"

fl)

I

~ ' ~

~

""

P"

~

~

m

""

I-"

~

X

I

I-'"

m

~

+

~"

~

+

~

I'-~

e

""

P"

~"

~

fl)

II I-'"

~

I

0

o

l- ~

~ ~ ~,:~.o

+

m ~m

I

II

I

~

+

II

~ H

o

I

k,.O

~

~

I

I-,~a.

~-'

~-

+

.

.

.

I

~'0

~

~

.

I

~~

~

~

~

.

~

.

~~

,~

+

~r]

I- ~

+

I- ~

I-'-

°°

....

~l-a-

0

~-t

I-,I-,-

0

~-

~..

+

~

I-'-

°°

H

II

I-i. I-'°°

0

~o

u

H

~0

k,-

~°

cn~

~o

~.

0

bD

Go

+

2~ ~

o ~o

[xg

+

r.q

~

~

@

~ ~-~

+

(41

t~d

8

~

bo

I

:~

+

rq c4 c4 cd ~

°°

+

~.

~-

~

p-

hi

~.

U3

-

I

+

@

cd

~

~-

~_~

+

"

@

o

~

t~ -

p.

~-

"~

~-

o, II

%

t:J"

U'

~

~-

~

p.

+

@

@

+

i

i

~

+

+

Q

~

~

('1

I

C]

-

~

+

~,.

oo

b,~

I

p-

pI-'-

II

~-

bo

I

p-

b4

i~I-'-

II

~-

v

"°

IZl

0

0

l-'-

0

+

~

+

ct

C~ ~ C4 COl

k4 ~

C4 I

+

*

~ O~

i- ~

i~

II

.°

,-.) ©-

>-

>