Robotica (1990) volume 8, pp 231-243

A closed-loop inverse kinematic scheme for on-line joint-basedrobot control*Bruno SicilianoDipartimento di Informatica e Sistemistica, Universita degli Study "Federico II" di Napoli, Via Claudio 21, 80125Napoli (Italy)

(Received in final form: November 14, 1989)

SUMMARYA computationally fast inverse kinematic scheme isderived which solves robot's end-effector (EE) trajec-tories in terms of joint trajectories. The inversekinematic problem (IKP) is cast as a control problem fora simple dynamic system. The resulting closed-loopalgorithms are shown to guarantee satisfactory trackingperformance. Differently from previous first-orderschemes which only solve for joint positions andvelocities, we propose here new second order trackingschemes which allow the on-line generation of jointposition + velocity + acceleration (PVA) reference tra-jectories for any computed torque-like controller insensor-based robot applications. The algorithms doexplicitly solve the IKP for both EE position andorientation. Simulation results for a six-degree-of-freedom PUMA-like geometry demonstrate theeffectiveness of the scheme, even near singularities.

KEYWORDS: Kinematic scheme; Closed-loop algo-rithms; On-line control; Robots.

I. INTRODUCTIONRobot control actions are naturally executed in the jointspace, whilst robot motion trajectories are betterspecified in the task space. Usually, the location of therobot's end-effector (EE) is commanded to vary as afunction of time along a given path. Three Cartesiancoordinates describe the EE position and three angularcoordinates (e.g. Euler angles) characterise the EEorientation in a sir-dimensional task-space.1 Let then thedirect kinematic equation of a manipulator with arbitrarystructure be known; the problem we want to solve in thispaper can be formulated as follows:

Assigned an EE trajectory (position + orientation),find a (PVA) joint position + velocity + accelerationjoint trajectory which is a solution to the directkinematic equation, i.e. it reproduces the desiredmotion at the EE.Most approaches proposed in the literature are aimed

at solving the inverse kinematic problem (IKP) for agiven constant EE location. It has been well-known for along time that analytical inverse kinematic solutions exist

* This paper is based on work supported by the Ministero dell'Universitd e delta Ricerca Scientifica e Technologica PubblicaIstruzione under MPI 40% and 60% funds.

only for special manipulator geometries,2'3 such as theso-called wrist-partitioned type of manipulator.

For all those structures which are not solvable inclosed-form, a number of numerical techniques havebeen proposed most of which are based on thecomputation of the manipulator's Jacobian. An iterativemethod based on a nonlinear optimisation algorithmwhich uses a modified Newton-Raphson method hasbeen proposed by Goldenberg et al.* Angeles5 derived anumerical method from the formulation of invariants inthe rotational part of the so-called closure equations.Another technique by LenarCiC6 solves the IKP by usingthe conjugate gradient method. A conceptually differentapproach by Tsai and Morgan7 is based on the use ofcontinuation methods. The resulting computationallylengthy technique has recently been simplified byManseur and Doty8 when applied to the so-definedorthogonal manipulators.

On the other hand, solving the IKP along a trajectoryis of crucial importance in order to provide the robotcontrolled in the joint space with the reference jointtrajectories to be tracked. Here, computation timebecomes a primary concern for those on-line sensor-driven tasks when computing the inverse kinematics atthe same rate as the joint servo rate becomes a must.3

Since the pioneering resolved motion rate technique,9

computationally efficient velocity and accelerationinverse kinematic solutions have been derived forwrist-partitioned geometries by Featherstone10 andHollerbach and Sahar,11 respectively. All these tech-niques are inherently open-loop computational methodswhich suffer from problems with long-term drift andinitial EE location errors.

A rather different approach to the solution of the IKPis obtained by constructing a simple closed-loop dynamicsystem, whose input is the desired EE trajectory andwhose outputs are the joint trajectories which give thedesired motion at the EE. The original idea wasindependently proposed by Balestrino et al.i2 andWolovich and Elliott13 for solving only the positioncomponent of the EE trajectory. A closed-loop schemebased on the computation of the Jacobian transpose wasdevised which generates the joint displacements andvelocities while guaranteeing a null positional error and anorm-bounded tracking error.

The non-trivial extension to account for the orienta-tion component of the EE trajectory has been described

232 Closed-loop control

in Siciliano14 and Balestrino et al.,15 and the investigationof a special non-solvable structure has been presented bySciavicco and Siciliano.16 The application to the case ofrobots with redundancy has been discussed by Sciaviccoand Siciliano171920 and Sciavicco et al.m The issue ofkinematic singularity robustness of the scheme hasrecently been addressed by Chiacchio and Siciliano.21 Ifthe EE location is assumed to be constant, closed-loopschemes based on the same concept of Jacobiantranspose have been designed in Asada and Slotine22 forgeneral positional tasks, and in Das et al.23 for redundantmanipulators.

The convergence of the above schemes is ensured byLyapunov stability theory which leads to establishingestimates of the region of attractiveness of the solutions.Also, the algorithms are remarkably based on the solecomputation of direct kinematic functions, and thereforethey avoid the typical numerical instabilities associatedwith any matrix inversion-based technique. Alterna-tively, closed-loop convergent schemes solving for EEposition + orientation based on the computation of theJacobian inverse has been suggested by Balestrino et al.15

and Tsai and Orin;24 they can be considered as thenatural closed-loop extension of the resolved motion ratetechnique.9 Similar is also the scheme proposed byWolovich and Flueckiger25 which ensures an exponen-tially decaying error only, and not perfect tracking as inthe above two schemes.15'24

All the above schemes can be termed first-orderschemes, since they solve for joint displacements andvelocities. For joint space control purposes, however, itwould be nice to generate joint accelerations as well,designing then a second-order inverse kinematic scheme.If the EE location is constant, a second-order positionalscheme based on the Jacobian transpose has beenproposed by Slotine and Yoerger26 for the general caseof redundant manipulators. Conversely, a so-called jointspace command generator has been derived by Vaccaroand Hill27 which is based on the Jacobian inverse, but itonly solves for EE position; the same idea has also beendeveloped for robot Cartesian control.28

For the most general case of an EE trajectory, asecond-order scheme logically derived from resolved-acceleration control29 has lately been obtained bySciavicco and Siciliano30 which requires the computationof the Jacobian inverse. More recently, Siciliano31 andNovakovi632 have independently established a newsecond-order scheme based on the use of the Jacobiantranspose which makes use of the sliding mode theory.33

Their scheme can be utilised, however, only for solvingEE position.

In this paper we propose a new second-order trackingscheme which solves EE trajectories (position +orientation) in terms of joint PVA trajectories by meansof a fast convergent algorithm which only requires thecomputation of the direct kinematic function and of theJacobian of the manipulator. The definition of anorientation error which is consistent with the adoption ofEE angular velocities as task space variables directlyfollows from the first-order scheme recently proposed by

Chiacchio and Siciliano.34

A PUMA-like manipulator is chosen to develop anumerical example. Being this geometry wrist-partitioned, the EE position IKP is solved separatelyfrom the EE orientation IKP. The limited number ofcomputations required allows for a 1 kHz solution rateon condition that a high-speed floating-point arithmeticprocessor is adopted. The solutions obtained with theJacobian transpose technique are then compared withthose obtained with the more computationally demand-ing Jacobian inverse technique in order to illustrate thepower of the proposed algorithm. The tracking errorswill be shown to be less than 0.45 mm for position withan average EE velocity of 1 m/s, and less than 0.35° fororientation with an average EE velocity of 180°/s. Also,the convergence of the Jacobian transpose algorithm istested in the neighbourhood of a double singularconfiguration.

II. THE INVERSE KINEMATIC PROBLEM (IKP)It is well-known3 that the manipulator's EE location canbe described as a function of time t by a position vectorp(t) and an orientation matrix R(t) = (n(f) s(f) a(f)); p isthe vector pointing from a reference base frame to an EEframe, and n, s, and a are the normal, slide, andapproach unit vectors of the EE frame expressed in thebase frame coordinates. The direct kinematic equation ofthe manipulator defines the transformation from then-dimensional vector of joint displacements q to thevectors of EE location (p, R) as

= p(q(0) R(t) = R(q(t)). (1)

Given a desired EE location (prf) Rd), the IKP can bestated as that to find a solution qd to (1).

In manipulator kinematics it is of interest also themapping from the vector of joint velocities q to thevector of EE velocities v, i.e.

(2)

through the (6 x n) Jacobian matrix. In (2), p is thevector of EE linear velocities obtained as the timederivative of p in (1), and co is the vector of EE angularvelocities. As a consequence, the matrix /(q) can bethought as partitioned into two (3xn) matrices, i.e.

(3)

Notice that an appropriate orientation counterpart for pwhich represents JOJ dt cannot be defined.3

Assigned a desired EE velocity vector \d, equation (2)can be solved for the joint velocity vector i\d according tothe so-called resolved rate technique9 as

qd(t)=rl(qd)vd(t) (4)

which, once q</(0) is known, can be integrated over timeto provide qd(t). In (4) it is assumed that an inverse to Jdoes exist for all qrf's; a pseudo-inverse must be used ifthe Jacobian degenerates or if the manipulator isredundant.35

Closed-loop control 233

Differentiating both sides of (2) with respect to timeyields the mapping from the vector of joint accelerationsq to the vector of EE accelerations v, i.e.

•('W(q)q(0+'(q)q(0 (5)

where j = dJ/dt. Solving (5) for the joint accelerationsqd, similarly to (4), according to the so-called resolvedacceleration technique29 gives

qd(t) = (6)

which, once qd(0) and qrf(0) are known, can beintegrated over time to provide qd(t) and qd(t).

Having obtained the desired PVA joint trajectories,one can design a computed torque-like controller.22 Ifthe robot operates in a sensor-based fashion, however, itis crucial to generate the joint trajectories on-line at aminimum of several hundred Hertz, ten to twenty timesthe robot structural resonant frequency,3 such thatsatisfactory trajectory tracking is obtained.

HI. CLOSED-LOOP FORMULATION OF THEIKPA conceptually different approach to the solution of theIKP which is independent of the particular robotgeometry is illustrated in the following. The idea is toreformulate the IKP as a tracking problem for a simpleclosed-loop dynamic system, whose input is the desiredEE trajectory and whose outputs are the jointtrajectories. This approach opposes the computationalmethod based on (6) which is an open-loop style method,thus overcoming drawbacks like long-term drift andinitial EE location errors.

Several first-order schemes based on this idea whichsolve for joint displacements and velocities have beenproposed in the literature. They can be distinguished intothose based on the computation of the Jacobian

transpose12-23,34 and those based on the computation of

the Jacobian inverse.24%25'27'28 Only some of the aboveschemes, however, do explicitly account for the EEorientation1*"16242534 which is not a trivial problem, asdiscussed in the previous section. The schemes based onthe inverse give better results than those based on thetranspose, but they are more computationally demandingand may fail in the neighbourhood of kinematicsingularities. It is to be mentioned, also, that theschemes based on the Jacobian transpose can be suitablyextended to redundant manipulators without noticeablecomputational efforts,17"20'23 whereas a pseudoinverse isusually required for the others.24'25'27'28 The reader isreferred to the wide list of references provided at the endof the paper for more details concerning first-orderschemes.

The goal of this section is to derive a new closed-loopsecond-order tracking scheme which solves the IKP forEE position + orientation trajectories. Partial solutionsbased either on the Jacobian transpose26-31 >32 or on theJacobian inverse27'28'30 have been proposed, but none ofthem accounts for the EE orientation.

Let then e(r) denote a six-dimensional error vectorbetween the desired EE location (pd, Rd) and the actual

EE location (r, R) which can be computed from thecurrent joint configuration vector q via (1). Notice that qis not to be interpreted as sensed at the robot jointactuators, but it is just algorithmically computed. Theerror e is though of as partitioned into

(7)

where ep denotes an EE position error and eo an EEorientation error.

The definition of ep is straightforward, i.e.

(8)

whereas the definition of eo is the same as originallyproposed by Luh et al.29, i.e.

eo(0 = x nd(t) + s(t) x sd(t) + a(f) x (9)

where (n, s, a) and (nd, sd, ad) denote the actual and thedesired unit vector triples of the EE frame, respectively.

It should be remarked that the desired EE orientationis usually described by a minimal number of coordinates,typically three Euler angles (ip, 6, <p). The desired unitvector triple of the EE frame (nd, sd, arf) can then becomputed through the rotation matrix associated withthe Euler angles representation.1

Additionally, let e(f) denote the error vector betweenthe desired EE velocity vector vd and the actual EEvelocity vector v which can be computed from thecurrent joint velocity vector q via (2). The partition of efollows accordingly to (7), i.e.

e(/) = (epJA . (10)It is easy to see that

*,(') = P-(O-P(O- (11)As regards the orientation error, it can be shown thatf

eo(0 = cod(t) - co(t). (12)

The second-order closed-loop scheme of Figure 1 can bedevised. If the control law q(t) is chosen so that thesystem is guaranteed to be stable, i.e. e(() asymptoticallytends to zero, it can be concluded that the systemperforms a second-order kinematic inversion; namely,given ($d(t), Rd(t)), vd(t), and \d(t) if needed, thescheme generates q(t), q(f), q(t) which are "infinitelyclose" to qd(t), qd(t), qd(t), respectively.

III. A The Jacobian inverse schemeA first result can now be established which follows fromSciavicco and Siciliano,30 but it is extended to include EEorientation; the explicit time dependence will often besuppressed for notation compactness.

Proposition 1. If the matrix J{q) has full rank for allJoint configurations q's, the control law

q = J~\q)(yd{t) - /(q)q + Kpe + Kve) (13)

t In fact, even if / <odt is not defined, Luh et al.29 argued that,for small errors, eo is the true time derivative of eo. Later, thishas been demonstrated by Yuan36 through the use ofquaternions.

234 Closed-loop control

JpM>

-K>

• f ,G,*

CONTROLLAW

"disdi»d. COMPUT.BLOCK

n,s,»

Fig. 1. The closed-loop IKP tracking solution scheme.

with Kp and Kv positive definite diagonal matrices suchthat the matrix

M: =-Kp -K

(14)

is a Hurwitz matrix, ensures that e(f)-»0 as f->°°, ife(0) # 0 , and e(t) = 0 along the EE trajectory if e(0) = 0.

Proof: Differentiating (10) with respect to time andaccounting for (5) yields

(15)

Direct substitution of (13) in (15) gives

e + Kve + Kpe = 0. (16)

In force of (14), the convergence of the error e to zerocan be suitably "shaped". Convergence of the positionerror ep needs not to be further explained. Convergenceof the orientation error eo is ensured except for thesingular case when the actual EE orientation differs fromthe desired EE orientation by an Euler rotation of 180°.This corresponds to having (n, s, a) = (-nd, -sd, -arf),with eo = 0. Therefore, as long as this case does notoccur at t = 0, equation (16) guarantees that theorientation error will asymptotically tend to zero. End ofProof.

III. B The Jacobian transpose schemeThe scheme that we derive in the following actuallyoriginates from the work independently developed bySiciliano31 and Novakovi6.32 The convergence of thescheme is proved by means of Lyapunov stability theory.

Proposition 2. The control law

where K is a positive definite diagonal matrix,

s = e + Ae (18)

with A being a positive definite diagonal matrix, and

z = v d - / (q )q + Ae, (19)

ensures that s(f)-*O as f->°°, if 8(0)^0, and s(f) = O

along the EE trajectory if s(0) = 0. Also, theconvergence of s to zero ultimately implies theconvergence of e to zero, by virtue of (18).

Proof: Define the positive Lyapunov function candid-ate of the error sliding vector s in (18) as

v = $sTKs. (20)

Its time derivative along the trajectories of the system(15) results in

v = sTKTz - sTKTJ(q)q (21)

with z defined in (19). Direct substitution of (17) in (21)

8"vesv = -sT TJ(q)JT(q)Ks ^ 0 (22)

which in turn implies that e(t)—>0. In detail, since v islower bounded by zero, in force of the positivedefiniteness of K in (20), s is bounded. This implies thate and e in (18) are bounded, being A positive definitetoo. If J(q)JT(q) is guaranteed to be uniformly positivedefinite, equation (22) ensures that s(f)—»0, andtherefore e(0-*0.

A crucial point then remains the positive definitenessof /(q)/ r(q), which is not guaranteed when themanipulator is in a singular configuration and / is not afull-rank matrix. From (22) it can be seen that thecondition v = 0 implies e = 0, except when the vector Ksbelongs to the null space of the matrix JT(q), where thealgorithm may in principle get "stuck". One can easilyshow, however, that such equilibrium point is unstable,and the time evolution of the desired EE trajectory willcontribute to decrease v again.26

Similar remarks for the convergence of the orientationerror as for the Jacobian inverse scheme of Proposition 1are in order also in this case. End of Proof.

Notice that the control law (17) slightly differs fromthe analogous solution established by Novakovi6.32

There, the resulting equation for the stability is of thekind i) = -av with a>0 which allows for prescribing adesired solution settling time. This might be advan-tageous if s(0) + 0.

It is worth remarking here that, from the conceptualviewpoint, the schemes just presented can be respec-tively regarded as closed-loop versions of the well-knownNewton type method and steepest descent method, buthere the convergence is established beforehand though.

A nice feature of the Jacobian transpose scheme overthe Jacobian inverse scheme is that its computationalburden is reduced. Also, it avoids the numericalinstabilities associated with matrix inversion nearkinematic singularities. One drawback of the scheme inProposition 2, however, is that it introduces, in theneighbourhood of s = 0, an equivalent gain which tendsto oo. The analysis of the second term generated on theright hand side of (17), indeed, reveals that this is givenby the ratio of two quantities that go to zero, as s-»0,with the same order two. This generates a "chattering"behaviour in the time evolution of the joint accelera-tions, which will be visible in the numerical resultsreported in Section IV.

Closed-loop control 235

Several remedies are possible against chattering, assuggested by Novakovi632 for instance. The mostattractive from the computational viewpoint is to simplifythe control law (17) into31

q=/r(q)A:S (23)

which implies

v = sTKTz - sTKTJ(q)JT(q)Ks. (24)

If z is assumed to be norm-bounded, one should chooseK large enough to ensure that v<0. In practice,however, when ||/r/fs| | becomes less than a significantlysmall positive number, it is v>0. Thus, it can beconcluded that the control law (23) no longer guaranteesasymptotic stability, but only ultimate boundedness ofthe tracking error.

Nonetheless, it might be argued that the assumptionon the norm-boundedness of z cannot be automaticallyguaranteed a priori. More precisely, one should assumethat the three terms on the right-hand side of (19) be allnorm-bounded. For the first term, it is natural to assumethat \d is norm-bounded. The second term /(q)q can beput in the form37 //i(q)[qq] + //2(q)[q2]> with [qq] =(<7i<?2<7ifs-- qn-AnY, W] = (q\ql- ••ql)T, and Hi,Hi matrices of appropriate dimensions. The third termAe, finally, can be written as A(vd —/(q)q). At thispoint, it can be recognised that //i(q), H2{q), A, /(q),and vrf can be all assumed to be norm-bounded.Therefore, the problem can be reduced to thenorm-boundedness of q. In practical implementation ofthe algorithm, however, when the scheme is workingwell, i.e. s is small, e and e are also small and then q isexpected to be bounded. A more rigorous proof cannotbe given, but numerical results of Section IV will bearout this issue.

Incidentally, it should be remarked that at steady-state, i.e. when vd = \d = 0, the control law (23) ensuresasymptotic stability. To see this, define the positivedefinite Lyapunov function26-31

v = !(ertfAe + q^q). (25)

Its time derivative along the trajectories of the system(11) and (12) with prf = <orf = 0, under the control (23),results in _ _

v = -eTKs + eTKAe (26)which, by virtue of (18), becomes

v = -eTKe<0 (27)

implying that e->0, and then q-»0 and e-»0.Furthermore, an appealing feature of the solution (23)

lies in the following intuitive physical interpretation.26 Itis well known that the relationship between the vector xof generalized joint forces and the corresponding vectorY of generalized EE forces is given by3

t = /r(q)Y (28)

which can be obtained by applying the principle ofvirtual work and accounting for the "dual" mapping (2).

As a consequence, the control law (23), with s as in(18), is analogous to applying an elastic force KAe and a

damping force Ke at the EE of an ideal manipulator withthe same kinematic structure as the manipulator ofinterest, but having a unitary inertia matrix andoperating in the absence of gravity. This in turncorresponds to apply an impedance control scheme38 tothe above ideal manipulator with simple dynamics.

In force of this analogy, it can be recognised, forinstance, that in the case when the vector Ks is in thenull space of the matrix JT discussed above, this isequivalent to applying EE forces in the direction alongwhich the manipulator cannot move.

We would emphasise again, however, that the purposeof the above presented schemes is only to numericallysolve for the IKP and not to design a robot task spacecontrol. In other words, the joint variables q and q tofeed back in the scheme of Figure 1 are not those sensedby the robot but just those computed by the algorithm.

Another remark is in order concerning the practicalimplementation of the algorithm. The solution (23)suggests that the tracking error can be made arbitrarilysmall by choosing K large enough. It should be pointedout, however, that the implementation of the discrete-time solution algorithm, through the sampling rate, limitsthe maximum values allowable for K. In order toestablish an optimum for that value, a discrete-timestability proof should be undertaken, as done forinstance in Das et al.,23 but this goes beyond the scopesof the present work.

IV. CASE STUDYThe schemes presented in the previous section have beenapplied to solve the IKP along given EE position +orientation trajectories for a six-degree-of-freedomPUMA-like manipulator with zero offsets. Being thisgeometry wrist-partitioned, it is possible to solve the IKPinto two stages; first for EE position through the firstthree joint variables, then for EE orientation through thelast three joint variables.

A numerical example has been worked out. Thedesired EE task is assigned in terms of a positiontrajectory pd(t) and an orientation trajectory that isassumed to represent three Roll-Pitch-Yaw angles fromwhich Rd{t) is generated.

The initial configuration of the manipulator is chosenas locating the EE in the desired position andorientation, i.e. e(0) = 0. The trajectories are straightlines of 0.5 m for EE position and 90° in the space ofRPY angles, to be executed in a very fast time of 0.5 s;standard trapezoidal velocity profiles are imposed.

The proposed inverse kinematic schemes have thenbeen applied at the same solution rate in order thatcomparisons be significant. The solution rate is then setup accordingly to the simplified Jacobian transposescheme which requires the least number of computa-tions. As a matter of fact, the higher the solution rate thebetter the tracking performance expected for thatscheme. It is estimated that, if a high-speed floating-point arithmetic processor is available, a solution rate of1 kHz is sufficient to perform all the computationsrequired by the solution (23).

236 Closed-loop control

The numerical integration method used for computingjoint displacements q and joint velocities q from jointaccelerations q is based on the Simpson rule ofintegration. More sophisticated integration methods havebeen tested, but they have been seen not to provide anyconsistent improvement in tracking performance.

Initially, the Jacobian inverse scheme based on thesolution (13) is applied with Kp = Kv = 0 in an open-loopfashion. The results are illustrated in Figure 2. It isinteresting to note that the position and orientationerrors after t = 0.5 s linearly increase, since no feedbackcorrection is active.

The feedback matrices Kp = diag(250000 250000250000 62500 62500 62500) and Kv = diag(1000 10001000 500500500) are then introduced such that a doublepole at —500 for the position error dynamics and adouble pole at —250 for the orientation error dynamicsin (16) are obtained. The tracking errors are reported inFigure 3 and the resulting joint accelerations in Figure 4.It is clear that the action of the feedback termsconsiderably improves the tracking performance andguarantees null steady-state errors. The peaks in the time

0.0 1 0tine [aec]

10 6.0E -1

(a)

0.0 1.0 2.0 3.0 4.0 5.0 6.0time [sec] E -1

(b)Fig. 2. Tracking errors with the open-loop Jacobian inversescheme: a) position error, b) orientation error.

0.0 1.0tine [sec]

5.0 6,0t -1

(b)

Fig. 3. Tracking errors with the closed-loop Jacobian inversescheme: a) position error, b) orientation error.

evolution of the errors are not surprising, sincetrapezoidal velocity profiles having discontinuous ac-celerations have been imposed.

The Jacobian transpose scheme based on the solution(17) is implemented next. In order to make a "fair"comparison, K = diag(100010001000 500 500 500) andA = diag(250250250125125125) have been chosen sothat equivalent proportional + derivative feedback ac-tions are obtained as for the above scheme. The trackingerrors are depicted in Figure 5 and the resulting jointaccelerations in Figure 6. It can be seen that the trackingperformance is degraded by an order of magnitude, butit is still excellent. As anticipated in theory, however, thejoint accelerations chatter about some "mean-value"trajectories which actually resemble the trajectoriesobtained in Figure 4. However, the joint velocity andposition trajectories, not displayed here, are smooth inforce of the filtering nature of the integrators in cascadeto the joint accelerations.

Finally, the computationally advantageous Jacobiantranspose scheme based on the solution (23) is adoptedwith the same feedback matrices as above. As expected

Closed-loop control 237

3.0 4.0

(a) (b)

0 0 1 0time [aec]

S.O 6.0E - 1

k

(c) (d)

• o

p1If

3s .0

8*o

1

1

1

1. i—

/

/

(e)

0.0 1.0 2 0 3.0 4.0 B.O 6 0tine CMC] E -1

(0Fig. 4. Joint acceleration trajectories with the closed-loop Jacobian inverse scheme.

238 Closed-loop control

^ o

O.D 1 0 2.0 3.0 4 0 5.0 6.0tine [sec) E -1

(a) (b)

Fig. 5. Tracking errors with the closed-loop Jacobian transpose scheme: a) position error, b) orientation error.

o.o i.otine [sec]

2.0 3.0 4.0 5.0 6.0E - 1

ISorES

0 0 10tine [sec]

2.0 3.0 4.0 S.O 6.0E -1

(a) (b)

(c)

1(1

0 0 1.0 2.0 3.0 4 0 5 0 6 0tine {sec] E -1

0 0 1.0 2 0 9 0 4 0 6.0 6.0ti«e (MC) I -1

Fig. 6. Joint acceleration trajectories with the closed-loop Jacobian transpose scheme.

Closed-loop control 239

0.0 1.0 2.0 3.0 4.0 5.0 6.0tiM [sec] E -1

Fig. 6 (continued)

(e)

0.0 1.0 2.0 3.0 4.0 6.0 6.0tine [sec] E -1

0.0 1.0tlae [sec]

2.0 9.0 4.0

(0

0.0 1.0 2,0 3.0 4.0 6.0 6 0time [aec] E -1

(a) (b)

Fig. 7. Tracking errors with the simplified closed-loop Jacobian transpose scheme: a) position error, b) orientation error.

0.0 1.0 2.0 3.0 4.0 6.0 6.0tl»e [s«c] E -1

(a) (b)

Fig. 8. Joint acceleration trajectories with the simplified closed-loop Jacobian transpose scheme.

0.0 1.0 2.0 3.0 4.0 5.0 6.0t u « tsec] E -1

240 Closed-loop control

h

2 0 3 0 4 0

i:2 0 3.0 4.0

(c) (d)

r0.0 1.0 2,0 3.0 4.0 5.0 6.0

tine [sec] E _i

(e)

0.0 1.0t i » [sec]

2.0 3.0 4.0 6.0 6.0E - 1

(0Fig. 8 (continued)

0 0 1 0 2.0 3.0 4.0 5.0 6,0time [sec] E -1

0,0 1 0tine [sec]

3.0 4,0 5.0 6.0E - 1

(a) (b)

Fig. 9. Tracking errors with the simplified closed-loop Jacobian transpose scheme (the initial configuration has a doublesingularity): a) position error, b) orientation error.

Closed-loop control 241

£ *

0 0 10tin« [sec]

2.0 9 0

s,-Siu u>

Hi

ito • -

0 0 1 0tine [sec]

2 0 3 0 4.0 E.O 6.0E - 1

(a) (b)

0.0 1.0tlm [sec]

2.0 3.0

(c)

6.0 6.0E -1

0.0 1.0tiM [sec]

2.0 3.0

(d)

6.0 6,0E -1

roo

0.0 10 20 30 40 50 6.0time [sec] E -1

(e)

0.0 1.0 2.0 3.0 4.0 6.0 6.0tlM [sec] E -1

(0Fig. 10. Joint acceleration trajectories with the simplified closed-loop Jacobian transpose scheme (the initial configuration has adouble singularity).

242 Closed-loop control

in theory, the tracking performance in Figure 7 is not asgood as that in Figure 3, but it is still satisfactory; lessthan 0.45 mm with an average velocity of 1 m/s and lessthan 0.357s with an average velocity of 180°/s certainlyare below the typical accuracy requirements for mostcurrent industrial robots. The joint acceleration trajec-tories in Figure 8 are seen to be "very close" to thetrajectories in Figure 4 which can be considered as the"true" ones.

In order to test the convergence of the scheme in theneighbourhood of singular configurations, anotherexample has been developed for the Jacobian transposescheme (23) with unchanged feedback matrices and zeroinitial conditions, i.e. e(0) = 0.

The initial joint configuration possesses a doublesingularity, namely a shoulder singularity—the wrist islocated along the shoulder axis—and a wrist singularity—(7s = 0, aligning the other two wrist axes.39 It is worthrecalling here that such singularities are the mosttroublesome ones since they may be encounteredanywhere through the manipulator workspace.

The trajectories have the same path length andduration as above; the EE position trajectory is chosenas having a non-zero component orthogonal to the planeof the structure, and similarly the EE orientationtrajectory is chosen as having non-zero roll and yawcomponents. It follows that the tracking error will have anon-zero component along the null space of the Jacobiantranspose at the initial joint configuration. As anticipatedin theory, this is undoubtedly a critical test for thealgorithm.

From the results illustrated in Figure 9 it can benoticed that both tracking errors initially increase,showing the effort to leave the singularity, but afterwardsthey decrease. This behaviour is reflected by the initialhigher values of joint accelerations in Fig. 10.

V. CONCLUSIONSClosed-loop second-order tracking schemes for trans-forming given EE position + orientation trajectories intojoint PVA trajectories have been presented in this paper.A first scheme based on the computation of themanipulator's Jacobian inverse has been derived fromthe concept of resolved acceleration control.

To the purpose of avoiding matrix inversion which isusually critical point in any numerical method, a newscheme has been proposed which is based on thecomputation of the Jacobian transpose. Asymptoticstability of the tracking error is guaranteed, so as in theJacobian inverse scheme, but at the expenses ofchattering accelerations. A computationally advan-tageous simplification of the scheme has been proposedwhich guarantees ultimate boundedness of the trackingerror and asymptotic stability of the steady-state error.

A case study has been worked out for the aPUMA-like manipulator, and numerical results haveclearly demonstrated that the performance achieved withthe computationally fast Jacobian transpose algorithmfavours the use thereof in solving the IKP for anyindustrial robot of arbitrary kinematic structure.

Convergence around singular positions has beensuccessfully tested.

An open research issue remains the application of theproposed scheme to kinematically redundant structures,when the number of joint variables exceeds the numberof task space variables. In fact, although the extension ofthe scheme appears to be quite straightforward, the nullspace of the Jacobian matrix in a redundant manipulatorallows for joint velocities unobservable at the output,which may generate internal motions of the structureuncontrollable though. This point, along with otherrelated topics such as inclusion of constraints andtask-priority strategies40 for these second-order schemes,will constitute the subject of further investigation.

AcknowledgementsWe would like to thank Professor Lorenzo Sciavicco forhelpful discussions relating to this work.

References1. M. Vukobratovk: and M. Kircanski, Scientific Fundamen-

tals of Robotics 3: Kinematics and Trajectory Synthesis ofManipulation Robots (Springer-Verlag, Berlin, Heidel-berg, FRG, 1986).

2. D.L. Pieper, "The Kinematics of Manipulators UnderComputer Control" Ph.D. dissertation, Stanford Univers-ity (1968).

3. R.P. Paul, Robot Manipulators: Mathematics,Programming, and Control (Cambridge, MA, MIT Press,1981).

4. A.A. Goldenberg, B. Benhabib and R.G. Fenton, "Acomplete generalized solution to the inverse kinematics ofrobots" IEEE J Robotics and Automation RA-1, No. 1,14-20 (1985).

5. J. Angeles, "On the Numerical solution of the inversekinematic problem" Intern. J. Robotics Resarch 4, No. 2,21-37 (1985).

6. J. Lenar5i£, "An efficient numerical approach forcalculating the inverse kinematics for robot manipulators"Robotica 3, 21-26 (1985).

7. L.-W. Tsai and A.P. Morgan, "Solving the kinematics ofthe most general six- and five-degree-of-freedom man-ipulators by continuation methods" ASME J. Mechanism,Transmission, and Automation in Design 107, No. 2,189-200 (1985).

8. R. Manseur and K.L. Doty, "A fast algorithm for inversekinematic analysis of robot manipulators" Intern. J.Robotics Research 7, No. 3, 52-63 (1988).

9. D.E. Whitney, "Resolved motion rate control ofmanipulators and human prostheses" IEEE Trans.Man-Machine Systems MMS-10, No. 2, 47-53 (1969).

10. R. Featherstone, "Position and velocity transformationsbetween robot end-effector coordinates and joint angles"Intern. J. Robotics Research 2, No. 2, 35-45 (1983).

11. J.M. Hollerbach and G. Sahar, "Wrist-partitioned, inversekinematic acceleration and manipulator dynamics" Inter. J.Robotics Research 2, No. 4, 61-76 (1983).

12. A Balestrino, G. De Maria and L. Sciavicco, "Robustcontrol of robotic manipulators" Preprints of the 9th IF ACWorld Congress 6, Budapest, Hungary, 80-85 (July, 1984).

14. B. Siciliano, "Solution Algorithms to the InverseKinematic Problem for Manipulation Robots" (in Italian)Ph.D. dissertation, University of Naples (1986).

15. A. Balestrino, G. De Maria, L. Sciavicco and B. Siciliano,"An algorithmic approach to coordinate transformation forrobotic manipulators" Advanced Robotics 2, No. 4,327-344 (1987).

16. L. Sciavicco and B. Siciliano, "Coordinate transformation:

Closed-loop control 243

A solution algorithm for one class of robots" IEEE Trans.Systems, Man, and Cybernetics SMC-16, No. 4, 550-559(1986).

17. L. Sciavicco and B. Siciliano, "An inverse kinematicsolution algorithm for dexterous redundant manipulators"Proceedings of the 3rd International Conference onAdvanced Robotics, Versailles, France, 247-256 (Oct.,1987).

18. L. Sciavicco, B. Siciliano and P. Chiacchio, "On the use ofredundancy in robot kinematic control" Proceedings of the1988 American Control Conference, Atlanta, GA,1370-1375 (June, 1988).

19. L. Sciavicco and B. Siciliano, "A solution algorithm to theinverse kinematic problem for redundant manipulators"IEEE J. Robotics and Automation RA-4, No. 4, 303-310(1988).

20. L. Sciavicco and B. Siciliano, "On the solution of inversekinematics of redundant manipulators" Preprints of theNATO Advanced Research Workshop: Robots withRedundancy, Said, Italy (June/July, 1988) to be publishedby Springer-Verlag.

21. P. Chiacchio and B. Siciliano, "Achieving singularityrobustness: An inverse kinematic solution algorithm forrobot control" IEE Control Engineering Series 36 RobotControJ,: Theory and Application 16, 149-156 (PeterPeregrinus Ltd., London, UK, 1988).

22. H. Asada and J.-J.E. Slotine, Robot Analysis and Control(Wiley-Interscience, New York, NY, 1986).

23. H. Das, J.-J.E. Slotine and T.B. Sheridan, "Inversekinematic algorithms for redundant systems" Proceedingsof the 1988 IEEE International Conference on Robotics andAutomation, Philadelphia, PA, 43-48 (Apr., 1988).

24. Y.T. Tsai and D.E. Orin, "A strictly convergent real-timesolution for inverse kinematics of robot manipulators" / .Robotic Systems 4, No. 4, 477-501 (1987).

25. W.A. Wolovich and K.F. Flueckiger, "Inverse kinematic-based control" Proceedings of the Workshop on SpaceTelerobotics, Pasadena, CA, 165-175 (Jan., 1987).

26. J.-J.E. Slotine and D.R. Yoerger, "A rule-based inversekinematics algorithm for redundant manipulators" Intern.J. Robotics and Automation 2, No. 2, 86-89 (1987).

27. R.J. Vaccaro and S.D. Hill, "A joint-space commandgenerator for Cartesian control of robotic manipulators"IEEE J. Robotics and Automation RA-4, No. 1, 70-76(1988).

28. S.D. and R.J. Vaccaro, "Cartesian control of roboticmanipulators with joint compliance" Robotica 5, 207-215(1987).

29. J.Y.S. Luh, M.W. Walker and R.P.C. Paul, "Resolved-acceleration control of mechanical manipulators" IEEETrans. Automatic Control AC-25, No. 3, 468-474 (1980).

30. L. Sciavicco and B. Siciliano, "A computational techniquefor solving robot end-effector trajectories into jointtrajectories" Proceedings of the 1988 American ControlConference, Atlanta, GA, 535-536 (June, 1988).

31. B. Siciliano, "Closed-loop computational schemes of robotinverse kinematics" Proceedings of the InternationalMeeting: Advances in Robot Kinematics, Ljubljana,Yugoslavia, 113-121 (Sept., 1988).

32. Z.R. Novakovid, "A solution of the inverse kinematicsproblem using the sliding mode" Report No. 5153, InstitutJo2ef Stefan, Ljubljana, Yugoslavia (Mar., 1988).

33. J.-J.E. Slotine and S.S. Sastry, "Tracking control ofnonlinear systems using sliding surfaces with application torobot manipulators" Intern. J. Control 38, No. 2, 465-492(1983).

34. P. Chiacchio and B. Siciliano, "A closed-loop Jacobiantranspose scheme for solving the inverse kinematics ofnonredundant and redundant writs" J. Robotic Systems 6,No. 5, 601-630 (1989).

35. P. Coiffet, Robot Technology Series 1: Modelling andControl (Kogan Page, London, UK, 1983).

36. J.S.-C. Yuan, "Closed-loop manipulator control usingquaternion feedback," IEEE J. Robotics and AutomationRA-4, No. 4, 434-440 (1988).

37. O. Khatib, "A unified approach for motion and forcecontrol of robot manipulators: The operational spaceformulation" IEEE]. Robotics and Automation RA-3, No.1, 43-53 (1987).

38. N. Hogan, "Impedance control: An approach tomanipulation: Part II—Implementation" ASME J. Dyna-mic Systems, Measurement, and Control 107, No. 1, 8-16(1985).

39. F.L. Litvin, Z. Yi, V. Parenti Castelli and C. Innocenti,"Singularities, configurations, and displacement functionsfor manipulators," Intern. J. Robotics Research 5, No. 2,52-65 (1986).

40. Y. Nakamura, H. Hanafusa and T. Yoshikawa, "Task-priority based redundancy control of robot manipulators,"Intern. J. Robotics Research 6, No. 2, 3-15 (1987).