Relaxed Path Tracking of Mechanical Manipulators by Parameter Estimation*

Copyright © IFAC Information Control Problems in Ma nufac turing T echnology . Maryl a nd . USA 1982

RELAXED PATH TRACKING OF MECHANICAL MANIPULATORS BY PARAMETER ESTIMATION* J. Y. S. Luh School of Electrical Engineering, Purdue University, West Lafayett e, IN 47907, USA

Abstr ac t. T o elimin a t e the burden of computin g the dyna mics whil e controlling th e mec ha ni ca l m a nipul a to r , its dyna mic behavior is modcled by a difference equ a tion . Qu adr a ti c te rms representing centrifugal fo rces are in clud ed in th e equ a tion to redu ce modelin g erro rs. Th e p ara m eters of th e model a re estima t ed based on th e minimum va ri a nce crit.erio n. Th e control torque for each joint is computed durin g each sampling period to minimi ze th e deviatio n from points on th e desired joint p at h . Desired joint pa t.h could be o btain ed fr om th e Cartesia n pa th using splin e fun ctio n a pproxim a tion s.

1. INTRODUCTION

DesForges [lO] simplifi ed t he prob lem by introdu cin g th e model-referenced ad a ptiv e control. Th e perform a nc(> a t t he ro bot's hi gh speed is, howeve r , not ve rifi (>d . K oivo a nd Gu o [11] a ppli ed Borison 's self- t unin g regul ato r [12] to th e ro bot control usin g a mod(>1 of lin ear difference The equat io n wit.h whi te Gauss ia n noise. pa ra m(> ters were itrrat iv ely o bta in ed fr om t he minimum va ri a nce estim ato r . Th e control v ariable (input to rqu r ) is th en co mputed fo r th e minimum an rage devi a tio n of th e joint pat hs fr om t he nomin a l paths. A lth o ugh t he com putat.io n of (> it her L ag ra ngia n o r Ne wt o n- E ul er equ ations is co mpl r t el)' avoid ed , th e itera tiv e compu t in g tim e is still too lo ng fo r a n o n-lin e o peratio n even based o n a t wo-stage observation . Ma th r m a tica lly th e conve rgence of t he ite ra tion is not guar a nteed . A lso, th e us(> of th e lin ear mod ('I igno res th e non lin ear cha rac t(> ristics of th e ro bot with co uplin g a mo ng its join ts, a nd t hus introdu ces und es ir a bl e erro rs. This papr r prl'sent s a n improved mod el by addin g qu adr atic t erms in th e diffl' rl'nce equ a tion to in cl ude C'o riolis a nd cent rifu ga l fo rce terms. A ll t he pa ra m (> ters a re es tim ated fr om th e ite rativ e minimum va ri a nce estim ato r [1 3]. T o speed up th e comput a t.io n a nd to ensur e th e conv ergence of t he ite ra tion , t he stoc hast ic ap proxim at.io n fo rmul a tio n propos(>d by Ast. rom [1 4] is adopt ed . Th e input to rqu rs a re t hen compu t.ed for t he minimum average devi a tio n of th e pa th trav ell ed. T o m a ke it poss ibl e fo r th e o n-lin e ope ra tio n , th e desired Ca rtes ia n pa th of t he end- effeclo r is pre-tra nsform ed int o join t paths by mea ns of sp lin e fun ctio n a pprox im a tio n [1 5].

Conv entio nally a n indu st rial robot is a comput er- controlled m a nipulator , which is b asically used as a posit ion ing device. Th e job assignm ent is spec ifi ed in t erms of th e path travell ed by t he ha nd (or end- effe ctor) . Th e comnw rcia lly avail a bl e robots perform well in a reas of sy nchro niz at ion t as ks such as a rc weldin g, paint spraying, et c. Th e existing prac tice of m a nipul a tin g t he ro bots is ac hieved by th e po in t-to- point progra mming, o r by a rigid " t eac hing by doin g" p rocedure. Control of the m a nipul a t or is done using conv ention al c1osedloop sc hr m es a t th e joint level. T o improve th e perform a nce such as smoothin g th e robot motion , redu cin g th e d ev iatio n of ac tu ally tr avell ed pa t.h fr om th e pl a nn ed pa th , minimizin g th e exec utio n tim e, etc., a mo re sophisticated co nt ro l system is required . In t he a vaila bl e lit erat ur e, a numb er of sc hem es were pres(' nt.ed whi ch includ e, for example, resolv edra t e co ntrol [1 ,2]' inv erse-probl em techniqu e [3], comput ed-t o rque t ec hniqu e [4,5]' resolvedacc('l(> ra tion control [6] . joint-torqu e cont rol [7], etc. Pra ct ica ll y in a ll th ese m ethods, it is required to co mput.e t he dynamical beh avior of th e robo t. usin g L ag ra ngia n or Newton-Euler equat io ns. Th e ro bo t , however , is a serial nwc ha nism which is hi ghly no n lin ear a nd co upl (>d a mong it.s joints. In additio n , th e valu es of th e coeffi cients in th e dyna mic equ a tion ch ange wh en th e robot moves. Thus t he on-lin e computat ion of t.h e d es ired joint torqu e becomes a bottl e-n ec k of t he control sc heme [8] . Consequentl y, it is desira bl e to search for new schemes t h a t r equire mu ch less computa tion . Yo un g [9] proposed a sc hem e using va riable stru ct ure th eo ry for th e m a nipul ato r. T o implem ent th e sc heme, th e motio n t rajectory must lie on th e swit chin g surface which , itself is difficult to implrm ent as o ne has ex peri enced in th e minimum-t.im e cont.rol th eory . Dubowsky and

11. EQUATIO N OF MOTIO N FOR MECI-IANICAL l\l<\I\'IPULATOR Th e L agra ngia n fo rmulation of d yna mic equ a tio n for a m ec ha nical m ani pul a to r wi t h n joints is giv (>1l as [3,5]: n

n

Ti

' Suppo rt ed by National Sc ience F ound ation Gr a nt

=

I: j= l

DiA

+ I: j=l

n

Dijj (Clj )2

n

+ I: I: j=l k= l

j>'k

M EA-8 JJ 9884.

103

Dijkq/lk

J.Y.S. Luh

104

(I) where

Tj = input generalized force for joint i, qj = generalized coordinates or displacement for joint j, Dj,Djj,Djjk = parameters depending on configurations of the manipulator.

The first term in (I) represents the contribution from inertias of all the joints. The second and t.hird terms are contributions from, respectively, from the centrifugal and Coriolis forces. They are nonlinear in qj. In addition, all three terms show the interactIOns between joints. The last term in (I) is resulted from the gravitation al acceleration. It has been shown [16] that the number of inertia force terms is of order 0(n 3 ) while the centrifugal and Coriolis force terms are of order 0(n4). To compute the values of required input torques for all joint.s, one may perform k adds and h multiplies [8] where 3 1 ry 1 1 k = 2.5n 4 + 66- n + 129- n- + 42-n - 96 3 1 2.5 3 1 and h = 32- n 4 + 86- n 3 + 171- n 2

2

]

]2

4

+ .53- n - ]28. To ease the computational 3 complexity , approximations of dynamical equations by means of omitting various torque/force terms, such as centrifugal and Coriolis forces, have been investigated. But errors in the computed joint torques resulting from omitting these terms cannot be corrected with feedback [8]. Although it is possible to have a fast computational scheme by using the Newton-Euler formulation []6], the sampling frequency, however, for a Stanford manipulator with six joints is still limited to approximately 60 Hz on a PDP 11/4.5 digital computer with floating point assembly language processing. Because the computational burden of manipulator dynamics is ex cessive, it is desirable to search for different methods of approach . One possible method is to estimate the parameters iteratively. If the estimation converges fast enough, then the objective of controlling the manipulator can be accomplished without the computation of its dynamics. To proceed this approach, a model for the manipulator is described in the next section . Ill.

DISCRETE-TIME MODEL OF MANIPULATOR

[atqj(k-O') + btTj(k-O')]

N

N

E E ~=I

-/=1

ot

k- I

E

JJk-l) =

E[
(4)

0=0

wher e E denotes the expected value. since the manipulator is modeled by (2) which does not include interactions among its joints, all the estimation of parameters and the computation of controls are for one specific joint.. Thus the subscript i is dropped in the remainder of this section to simplify the notatiOIl .

TV. ESTIl\1ATION OF PARAMETERS For the estimation problem outlined in Section lIT, th ere are more than one possible way to approach the problem.

A. K a/man FilteT Let Q (k-l) be a 2N+ 1+ N(N+ 1)/2 dimensional vector defined by Q' (k-]) = [1 q(k-I) ... q(k-N)T(k-l) .. . r(k-N) q(k-lj2 q(k-l)q(k-2) ... q(k-l)q(k-N) q(k-2f q(k-2)q(k-3) ... q(k-N)2]

(.5)

(·r

where = transpose of (.). Let £ be a parameter vector with the same dimension defined by £' (k) = [aO a l . . . aN b l . . . b N c ll c l2 . . . c lN c 22 c23

•.•

C NN ]

(6)

(7)

This is a noiseless system equation for the parameters. By (.5) and (6), the combination of (2) and (3) yields q(k) = Q' (k-l)£ + E(k)

(8)

which is linear measurement equation in Q(k) with additive, Gaussianly distributed white noise. The system consisting of (7) and (8) has a discrete Kalman filter [13, p. 107]: Updated parameter estimate: 12(k+ ) = 12(L) + !S(k)[q(k)-Q' (k-l)12(L)]

",=1

+

Since qj(k) is unknown at time k, so as
£(k) = Q(k-l) = £

N

E

(3)

Since the parameters are assumed to be constants, then

Let qj(k) and qjm(k) be the actual displacement of joint i, i=I,2, .. .n, and its model, respectively, at time k. At that time, qj(k-O') for 0'=1,2, ... ,N are assumed known through measurement while the joint torques Tj(k-O'-I) have been determined. The main purpose of this paper is to determine Tj(k-l). To include the nonlinear effect caused by the centrifugal torque/force, the model is chosen to be qjm(k) = ajo +

yet to be determined. Although it is nonlinear, this mod el still does not include the interactions between joints . Even so, the estimation of the parameters is quite involved. Before Tj(k-I) is determined, qj(k) is not known and cannot be measured. If qjm(k) is used as its model, then the modeling error is

(9)

Parameter estimate extrapolation:

cf'1

qj(k-j3h(k-,)

(2)

12(L) = 12((k-1)+) Kalman gain vector:

(10)

105

Relaxed Path Tracking of Mechanical Manipulators

!ilk)

= E(L)Q(k-l)/[Q' (k-l)E(L)Q(k-l)+ a 2 ] (11)

Error covariance matrix extrapolation:

= E((k-l)+)

ElL)

Q(k) (12)

Error covariance matrix update: with

1 = identity

matrix, and initial conditions (14)

E[{Q(O)-Q(O)}{Q(O)-pJO)}']

= E(O)

(15)

An alternative representation of the noiseless system equation for the parameters and its linear measurement equation is as follows. Let

e' (k-l) = [l

q(k-l) ... q(k-N) 1{k-l) ... 1{k-N)] (16)

be a (2n+ 1) dimensional vector, and a

-

aO a

-

I

2

aD

<1>=

2 I -b 2

l:~

I

bI 2

aD

~

2

e"

CID

0

o

0

o

o

o

o

o

be a (2n+ I) by (2n+ 1) constant parameter matrix. Combining (2), (3), (16) and (17) yields

e' (k-l) 2

=

e(k-l) + E(k)

(18)

This is a scalar equation. Multiply it bye' (k-l) to yield q(k)e' (k-J)

= e' (k-1)2 e(k-l)e' (k-l)

+ dk)e' (k-J)

(19)

Let .!l(k-l)

= e(k-l)e' (k-l)

(20)

and .!l + (k-l) be its pseudo Inverse. Then (19) becomes q(k)e' (k-J).!l + (k-l)

= e' (k-l)2

+ dk)e' (k-J).!l+ (k-l)

(21)

which is a linear measurement equation in parameter matrix 2. The noiseless system equation for the parameter is 2(k)

(24)

and the covariance matrix E(k)

= [-[(k-l)Q' (k-l)]E(k-l)

(25)

C. Stochastic Approximation Although the Kalman filter and the leastsquares estimation are applicable to estimate the parameters in this problem, the computation is quite involved. To reduce the computational burden , one may adopt the technique of stochastic approximation by replacing equation (17) by [14 , p. 148]

=

~

Q(k-l)

(26)

V. PATH TRACKING CONTROLLER

(17)

q(k)

+ Q' (k-l)E(k-l)Q(k-l)]

where A is a positive definite matrix, and could be 1 fOr simplicity. It has been shown that the choice of (19) for r(k-l) results in the convergence of the estimation of Q(k) [17]. Although the algorithm of stochastic approximation always reduced computational effort and converges , the resulting estimates, however , have larger variances than those of th e least-squares estimate.

0

o

= E(k-l)Q(k-l)/[a2 +

[(k-l)

bD 2

o

COl

where the vector

(13)

= 12(0) = E[Q]

E[Q(O)]

= Q(k-l)+ [(k-l)[q(k)-Q' (k-l)Q(k-l)] (23)

[(k-l)

= [-!i(k)Q' (k-l)]E(k+)

E(k+)

It has been shown [14, p. 149] that, by an algebraic manipulation, the recursive equations of the least-squ ares estimate can be obtained directly from the Kalman filter as:

= 2(k-l) = 2

(22)

Let qid(t) be the desired displacement of joint i for all t. Ideally one wishes that the deviation of the actual displacement qi(t) of joint i from CJid(t) is zero for all t. This is a difficult problem to solve. Since, in this paper, the dynamic model for the manipulator is of discrete-time type with a gaussianly distribut ed modeling error, and the parameters of the model are estimated based on the minimum modeling error, the path tracking control input is determined in the sense of minimum av erage deviation. Thus, the path constraint is not strict. At eath time instant k, let qid(k) be the desired di,pbcement of joint i. Whenever the actual di splacement at time k-l, qi(k-l) is fixed, qi(k) is determined by the torque of the joint i actuator Ti(k-l). Again omit subscript i to simplify the notation . Thus 1{k-l) can be obtained by minimizing J(1{k-l))

= E{[CJd(k)-q(k)]2/q(k-l)}

(27)

By (8) and noting that
= [qd(k)-Q' (k-l)Qj2+ a 2

(28)

Equations (21) and (22) represent the same system described by (7) and (8), and the resulting Kalman filters are equivalent.

which has a derivative

B . Least-squares Estimation

Since bit 7- 0, then the zero derivative leads to

dJ(1{k-l))/d1{k-l) ~ 2b'[qd(k)-Q' (k-l)Q]

(29)

J.Y.S. Luh

106

VII. CONCLUDING REMARKS

(30) or r(k-l)

The block diagram for the overall process is shown as in Figure 1. It shows the information flow for one joint during one sampling period. It is seen that both the estimation of model parameters and the computation of the control must be done on-line and completed during one sampling period . The speeds of estimation and computation are crucial since they determine the upper bound of the sampling frequ ency; and the higher the frequency, the more accuracy the process will have. In the above discussion, estimations by means of Kalman filter, least-squares estimate and stochastic approximation have been presented . Among them there is a trade-off between speed and accuracy . The method of stochastic approximation is preferrable since it requires the least amount of computing time and its iterations always converge. But the resulting estimate has the largest variance. The model of the mechanical manipulator includes a quadratic term representing the centrifugal forc es. Its purpose is to reduce the modeling error at the expense of more computation. In short , the goal is to track the Cartesian path of the manipulator with accuracy. A real time experimentation, which is being conducted, is necessary to determine the feasibility of the presented scheme. The computer simulation often leads to an inaccurat.e conclusion because the nonlinearities such as Columb frict.ion, sticktion, etc ., are very difficult to simulate and often ignored. For the commercially available manipulators, these nonlin earities turn out to have significant. effect.

= !qd(k)-a -a 1q(k-l) O

N

- ~ [aOq(k-a l+ bar{k-a l]

-:¥. .~. "'q(k-~)q(h) lib'

(31)

In (31), t.he values of parameters aa, b a and cP, are the estimates obtained in Section IV. The computed control is for the immediately following sampling period to ensure the stability of the system.

,,1. DESIRED JOINT PATH In reality, the desired path traveled by the hand or endeffector of the manipulator is specified in Cartesian coordinates. Intuitively one may determine the equivalent joint displacement qjd(k) by means of inverse Jacobian tran sformation. Wh enever the sampling frequen cy is changed, the point of qj between the time instants k and k+ 1 may be obtained by int erpolation. To st.ore the values of qjd(k) for all k in a comput.er requires a large memory. This problem can be avoided by pretransforming the three-dimensional Cartesian path int.o n.-dimension~l jO.int path in ~erms. of spline-functlOn apprOXImatIOns .. The dISCUSSIOn on this subject was presented III Reference 15, and will not be repeated here.

€. (k) I

L

Estimate Parameters of the 110de I

Construct the Model

r

q. (k) Im

-

E

')

't'.(k-I)

I

~IAN

l De~y JI t"i (k)

De sired Cartesian Path

I PULATOR

+ ~

q. (k) I

,

I

t

I

Estimate the Control Sp I i ne Function Approximation

De 1dy

JI

De lay

I

Para meters of the Mode

-

qid(k)

Fig. 1. Overall process for joint i during one sampling period.

Relaxed Path Tracking of Mechanical Manipulators

H EFEH E;\iCCS

[] ]

D. E. \\'hitllC'v, "Resolved ~fotion Rate COlltrol of ~[anipulators and Human Pros! hpses," IEEE Tran s:J d ions on Man~I:l('hille Systems, Vol. ~1~[S-1O , No. 2, JUIlC' Hl[)fl , pp. ·~7-.'j3.

[2]

D. E. Whitney , " The Mathematics of Coordinated Control of Prosth etic Arms and \!anipulators," Journal of Dynamic SYSt C'IllS, \h'a~llrell1('nt and Cont.rol Tran5;;ct ion s ' of AS\1E, Dece mber 1972, pp. 30,'3-300. R. C. Palll, Modeling, Trajectory Calcula-

[3]

tion and Sen'oing of a Computer Controlled Arm, A. I. I\1C'mo 177, Stanford Art.ificial

[I]

Intr'lligencc L:Jboratory , Stanford University, Septeml)('r 1972. n. H. f\larkiewicz , Analysis of the Computed Torque Dril'e Afethod and Comparison u'ith COnl'entional Position Servo Jor a Computer-Controlled Manipulator,

[S] [6]

[7]

Tec 1.lIm a I Mrmorandum 33-601, Jet PropulsIon L:Jboratory, March 1073. A. 1\. £3ejczy, Robot Arm Dynamics and Control, Tcchnical l\1emorandum 33-669 Jct Propulsion Laboratory, February 1974.' J. Y. S. Luh, M. W. \ValkC'r ancl R. P. C. l'nul. " Rrsolvcd-Arr elcration Control of l\!C'dl:Jnical Maniplllators," IEEE Tran sactions Oil l\lltomat ic Control, Vol. 25, No. :3 , ./UIlC' 1980, pp.IGS-174. c:. 11. \Vu :llld H. P. Pnlll, " Manipulator ('ornpli:lnce BnsC'd on Joint Torqu c Cont rol." ProcC'C' c1 ings of th e 19t.h IEEE Confrrrll('e on Decision and Control, Albuqu('rqlll' , nrw l\1exico , DC'c C'mbC'r 10-12, ]fl80.

[8]

IIollrrhach , .I. M., " A RC'rursive Lacrrangi:lll Formulation ~f l\lanipubtor DynaOlllics and a ('ooIwratiH St.udy of Dynnmics ForlllU Lit ion C'omplf'x ity ," IEEE Transaetions on Systl'lllS, l\lan and Cybrrnrtics, Vol. 10, :\0. 11, NOY(,IllI)('r ]080, pp. 730-736.

[0]

107

1\. 1\. D. 'Young, "Contro]]er DesIgn for a ~1anipulator Using Theory of Variable St ruelu re Systems," IEEE Transactions on Systems, i\lan and Cybernetics, Vol. 8, No. 2, Fdm13ry 1978, pp. 10]-109. [10] S. Dubowsky and D. T . DesForges, " The Applica t ion of Model-R efE'f enced Ada ptiv e Control to. Robotic Manipulators ," Journal of Dyn:Jmlc Syst ems, i\[easurement , and Con t 1'01 , Tran s. of ASME, Sept em ber ] 979 pp. 191-200. ' [I 1] A. J. Koivo and T. H. Guo, "Control of Robotic ~Ianipubtor with Adaptive Contro]]('['," Proc. 20th IEEE Conference on Deci~ion and Cont.rol, San Diego, California, December 16-]S, 1081, pp. 271276. [12] U. Borison, "Se lf-Tuning Regulators for a Class of i\lultivariable Systems," Automatica, \'01. 15, 1970 , pp. 200-21!i . [J ,'3] A . Gelb, .1. F. J\:asp er, et. aI., Applied Optimal Estimation, i\lIT Press, ]074. [t -I] 1\ . .I. Astrom and P. EvkhofT "System IdC'ntificatioll--A Survey,;' A~ltomatica, Vol. 7, lOil, pp. 123-162. [Vi] .1. Y. S. Luh and C. S. Lin , "Approximate Joint Paths for Control of Mechanical i\bnipulators," Proeeedings of IEEE ConferC'nce on Pat.tC'rn \lC'cognit.ion and Image ProcrsslIlg, PHIP 82, Jllne 13-17, 1082, Las Vrgas. [16] J. Y. S. Luh, M. \-V. Walker and n. P. C. Palll , "On-lillc Computat.ionnl Scheme for ~[rchallical Manipula.tors," ASME Transactions: Journal of Dyn:JlIlic Systems, i\[C';lsllrrmrnt and Control, Vol. 102 , No. 2, Jun e H1SO , pp. 60-76. [17] A. E. Albert and L. A. Gnrdner, Stochastic Approximation and Nonlinear Regression i\IIT Prrs5, 1967. '