Principle of Orthogonalization for Hybrid Control of Robot Arms

Copyright © IFAC 12lh Triennial World Congress. Sydney. Australia. 1993

PRINCIPLE OF ORTHOGONALIZATION FOR HYBRID CONTROL OF ROBOT ARMS S. Arlmoto·, Yun Hul L1u" and T. Nanlwa· ·Facwty ofEngineering. University ofTolcyo, BU1IIcyO.ku, Tolcyo 113. Japan ··Electrotechnical Laboratory, AlST. MITt. 1-1-4 U~zono, Tsukuba·cily,tbaraki 305, Japan

Abstract. A principle of "orthogonalization" is proposed as an extended notion of hybrid (force and position) control for robot manipulators under geometric endpoint constraints. The principle realizes the hybrid control in a strict sense by orthogonalizing position and velocity feedback signals in joint space to the contact force vector whose components are exerted at corresponding joints. This orthogonalization is executed via a projection matrix computed in real-time from a gradient of the equation of the surface in joint coordinates and hence both projected position and velocity feedback error signals become perpendicular to the force vector that is normal to the surface at the contact point in joint space. To show the important role of the principle in control of robot manipulators, the following three basic problems are analyzed: hybrid trajectory tracking by means of a "modified hybrid computed torque method" , mo~el-b~ed adaptive control under geometric endpoint constraints, and iterative learning control under the same slt~atl?n. It is shown in every case that the passivity of residual error robot dynamics follows from the orthogonallzahon principle and plays a crucial role in convergence of both positional and force error signals.

Key Words. Robot manipulator, force control, hybrid control, principle of orthogonalization.

1.

INTRODUCTION

It is still claimed that even the most advanced industrial robots at the present stage lack versatility in fulfilling various tasks imposed on them. This may be caused from lack of studies on analysis of dynamics and control actions for robot arms that are subject to actual execution of those tasks. One of typical situations during execution of tasks is that an endpoint of the arm is in touch with a surface of an object in the task environment. Polishing, grinding, and even writing with a pen are such examples. In those situations, both position of the endpoint and force at the contact point of the robot must be controlled simultaneously. To solve such a control problem, Raibert and Craig (1981) proposed the so-called "hybrid position/force control" by introducing a mode selection that distinguishes positional control components from force control components in cartesian coordinates. However, this concept can be in principle applied for only the case that the contact surface is a fiat plane in cartesian coordinates. Very recently, MaClamroch and Wang (1990) gave a proof oflocal feedback stabilization using linear feedback for set-point control when both constant target position and contact force are given to the manipulator. They showed that there is in principle no need of use of the mode selection matrix. They (1988) also dealt with a general tracking problem for constrained robots by

using nonlinear feedback on the basis of a modified computed torque method. Kankaanranta and Koivo (1988) treated the problem by using nonlinear transformation of the model in joint coordinates into the constraint frame and reducing the dimensionality of the model as likely in MacClamroch and Wang (1988). However, a control problem of tracking both endpoint position and contact force trajectories still remains unsolved when some of basic physical parameters of robots are unknown or uncertain, though an efficient adaptive control method to track a prescribed positional trajectory was recently proposed by Slotine and Li (1988) when the endpoint is free to move. This paper renovates the concept of hybrid position/force control for a general class of such tasks that the endpoint touches with a general smooth surface. Instead of using the mode selection or formulating the constrained dynamic equation of motion expressed on the constraint manifold, the proposed method uses a so-called "Orthogonalization Principle" that distinguishes positional feedback signals from force feedback signals by introducing a projection matrix in joint coordinates. The matrix is defined so as to project velocity and position error signals onto the plane tangent to the surface described in joint-space at each instantaneous contact point. Hence, the residual error position and velocity signals become automatically per-

335

inertia matrix H(q) for such a class of robot arms is symmetric and positive definite and, moreover, each entry of H(q) is constant or a trigonometric To show the crucial role of this orthogonalization, function of components of joint vector q. It is also three basic trajectory-tracking problems are ana- well known that matrix S(q, q) is linear and homolyzed. The first treated in section 3 is hybrid tra- geneous in q and skew symmetric. First we note jectory tracking by means of a "modified hybrid that the dynamics of eq.(l) under geometric concomputed torque method", the second in section straints also satisfies the passivity between torque 4 is model-based adaptive control for manipulators input vector u and velocity vector q. In fact, note under geometric constraints, and the last in sec- that J.,,(q)q = 0 and therefore tion 5 is iterative learning under the same situation. These last two problems are also treated somewhat qT(r)u(r)dr = V(t) - V(O) in detail in our recent papers (see Arimoto et al (1993) and Naniwa et al (1993)). Finally in sec+ [qT(r)Boq(r) + WI±ID 1I±1I 2 ] dr tion 6 a method for constructing a projection matrix without knowing the equation of the surface is where V(t) is the total energy defined as V = K + given. U. Since the constant term of potential is arbitrary, it is reasonable to assume that min q U(q) = O. Then, V(t) 2: 0, and therefore it follows that 2. ROBOT DYNAMICS UNDER GEOMETRIC ENDPOI IT CONSTRAINTS

pendicular to the force vector normal to the surface in joint-space.

it

it

it

Suppose that the manipulator end point is in touch with a surface as shown in Fig.L The surface is described by a scalar function,
qT(r)u(r)dr 2: - V(O) = 'Y

which shows the passivity of eq.(l) even when the endpoint is geometrically constrained on a surface.

3.

ORTHOGONALIZATION PRINCIPLE AND HYBRID TRACKING CONTROL

Suppose that a pair of desired position trajectory qd(t) and desired force trajectory fd(t) on the surface is given in joint space. The problem is to find an efficient tracking control that makes the manipulator track asymptotically the specified position and force trajectories as t -+ 00. In this section we H(q)ij + (Bo + + S(q, q)) q + g(q) assume that the exact dynamic model of the manipulator together with friction characteristics of the = J~(q)f - €(II±!DJ;(q)±+ u (1) contact is available. However, we reasonably suppose that the acceleration signal ij(t) is not available where the first term of the right hand side denotes in real-time because of the law of causality. Instead the contact force exerted at joints, Jz(q) and J.,,(q) we reasonably assume that the velocity and posidenote the 3 x n Jacobian matrix of x in q and tion signals q(t) and q(t) and the momentum signal 1 x n unit normal vector of
~H(q)

F(t) =

it

f(r)dr

(2)

can be measured in real-time and used in real-time computation of a control input. The left hand side of eq.(l) expresses that of a Lagrange equation for a Lagrangian L = K - U, Let us now introduce a signal called "nominal refwhere K( = qT H( q)q/2) denotes the kinetic energy erence" that is defined by and U(= U(q)) the potential energy. In this form, qr = Q(q) {qd - a.6.q} + f3J~(q).6.F (3) B o denotes a positive definite matrix representing damping factors, H(q) an inertia matrix including where a > 0 and f3 2: 0 are constants and inertial terms of internal load distribution of actuators, g(q) = (8U/8ql, .. · ,8U/8q"f, and u a .6.q = q-qd, .6.F = F-Fd' Fd(t) = fd(r)dr. vector of input torques generated at servo actuators. We consider in this paper a class of anthropoid The n x n matrix Q(q) in eq.(3) is defined by robot manipulators that have only revolute-type (4) joints as shown in Fig.I. It is well known that the Q(q) = I - J~(q)J.,,(q)

it

336

which can be regarded as a projection matrix that plays a role of Lyapunov's function. Further, projects vectors in joint space onto the plane tan- eq.(10) implies the existence of a constant e > 0 gent to the surface ep(q) = 0 at point q (see Fig.2). such that Since ep( q) = 0 as long as the manipulator endpoint V(s(t), 6F(t)) ~ -cV(s(t),6F(t)), (12) is in touch with the surface, it holds that J",(q)q = 0 and hence Q(q)q = q where and Q(q)JJ(q) = O. for H( q) is positive definite and periodic in comLater in the sequel we will deal with the signal beponents of q and B o is also positive definite. From tween current velocity q and nominal reference qr, eq.(12) it follows that i.e.,

s = q-qr

= Q(q) {6q + o:L'J.q}-!3J; (q)6F = SO+SI

where 6q

=q-

So

= Q(q){6q + G6q},

SI

= -!3J;(q)6F.

(6)

s(t)

It is important to note that the nominal reference signal qr is composed of two parts which are 01'thogonal to each other, and hence the residual signal s( = q- qr) is divided into two parts So and SI that are orthogonal to each other, too. The design concept of "nominal signal" based on this idea is called in this paper "Orthogonalization Principle" .

-+

0, L'J.F(t)

-+

0, and

so(t)

-+

0

(14)

as t -+ 00. The speed of convergence is also of the order of exponentially decreasing in time t. Let us now show the convergence of 6/(t) -+ 0 and -+ 0 as t -+ 00 in the following provided that the magnitude of initial difference s(O) is not so large and both qd(t) andijd(t) are bounded. Multiplying both sides of eq.(9) by J",(q)H-l(q), we obtain

6q(t)

To show the effectiveness of introduction of the nominal reference signal on the basis of Orthogonalization Principle, consider a control input that can be computed in real-time by means of a modified computed torque method, which is defined by

u(t) = H(q)ijr + G(q,q)qr +g(q) - J;(q) {Id - ')'6F} ,

(13)

since 6F(0) = 0 by definition of F(t) and 6F(t). Thus, we conclude that

(5)

qd and

V(s(t), 6F(t)) ~ V(s(O), 0) exp( -et)

J",(q)s + J",(q)H- 1 (q)G(q, q)s = J",(q)H- 1 (q)J;(q) {61

+ ')'L'J.F}. (15)

From definition of s in eq.(5) and differentiation of the equality J",s = -,8L'J.F, it follows that

(7)

where we put

(16)

G(q, q)

= B o + ~iI(q) + S(q, q)

+~(IIJ.,(q)qIDJ;(q)J.,(q)

Thus, substituting this into eq.(15), we obtain

(8)

(,8 + J",H- 1 J;) 61

and')' is a positive constant. Note that real-time = (J",H- 1 G - i",) s - ')'J",H- 1 J; L'J.F.(17) computation of the right hand side does not include the computation of ij but includes the differentiaSince s(t) -+ 0 as t -+ 00 and qd(t) is bounded, tion of 6F in time t. This implicitly assumes the q(t) is also bounded (in detail, see Theorem 1 and use of a force sensor for on-line measurement of the Appendix A). Hence i",(q) is also bounded. On force signal I(t). However, we do not use eq.(7) the other hand, all entries of H- 1 (q) are trigonofor real-time compensation of the contact force I (t) metric functions of q and G (q, q) is linear in q and arising in the right hand side of eq.(l). The con1 trol law defined by eq.(7) is called in this paper periodic in q. This implies that (J",H- G - i",) is a "Modified Hybrid Computed Torque Method". bounded. Thus, from eq.(17) together with eq.(14), we conclude that Substituting this into eq.(l) yields

H(q)s + G(q,q)s = J;(q) {L'J.I + ')'L'J.F}.

(9)

L'J./(t)

Taking an inner product between sand eq.(9) gives rise to

:t

_sT {B o + WI±IDJ;(q)J.,(q)} s

I(t) (10)

= 21 {sTH(q)s+,86F 2 }

as t

(18)

-+ 00.

= Id(t) +,8 + J",~-lJJ; { (J",H- G 1

This shows that the quadratic quant.ity

V(s,L'J.F)

0

More strictly, if Id(t) is bounded from below by a positive value 10 > 0, i.e., Id(t) ~ 10 > 0, and the magnitude of the initial residual vector s(O) is as small as the inequality

[~ {sT H(q)s + ,86F2 }] -!h6F 2 •

-+

(11) 337

-')'J",H- 1 J; L'J.F } > 0

i",)

S

(19)

holds during an early stage of maneuvering, Le.,

side of the dynamic equation (1) together with a

t E [0, td for some positive value tt > 0, then term of contact friction can be rewritten as follows: f(t) remains positive for all t E [0,00) because Y(q, q, q, ij)0 = H(q)ij + G(q, q)q + g(q), (22) V(s(t), LlF(t)) :5 V(s(O), LlF(O)) and s(t) -+ 0, LlF(t) -+ 0 as t -+ 00. where Y is a known matrix without depending on masses and inertia of the links. The first q in Finally we show the following under an appropriate Y (q, q, q, ij) denotes the q appearing linearly and condition of smoothness of the surface 't'(q) = 0: homogeneously in If (q) and S (q, q) and the second represents the linear form outside the bracket Theorem 1. There is a positive constant T'f > 0 In design of an adaptive controller, we usually use depending on the smoothness of the surface such a matrix Y( q, q, qr, ijr) instead of the last two terms that, for any initial discrepancy Llq(O) and Llq(O) q and ij in Y(q,q,q,ij). In detail, let satisfying IIq(O) 11 /a + IILlq(O)1I < T'f, eq.(13) implies

n.

Llq(t)

-+

0 and Llq(t)

-+

0 as t

-+

00.

(20)

provided that a > 0 is large enough satisfying a/2 ~ cO'SUPt>o IIqd(t)lI, where Co is a positive constant that dep-ends on the smoothness of the surface and the minimum radius of curvatures of the surface (in further detail, refer to Appendix A). The proof of this will be given in Appendix A.

where vector qr is the nominal reference signal defined by eq.(3). It is implicitly assumed that all q, q, qr and qr and hence Y(q,q,qr,qr) are computable in real-time. If the frictional coefficient WlxlD is unknown, it should be parameterized by unknown coefficients which can be put into the vector 0 of unknown parameters. Let us design an adaptive control law by the form

Finally it should be remarked that the momentum error LlF in the nominal reference signal qr can be excluded by letting fJ = 0 for the sake of showing only the convergence of Llq and Llq. Even in this case, it is possible to prove the convergnece of LlF ad Llf from eq.(17).

where 0 is an estimation at time t of unknown parameters 0. Substitution of eq.(24) into eq.(1) yields

It is also important to see that, even in ordinary cases without geometric constraints, the conventional computed torque method defined by

(25)

u(t) = H(qd)ijd + { Eo

{Y(q,q,q,ij) - Y(q,q,qr,ijr)} 0 +Y(q, q, qr, qr)(0 - 0) = J; (q) {Llf + 1'LlF} which can be written in the form

+ ~If(qd) + S(qd, qd)} qd

+g(qd)

Y(q, q, s, s)0 + Y(q, q, qr, qr)Ll0 = J;(q) {Llf + 1'LlF}

(26)

(21)

where Y(q,q,s,s)0 = H(q)s+G(q,q)s and s is dedose not ensure the convergence of tracking errors fined by eq.(5). Estimation 0 is updated according as t -+ 00 if Llq(O) :f. 0 or Llq(O) :f. O. Instead of to the adaptive law eq.(21), the modified method defined by

u(t)

= H(q)qr + { Eo + ~If(q) + S(q, q)} qr + g(q)

ensures the convergence s(t)(= Llq + aLlq) -+ 0 as t -+ 00 and hence the convergence Llq(t) -+ 0 as t -+ 00, even if Llq(O) :f. 0 and Llq(O) :f.

o.

4.

which implies

d UA0- = r-tyT( q,q,qr,qr . . ..) S dt

(28)

where Ll0 = 0 - 0, because the unknown parameter vector 0 is fixed and hence dLl0/dt = -d0/dt. Now, let us take an inner product of both sides of eq.(26) with s. This results in

ORTHOGONALIZATION PRINCIPLE IN ADAPTIVE HYBRID CONTROL

1 d

The design of model-based adaptive controllers is based on the property that the manipulator dynamics can be parameterized as a linear form of a parameter vector 0 whose components are functions of unknown or uncertain masses and moments of inertia of the links. In other words, the left hand 338

2dt {sTH(q)s+Ll0 T rLl0} +sT {Eo = sT J;(q) {Llf = -fJ { 1'LlF

2

+ ~(lIxll)J;(q)J",(q)} s + 1'LlF}

+ ~ :t LlF 2 }

,

(29)

which is reduced to

V(t)

= -ST(t) {B o + {(lIxII)J",(q(t))J;(q(t))} s(t) -(JIt1F 2 (t),

(30)

where

= ~ {sT(t)H(q(t))s(t)

V(t)

+t1E>T(t)rl!1E>(t) + (Jl!1F 2 (t)}.

lim s(t)

= 0 and

lim AF(t)

t--+oo

= 0 as

t

principle, it is possible to prove that the residual error dynamics is passive in the sense of exponential weighting for output Yk = Qkt1qk versus residual input AUk. In detail it is shown (Naniwa and Arimoto (1993)) that

(31)

Since V is positive definite in s, t1E>, and t1F and the right hand side of eq.(30) is negative definite in sand t1F, eq.(30) implies the finiteness of L 2 _ norms of sand t1F and the boundedness of s(t) and AF(t). Further, if we reasonably assume that qd,qd, and qd are bounded and qd is continuous uniformly in t, it is possible to prove the uniform continuity of s(t) and AF(t) in t (see Arimoto et al (1993)), which implies t~oo

where AUk = Uk - QdUd, Qk = Q(qk), AUk = Uk - Ud, and hk is a remaining nonlinear term of Aqk and Aqk. By virtue of the orthogonalization

it

e->.r Aqf QkAukdr

+

it

~ e->.tV(t) -

V(O)

e- Ar [t1qf{AQk H (qk)Qk

+Qk(B - PI 1)Qk}t1Qk +Aqf {AA - (Po + lo)1} Aqk] dr. (39) where Po, PI, and and

,0 are some positive constatns

-+ 00,

(32) and hence so(t) -+ 0 as t -+ 00. This leads to Theorem 1 again, though the proof of it should be devised more carefully than that given in Appendix.

If B

5. LEARNING CONTROL UNDER GEOMETRIC ENDPOINT CONSTRAINTS Consider now iterative learning for a class of robot tasks in which the manipulator endpoint is in touch with a surface cp(q) = 0 (see Fig.l). We propose the following learning update law:

Uk+I(t) = Uk(t) - 4>Q(qk(t))t1qk(t), (33) Uk+l(t) = Uk(t) + tf;t1lk(t). (34) At the k-th trial the manipulator obeys the dynamICS

H(qdiik + G(qk, qd)qd + g(qd = J:(qk)/k + Vk (35) where the input Vk is designed as the PD feedback plus the feedforward term based on the learning law, i.e.,

Now, to derive a residual error equation for the manipulator, let

(Po

~

PI1 and A > 0 is chosen so that AA

+ 10)1, it holds that

~

which means the exponential passivity of error dynamics. The convergence of Aqk and t1qk to zero as k increases follows immediately from inequality (39) (see Naniwa and Arimoto (1993)).

6.

CONCLUDING REMARKS (PROJECTION BASED ON FORCE SENSATION)

In this paper we proposed the principle of "jointspace orthogonalization" and showed its crucial role in hybrid control of robot arms under geometric constraints. Finally we remark that the principle can be applied even if the equation of the constraint surface is not available. In such a case, it is possible to construct the projection matrix Q(q) from the measured data of the force sensor. Since the sum of contact and friction force vectors is measured, this total force must be transformed onto joint-space by means of the transpose of the Jacobian matrix, Le.,

H(qd)Qdiid + H(qd)Qdqd + G(qd, qd)Qdqd +g(qd) = J! Id + QdUd + J! Ud, (37)

Then, the contribution of the contact friction can where G G + B I and Qd Q(qd). Subtract- be cancelled by extracting it by taking an inner ing this from eq.(35), we obtain the residual error product between the total force and the velocity q in such a way that dynamics

=

=

I

H(qk)l!1iik + H(qk)Qk t1q + G(qk, qk)QkAqk +Al!1qk + hk = t1uk + J'[(AUk + l!1lk)(38) 339

I~ -

'T

~

q 1 JT()J ( ). - JT( )1 IIJ",(q)qIl2 '" q '" q q - 'f> q .

This leaads to the construction of the projection matrix in the following form:

Q(q)

=I

- f{,(q)ffJ",(q)/ 1IJ:(q)fI1

2 •

APPENDIX A (PROOF OF THEOREM 1) It is evident from eq.(12) that there is a constant

,0 ~ 1 such that IIso(t)1I ~ 'ollso(O)II. The inner product of So and 6.q yields d 2 {2 2} 2"1 dt l16.qll = -a l16.qll - IIJ",6.qll

+6.qT J:(q) {J",(q) - J",(qd)} qd + 6.qT So. Since /p( q) is smooth, it is possible to assume reasonably as shown in Fig.A-1 that there are positive numbers 'TJo > 0 and Co > 0 such that at any t > 0 the inequality l16.q(t)1I ~ 'TJo implies IJ",(q(t))6.q(t)1 < l16.q(t)1I and IIJ",(q) - J",(qd)1I ~ Co l16.qll. Substitution of this into eq.(A-l) yields

Raibert, W.H. and J.J. Craig (1981). Hybrid position/force control of manipulators. ASME J. of Dynamic Systems, Measurement, and Control, 103-2, 126-133. Sadegh, N. and R. Horowitz (1990). Stability and robustness of adaptive controllers for robotic manipulators. International J. of Robotics Research, 9-3, 74-92. Slotine, J.J. and W. Li (1988). On the adaptive control of robot manipulators. International J. of Robotics Research, 6, 49-59. Whitcomb, L.L., A.A. Rizzi, and D.E. Koditschek (1991). Comparative experiments with a new adaptive controller for robot arms. Proc. of 1991 Int. Con!. on Robotics and Automation, April 2-7, Sacramento, California.

!

d -1I6.qll

dt

(3 - -COCl) l16.qll + II s oll < - - -a 4 2

(A-I)

where Cl = SUPt>o IIqdll. If a ~ 2coc}, then it follows from eq.(A-l) that

~

2'0 {1I6.q(O)1I + a II6.q(O)II}. (A-2) a Thus, by taking 'TJ = 'TJo/2,o, it follows that if l16.q(O)1I /a + l16.q(O)1I < 'TJ then l16.q(t)1I ~ 'TJo for any t ~ to. Since IIso(t)1I -+ 0 as t -+ 00, eq.(A-1) implies 6.q(t) -+ 0 as t -+ 00 and, at the same time, 6.q(t) -+ 0 as t -+ 00. l16.q(t)1I

REFERENCE

Fig .1

Hybrid (position/force I control under geo~etric constraint

{3.(qJ6q}J~(ql

Arimoto, S., Y.H. Liu, and T. Naniwa (1993). Model-based adaptive hybrid control for geometrically constrained robots. to be presented at 1993 IEEE Int. Conf. on Robotics and Automation, May 2-7, Atlanta, Georgia. Kankaanranta, R.K. and H.N. Koivo (1988). Dynamics and simulation of compliant motion of a manipulator. IEEE J. of Robotics and Automation, 4-2, 163-173.

:;..-"-------;".L----....;o. "

Fig.2 Projection of 6q onto the tangent plane to the surface at the contact point 4>(q)-o

McClamroch, N.H. and D. Wang (1988). Feedback stabilization and tracking of constrained manipulators. IEEE Trans. on Automatic Control, AC-33-5, 419-426. McClamroch, N.H. and D. Wang (1990). Linear feedback control of position and contact force for a nonlinear constrained mechanism. Trans. on the ASME, J. of Dynamic Systems, Measurement, and Control, 112, 640-645. Naniwa, T. and S. Arimoto (1992). Learning control for robot manipulators under geometric endpoint constraint. to be published in Trans. of SICE, 29-4, (in Japanese). 340

Fig.A-1 Smoothness of the surface in a neighborhood of the contact point