Passive Coordination Control of Nonlinear Bilateral Teleoperated Manipulators

D o n g j u n Lee a n d P e r r y Y. Li * Department of Mechanical Engineering, University of Minnesota,

111 Church St. SE, Minneapolis MN 55455. {d j l ee ,p l i }©me.umn.edu

Abstrac t - - In t h i s p a p e r , a t e l e o p e r a t i o n c o n t r o l l e r is p r o p o s e d for a p a i r o f nonlinear m a s t e r a n d s lave m a n i p - u l a t o r s . T h e c o n t r o l l aw e n a b l e s t h e n o n l i n e a r r o b o t s t o b e p e r f e c t l y c o o r d i n a t e d d e s p i t e a r b i t r a r y e n v i r o n m e n t a n d h u m a n f o r c i n g s , w h i l e e n s u r i n g t h a t t h e c l o s e d l o o p t e l e o p - e r a t o r s y s t e m is pass ive w i t h r e s p e c t to a s u p p l y r a t e t h a t i n c l u d e s a u s e r s p e c i f i e d b i l a t e r a l p o w e r s c a l i n g f a c t o r . Af- t e r p e r f e c t c o o r d i n a t i o n has b e e n a t t a i n e d , t h e c o n t r o l l aw cons tra ins n d e g r e e s o f f r e e d o m ( D O F ) . T h e d y n a m i c s o f t h e r e m a i n i n g n d e g r e e s o f f r e e d o m r e s e m b l e s t h o s e o f a u s u a l r i g id r o b o t . T h u s , t h e t e l e o p e r a t o r b e c o m e s a c o m m o n r i g id m e c h a n i c a l t o o l w i t h w h i c h b o t h t h e h u m a n o p e r a t o r a n d t h e p h y s i c a l e n v i r o n m e n t i n t e r a c t . I n a c o m p a n i o n p a p e r [1], a c o n t r o l m e t h o d o l o g y to e n d o w t h e n - D O F c o m m o n pass ive t o o l w i t h u s e f u l t o o l d y n a m i c s is p r o p o s e d .

I. I N T R O D U C T I O N

In this paper, we propose a control scheme for a 2n- DOF n o n l i n e a r teleoperator consisting of a n-DOF master and a n-DOF slave nonlinear robotic systems. The control scheme renders the 2n-DOF teleoperator as a n-DOF com- mon passive mechanical tool to human operators and slave environment by achieving perfect coordination between the master and the slave robots, which effectively imposes a n- DOF holonomic constraint on the system.

The control scheme utilizes feedforward cancellation of the mismatched disturbances to achieve the perfect master- slave coordination in the sense that configurations of the master and the slave are coordinated in the presence of ar- bitrary human/environmental forcings. Coordination be- tween the master and slave configurations is the primary prerequisite for good realism [2]. Recall that ideal trans- parency [3] can not be obtained without perfect coordina- tion, since it implies perfect coordination with zero inter- vening apparent inertia [4].

Energetic passivity of the teleoperator is also crucial to ensure safety of human operators in the control loop and delicate slave environments (Eh],E6]). In order to pre- serve the passivity of the closed loop teleoperator, a special structure is used for controller implementation. The spe- cial structure preserves the passivity not only with using the feedforward cancellation but also in presence of model uncertainties or inaccurate force measurement.

In ET], [8], control laws were proposed to achieve simi- lar control objectives (perfect coordination via feedforward cancellation and robust passivity). However, the control laws were restricted to linear dynamically similar systems. This paper removes this limitation.

. • Corresponding author. Research supported by National Science Foundat ion under grant CMS-9870013

The rest of the paper is organized as follows. In section II, the control problem and control objectives are formu- lated. In section III, we decompose the nonlinear 2n-DOF teleoperator systems into two n-DOF systems according to two aspects: overall motion and coordination. Individual control laws are first designed for the decomposed systems in section IV. The implementation of the control laws to enforce closed-loop passivity is presented in section V. Ex- perimental results are presented in section VI and section VII contains concluding remarks.

A control methodology to endow the resulted n-DOF common passive mechanical tool with useful tool dynamics is proposed in the companion paper Eli.

II. P R O B L E M F O R M U L A T I O N

A. Plant

We consider a 2n-DOF n o n l i n e a r teleoperator con- sisting of a n-DOF master and a n-DOF slave robots:

fl{Ml(ql)Cll if- C l (q l , Ctl)Ctl : T1 if- F1} (1)

M2(q2)ti2 + C2(q2,/12)/12 = T2 + F2 (2)

where p > 0 is a user specified power scaling to be de- fined later and (ql ,q2) , (T1,T2), (F1,F2) are configu- rations, control commands from actuators, and environ- mental forces for the master and the slave robot, respec- tively. The system (1)-(2) satisfies the structural property of robotic systems s.t. inertia matrices M1 (ql), M2(q2) c ~nxn are symmetric and positive definite and M l ( q l ) - 2C1(ql , / t l ) and M2 (q2)-2C2(q2, it2) are skew-symmetric, where C l ( q l , / t l ) , C2(q2, it2) • ~nx~ are Coriolis matrices of the master and the slave robots.

B. Energetic Passivity with Power Scaling

Since it is useful to amplify / a t tenuate power bilater- ally, we want to incorporate power scaling p as in (1)-(2). With the power scaling p > 0, the scaled power input by the human operator and the slave environment is given as:

T . 8p ( ( t l , (t2, F 1 , F 2 ) - - P " FIT( t l -Jr- F 2 q2 • (3)

s c a l e d e n v i r o n m e n t h u m a n p o w e r p o w e r

Here, p > 1 amplifies the human strength, whereas p < 1 attenuates it w.r.t, the slave environment. With the supply rate (3), we require the closed-loop teleoperator to satisfy the energetic passivity condition with power scaling p s.t.:

fo tSp ( C t l ( T ) , ( t 2 ( T ) , F I ( T ) , F 2 ( T ) ) d T ~ ~ 0, (4) ~ C 2 ~ Vt

3278

~iiiii!ii!iiiiiiiiiiiiiii~ ::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: :i~i~-~{::i

i::i::i::i::i::?i- ============================================================================================================================== ........................... ii~i: ~ i i ~:~:%a~;ii)ii/ii~ .~iiii{iiiiii~{~i~i~i~i~iiiiiiiiiiiii{iiiiiii{i;!}?;i~i;i;1i!iii;iii{{;i{{;{;i;!;iiii;i{;ii;i!iiii;i;i{ii{iiiN:* .{{{i{i i::i~: :~ii{ii::{{::{Ii l, !iiiiii!i:: ::i:: !{iii :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: ~i::::i:: }:: :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: ::::::::::::::::::::: ::::::::,::::::::.~:::: 4::::::~s~`.```:~.~.~::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: ' : . :::::::::::::::::::::::::::::::::::::::::::::: :.:.:,:,:,, ,:.:: . . . . . . . :.:.:.::..,:,:.:.:..~..,.~,,.::::::::::::::::::::::::::::::::::::::::::::::::: .:,:,:,::,:,::::~,~x,::::,:::::::,:,: ::::::::::::::::::::::::::::::::

~ !~ii~l iiiiiii~,ii ii,,ii,,i~,i,,!,,!ii,,i~i,~i!::ii!iiii~ii~iiiiiiiiiiiiii :~::i~i~ii~!~::~i:::::i:>~i::i::i::i::i~

iiilN{{{{

Fig. 1. A teleoperator consisting of two 2-DOF planar master and slave robots. Each robot is equipped with force sensor.

for some c C ~ which would depend on the initial condition at t - 0. The supply rate (3) inspires the following defini- tion of scaled kinetic energy for the teleoperator (1)-(2)"

P . T M 1 .T /~P((tl, ( t 2 ) - ~ q l l (q l ) ( t l + ~q2 M2(q2)(t2 _ 0. (5)

C. The Control Objectives

In order to render the 2n-DOF teleoperator system (1)-(2) as a n-DOF common passive mechanical tool, a con- troller will be designed to achieve the following objectives: 1. The closed-loop teleoperator preserves the energetic pas- sivity with the power scaling p in the sense of (4), 2. The master and the slave configurations are perfectly coordinated s.t.:

ql - q2, V F1,F2; (6)

3. Overall motion of the closed-loop teleoperator mimics the n-DOF robot dynamics:

ML(q)tiL + CL(q,/t)/tL -- FL, (7)

where qT _ [qg, q T ] , / t - [/t T,/t T] and /tL C ~n×l is the velocity of the overall motion. One natural way to define /tL is s.t. /tL = /tl = it2 after achieving the perfect coordi- nation. Also, we define ML(q) = p M l ( q l ) + M2(q2) and CL(q) = pCl(ql ,Cll) -+- C2(q2,/t2). Notice that ML(q) and CL(q,/ t) are the naturally obtained inertia and Cori- olis matrices when the master and the slave are perfectly coordinated (by summing the master and the slave dynam- ics in (1)-(2) with condition ql : q2).

III. D E C O M P O S I T I O N

In this section, we decompose the 2n-DOF original teleoperator system (1)-(2) into two n-DOF systems ac- cording to two aspects of teleoperation: overall motion and configuration coordination. This decomposition is appli- cable to any 2n-DOF teleoperator system as long as the master and the slave are n-DOF robotic systems, thus this approach removes the restriction of the decomposition pro- posed in [7], [8] that the master and the slave must be linear dynamically similar.

ML (q) 0

Decomposition is designed according to the following design criteria: 1) coordinated teleoperator (locked system) has the natural inertia ML(q) -- f l M l ( q l ) + M2(q2) as in (7); 2) one of the decomposed system (shape system) explicitly represents the coordination error (6), s.t. qE -- ql -- q2, where qE is configuration of the shape system.

Define a decomposition matrix S(q) C ~2n×2n to be

( (tL ~ [ -- ¢(q) ¢ ( q ) ] ( ( t l ( : t E / - - L I I --I ' ) (s)

J ~ r

S(q)

where ¢(q) - IpM21(q2)M1(q1)+ I] -1 . (9)

Since ¢(q) is nonsingular, S(q) is also nonsingular (full- rank). According to the transform (8), the compatible transform for T i , F i i -- 1, 2 are given as:

( ) ( ) (Pl~ 1) F2 (pTI~ FL _ s _ T ( q ) \ I n _ s - T ( q ) \ T 2 ] FE (10) TE

Then, inertia of the teleoperator system (1)-(2) is trans- formed into another block diagonal inertia matrix:

0 ] -T [ pMl(ql) ME(q) "-- S (q) o

where ME(q) -- p M l ( q l ) + M2(q2)

ME(q) -- peT (q)M1 (ql)¢(q)

_+_ {¢T (q) _ I}M2(q2){¢(q) - I}

o ] 1 M2(q2) S - (q),

(11)

(12)

(13)

are symmetric and positive definite matrices. Using (8)-

(13), and the fact that ( ~ ) - ( - - ¢ ( ~ ) / t E ) + S ( q ) ( ~ ) , we

have the following two n-DOF decomposed systems"

ML(q)tiL + CL(q,/t)/tL + CLE(q,/ t) / tE -- TL + FL (14)

ME(q) t iE + CE(q, / t ) / tE + CEL(q,/t)/tL -- TE + FE, (15)

where

CL(q,/ t) = f lCl (q l , c11) + C2(q2,it2)

CE(q, / t ) = f lCT(q)Cl (q l , ¢11)¢(q)

+ {¢T (q) _ I}C2(q2,/12){¢(q) - I}

CLE(q,/ t) = pCl (q l , (tl)¢(q) + C2(q2,/t2){¢(q) - I}

+ ME (q)q~(q)

CEn(q, CI) = flcT (q)C1 (ql, c11) -+- {¢T (q) _ I}C2(q2, c12).

We call the n-DOF system (14) the locked system, since it represents the dynamics of the teleoperator after being perfectly coordinated (locked), whereas the n-DOF system (15) will be referred to as the shape system, since it re- flects coordination aspect. Notice that the locked system has the natural inertia ML(q) in (7) and the shape sys- tem configuration qE explicitly represents the coordination error as proposed by the design criteria. FE in (15) rep- resents mismatched human / environmental forcing. The locked and shape system have useful properties as in the following proposition.

3279

~ c~_q

/V

to cancel out the coupling term CLE(q, it)itE in (14) to achieve (7). Then, it is easy to show that the locked sys- tern (14) with the control (19) duplicates the desired target locked system dynamics (7). Tc will also be used to endow the coordinated teleoperator with useful tool dynamics in the companion paper [1].

B. Shape system control

Recall the shape system dynamics (15): 1 E~} Fig. 2. ~ versus flywheel energy t~f(x) -- -ffEM 1, . Note

that the slop of the border of C1 is 75v Also shown are 4 ME(q)t iE + CE(q, dl)dlE + CEL(q, all)rilL WE + FE 2 F m a a ~ " _ _ ,

sample paths of (~f(t) , X//-V(t)). In Paths 1 and 2, V(t) --+ O, and in Path 3, V(t) becomes ult imately bounded by IF. In Path 4, V(t) --+ 0 but it is not guaranteed.

(20)

d where qE(t) -- ql --q2 and file -- ~dlE. The control ob- jective for the shape system is

P r o p o s i t i o n 1 Consider the systems (14) and (15). 1. ME(q) -- 2Cn(q, / t ) is skew-symmetric, 2. 2C (q,Q)i 3. CLE(q,/ t) + CTL (q,/t) -- 0.

Therefore, if we cancel out the couplings CLE(q, Ct)CtE in (14) and C EL (q, /l) dlL in (15), the locked and the shape systems have dynamics reminiscent of the usual n-DOF robotic dynamics.

Remarkably, with the coordinate transformation (8), the scaled kinetic energy (5) of the original 2n-DOF tele- operator system is decomposed into the sum of the kinetic energies of the shape and the locked systems s.t.

1 1. T ( ( t l , ( t 2 ) - KdlLTML(q)~tL + -~qEME(q)dlE, (16) /~p

using (11). Also, because of (8)-(10), the supply rate in (3) is the sum of the usual individual supply rates of the locked and shape systems:

8p (1:tl, 1:t2, :F1, :F2 ) T . T . - - F L q L + FEqE. (17)

The following proposition is a direct consequence of (17).

qE = q l - - q 2 --+0 ¢:~ q l = q 2 .

Thus, we design the shape system control TE to be:

T E = C EL (q, it)itL -- K~itE -- KpqE -- FE (21)

where K~ and Kp are constant symmetric and positive def- inite kinematic feedback gains. Notice that the control in- corporates the feedforward cancellation of F E in (20) with a proportional-derivative (PD) control for stabilization.

P r o p o s i t i o n 3 The shape system (15) with the control (21) has (qE,/tE) = (0, 0) = as globally exponentially sta- ble equilibrium. Without feedforward cancellation of FE in (21), i fFE( t ) are bounded, then qE and die are ultimately bounded.

Proof : The closed-loop shape system dynamics with the control (21) is:

ME(q) t iE + CE(q, it)itE + KvitE + KpqE = O. (22)

P r o p o s i t i o n 2 Consider the teleoperator system (1)-(2). I f its locked system and the shape system in (14)-(15) are controlled using T L and T E respectively, such that the com- bined system is passive : i.e. 3 Cd s. t. V F L , F E , and Vt > O, t

~0 T . FT dlEd7 . > c 2 FLqL -+- - - - - d , (18)

then, the teleoperator system (1),(2) is passive with respect to the supply rate Sp in (3) (i.e. (4) is satisfied).

Proof : Integrating (17) and making use of (18),

we h a v e fo 8p( ( t l (T ), t~t2(T ), :FI (T ), F2(T))dT - fo FLqLT" + FTitEdT >_ --C2d---C 2. I

IV. C O N T R O L L E R DE SI GN

A. Locked system control

Comparing the locked system dynamics (14) with the target locked system dynamics (7), the locked system con- trol is designed to be

TL = CLE(q, dl)/tE (19)

It is well-known that nonlinear robots exponentially con- verge to the origin under PD control [9]. Thus, we can choose positive definite matrices P(t) , Q(t) c ~2nx2n s.t. if we define a Lyapunov function,

(q;) then, for some 7 > 0,

q )Q(t)qE <_ - 7 " V(t) (24)

where 3' is the exponential convergence rate (which may be

estimated from 3' _ > infxc~ 4n xTQ(t)XxTp(t)x > 0).

If FE in (21) is not canceled out, the closed-loop shape system dynamics are

ME(q) t iE + CE(q, all)diE + KvitE + KpqE = FE, (25)

which is exponentially stable with F E(t) as the input. Thus, it is easy to show that Lyapunov function (23) sat- isfies V(t) _< - 7 . V ( t ) + AVI(t)F,~a~, for some A > 0,

3280

where Fmax >_ IIF (t)ll vt > 0. From the inequality, the ultimate bound for the Lyapunov function is found to be

1 ) l Fmaz for a l l 1 ---~F , then V5 (t) < Therefore, if V~(0) < ~ max

t > 0. Otherwise, for any V 1 (0), and for any e > 0, there A Fmax + e Vt > T . • exists T > 0, so that V-} (t) _< ~

V. PASSIVE C O N T R O L L E R I M P L E M E N T A T I O N

In order to ensure passivity robustly, a fictitious energy storage (simulated in software) is utilized, which has the usual flywheel dynamics:

EMfE~f -- ETf, (27)

where E Mf is the inertia, E Xf is the configuration, and ETf is the coupling torque to be designed. The flywheel is used as an energy reservoir to generate feedforward cancel- lation of F E without violating energetic passivity and to recapture the energy dissipated through Kv in (21).

Incorporating the fictitious flywheel, the total con- troller is implemented using a negative semi-definite struc- ture as follow:

(TL/ [ O CLE q0 O O ](0L) TE __ C E L (q, dl) Ad(t) - K p EE(t) ClE

Kp(:lE 0 Kp 0 0 qE " \ ETI ] 0 --ET(t) 0 0 E&I

J Y

~ * (t): negative semi-de f in i te E

(28)

Since c T E ( q , ~ t ) - - C E c ( q , dt) (proposition 1) and Ad(t) will be designed to be negative-definite (given later), the matrix f~)(t) will be negative semi-definite.

The implementation structure a ? ( t ) i n (28)has the following components: 1. Cancellation of coupling CcE(q, dt) and CEc(q, dt) in (14) and (15). The property cTE(q, dl) -- - -eEL(q, dl) (proposition 1) makes n)(t) negative semi-definite, i.e. an- other fictitious energy storage is not needed for cancellation of this coupling. 2. Feedforward cancellation of the mismatched distur- bances FE is implemented through

E E ( t ) - --g(E2f )KvitE - P(t) FE (29) E ~ f '

using energy stored in the fictitious flywheel without vio- lating the passivity. Dissipated energy through Kv is also recaptured by this entity and stored in the fictitious fly- wheel. Here, the threshold function g(x) is defined by

1

g(x) - - ~sign(x) 1

xl>f0 0 ¢ x ___f0 x l -0 ,

(a0)

where f0 > 0 is a threshold on the flywheel speed I E2fl to ensure the controller variables in (28) are well-behaved. The switching function p(t) is designed to be

1 if (E2f,qE,dtE) • C at time t (31) p( t ) - - 0 otherwise

to turn on/off the feedforward cancellation in (21) to pre- vent the energy in the flywheel E Mf from depleting. De- sign of the switching region C will be given in theorem 2. 3. Constant damping effect Kv in the shape control (21) is achieved regardless of the value of I E2fl by a negative semi-definite entity:

Ad(t) -- --{1 -- E2fg(E2f ) }Kv

and E E ( t ) i n (29). When < f0, some portion of energy can not be recaptured by EE(t) in (29) and it is dissipated through Ad(t). 4. Symmetric and positive definite kinematic feedback gain (spring) Kp • ~nxn in (21) improves the convergence rate of the coordination.

Notice that when IE2II >_ fO, the implemented control law (28) duplicates the intended control (19) and (21), so the locked and shape system control objectives will be ac- complished. The advantage of the implementation in (28) is that it robustly preserves the passivity of the closed-loop teleoperator in the presence of the inaccurate force sensing and uncertainty in model parameters, since the structure still remains negative semi-definite.

T h e o r e m 1 (Main resul t ) Consider the teleoperator of the master (1) and the slave (2) with the control law (28). 1. The closed-loop system is energetically passive regardless of force measurement accuracy and model uncertainty, i.e. satisfies (4) (or (18)) despite of inaccurate F 1 , F 2 , M I ( q l ) , and M2 (q2). Furthermore, suppose that IEicf(t)l > fo and (Eicf, qE,/tE) • C Vt > O, so Eicfg(EiCf) -- 1 and p(t) -- 1. Then, 2. (ql, ql) ~ (q~, q~) ~ x p o ~ t i a @ . Al~o, QL -~ ql -~ q~.

3. the target locked system dynamics (7) is achieved.

Proof: 1. Define a new storage function"

1 T (32) 1EMfE2) + qEKpqE t~n(t) -- t~p(t~tl, 1~t2)-~- 2 2 '

where np(/tl,/t2) is the scaled kinetic energy defined in (5) (or (16)). Then, using proposition 1, (8), (10), and (28), it can be shown that

d T . T * -~nn(t) - FTitL + FEqE + X a E ( t ) x

T . < FT~IL + F EqE , (33)

since f ~ ( t ) is negative semi-definite, where X T - -

[/t T,/t T, q T LiCf, EiCf]" Thus, using proposition 2 and the fact that nn(t) > 0, the passivity condition (4) is obtained by integrating the inequality (33) with c 2 - n(0). Since

3281

Invariant Region C (Switching zone)

. . . . . Initial Condit ion 1 Initial Condit ion 2

2 i i i i i i i i ,~"

~.s i i i i i i i ~, i . > . :

° /! i li i i i ~ ~ : : : :

i' t/i ~ ~+,+>'°'i i i i i

o ' ' ~ ' ~ ' ~ ' ~ 0 2 4 6 8 10 12 14 16 18

E k / t )

Fig. 3. ~ - flywheel energy pa th and the invariant region C1 of the two sets of initial conditions. See figure 2 for interpretat ion.

f ~ ( t ) is negative semi-definite regardless of accuracy in F 1 , F 2 , g l ( q l ) , g 2 ( q 2 ) , t h e inequality (33)is preserved, thus passivity is ensured robustly.

1 and p(t) - 1 Vt > 0, the 2. Since g(Eicf(t)) -- E~s(t ) shape system control in the control law in (28) repro- duces the intended control law (21). Thus, by proposi- tion 3, (qE,/IE) --+ (0, 0) exponentially. It implies that (ql , / t l ) --+ (q2,/t2), since qE = ql -- q2 and/ tE = /tl --/t2. Also, from the definition of /tL in (8), /tL --+ /tl,/t2 as /tE --~ 0. 3. With the cancellation of the coupling CLE(q,/t) in (28), the locked system control in the control law (28) duplicates the intended locked system control law (19), thus, the tar- get locked system dynamics (7) is achieved.

R e m a r k 1 (Effect of Flywheel Energy Depletion) The im- plemented controller in (28) preserves the passivity even when ]E2f(t)[ < fo. However, when the energy depletes below the threshold ~Eicf(t)[ < fo), the feedforward cancel- lation in (29) will be turned off (p(t)= 0), so coordination will degrade.

From theorem 1, it is clear that suitable flywheel speed Eicf is imperative to ensure that IEicf(t)l > fo and (Eicf, qE,/tE) C C so that (28) duplicates the intended con- trol (19) and (21) to achieve the perfect coordination (6) and the target dynamics (7). In the following theorem, a way of initializing the flywheel speed is given so that Exf(t)] > fo a n d (E~cf,qE,qE) C C Vt ~ O. Proof is

similar as in [8]

T h e o r e m 2 ( In i t ia l iza t ion of t he f lywheel) Consider th~ td~op~ato~ ~y~t~,n (1)-(2) ~ith th~ control (2S). S~p- pose that we have initialized E[cf(t) s.t.

1 E M f f o 2 + ~Ms~}(0) > 7 2 FTt~ a x

75~ min{V 1(0), ~1}, (34)

and designed the switching region for p(t) to be

p(t) - 1 when (Eicf, qE,/tE) E C - C1 U C2, (35)

Track ing E r ro r - In i t i a l Cond i t i on 1

el i i i i I i i i i I '- .....

Track ing E r ro r - In i t i a l Cond i t i on 2

el ! ! ! ! ! ! ! ! I ' - ..... 3 I- [ i i i i i i i i I ~ L ink2

~'1I- i

5 10 15 20 25 30 35 40 45 50 Time[$ec]

Fig. 4. Coordination performance of the two initial conditions in figure 3.

with definitions (see figure 2)"

C1 - {(E~f , qE,/tE) I 2~Ms [~}(t) - f0 ~] >

C2 - - {(E~f, qE,/tE) I 2~Ms E~}(t) - f0 =] >

2Fma___________~x V 1 (t)} 75~

1 2Fmaz~5}, 75~

where V and F,~a= are in (26) and 5~ > 0 is defined by

q E ) _> 52 IIq~ll = (36) v ( t ) - (q~ / t~)P(t) /tE

w h ~ V(t) i~ i~ (23). Th~, IE~f(t)l (E2f(t), qE(t), itE(t)) C d Vt > O.

> fo, and,

In theorem 2, C1 is invariant, since coordination er- ror (or Lyapunov function V(t) in (23)) converges to zero exponentially and depletion rate of flywheel energy in EM S is proportional to v/V(t) when the feedforward cancellation in (21) is turned on. This implies that when (E&s,qE,/tE) C C1 at t = to, trajectory of the state con- verges to V(t) = 0 faster than the slope of C1 Vt _> to as in figure 2. The region d2 in theorem 2 is also invari- ant, since, when the feedforward cancellation is turned-off, (E&f,QE, qE) should go below the ultimate bound 1?, but the energy in the flywheel E Mf is always increasing by

d [1E f xfJ E M E.2] ~ - 2 f ( t ) . g(EiCf)itTKvitE >_ O, (37)

using (28)and (29)with p ( t ) - O.

R e m a r k 2 Practically, even if the flywheel has not been initialized according to (34), an operator can shake up the teleoperator first so that shaking energy is transferred to the flywheel via the damping as (37) and the state enters d be- fore real manipulation. Hereafter, d will be invariant so the teleoperator will be rigidly coordinated with the feedforward cancellation.

3282

Coordination of Pushing against the Wall -o.14 i i

Directio n of Forcing J -0.145 4 , ~ , ¢ % ~ , ~ j ~ , ~ ~ and Deflection due to |

Z . . . . . ~ . . . . ~ [ End-effect°rCompliance

-0.16

-0.165

. . . . . S . . . . . . . . . . t I FF I i " ~ 1 ~ ' ~ ' ~ ' ~

-0.14 -0.135 -0.13 -0.125 -0.12 -0.115 -0.11

x-Axis[m]

4~ with°utFeedf°rward ~ ! 5 10 15 20 25 30 35 40 45 50

Time[see]

Fig. 5. Coordination while the slave makes a rigid contact with an aluminum wall with or without force feedforward.

VI. E X P E R I M E N T

Exper iments are per formed using the nonlinear teleop- era tor in figure 1 with 2 m s sampling rate. A power scaling p - 15 is assumed so tha t the opera tor perceives a shrinked slave and the slave environment feels the mas te r enlarged.

C o o r d i n a t i o n P e r f o r m a n c e We consider two sets of initial condit ions under the same human posit ion c o m m a n d (4-140 °, 0 .2Hz) . For the f i r s t

cond i t i on , the flywheel energy and coordinat ion error are initially zero. The s ta te s tar t s from the origin in the fig- ure 3 and the flywheel energy increases as the energy from human exci ta t ion is ex t rac ted th rough damping Kv as in (37). The s ta te finally enters into C so tha t feedforward cancellat ion is ac t ivated and good coordinat ion (4-0.5 ° for bo th links) is achieved. However, nonzero coordinat ion er- ror requires more flywheel energy for the s ta te to be total ly confined in C. A sudden degrada t ion in figure 4 (around 24 s) is because the s ta te is not to ta l ly confined in C so the feedforward becomes t emporar i ly deact ivated. From the s e c o n d cond i t i on , the sys tem s tar ts outside of C in figure 3, but it enters into the region C quickly due to the unidi- rect ional energy flow through the damping to the flywheel as in (37). Once the feedforward cancellat ion is tu rned on, good coordinat ion (4-0.5 ° for bo th links) is achieved afterwards.

H a r d C o n t a c t and Force Feedforward The opera tor pushes against an a luminum wall installed in the slave environment with and wi thout the feedforward cancellation. Similar levels of coordinat ion are repor ted in bo th cases (figure 5). However, the mas te r posit ion is off from the slave in an opposite direction of human forc- ing, since the slave end-effect is d e f o r m e d along the forcing direction while the feedforward cancellat ion tries to com- pensa te for react ion force genera ted by the deformat ion (above figure 5). In contrast , the mas te r moves fur ther along direction of the human forcing when the feedforward is not used. As in figure 6, f o rce sca l ing of a round 15 (=p) is achieved. Note from (14) tha t power scaling p becomes force scaling, when mot ion of the te leopera tor is negligible

(i.e. FL = 0 ¢:~ pF1 = - F 2 ) .

x-Axis Master and Slave Forcing

140 , ! ! ! ! ! ~'%~k'*,4"~'~"~ ! ~ I M t F 1 2 o l - : , ' ~ ~ ~ ;" ~ ....... ~;~Fx x lOO . ~ i , I t . . . . . . . . . V

60 l'~ : [: : : ! : [ : :

i~ 204° ,!i: with Feedfor w~ rd : : :If: without Feedfo' ward :

0 5 10 15 2o 25 3o 35 4o 45 5o

y-Axis Master and Slave Forcing 200 i i i i i i i i

I .... , , y ~ . . . . . " ~ , : G y ....

i / i ~i i i i oJ i i i

: :::L i L 5

5 10 15 20 25 30 35 40 45

Time[sec]

Fig. 6. Force scaling (p -- 15) while the slave makes a rigid contact with an aluminum wall with or without force feedforward.

VII . C O N C L U S I O N

Coordinat ion problem for a 2n-DOF nonlinear teleop- era tor consisting of two n-DOF robotic systems is consid- ered. The proposed control law achieves perfect coordina- t ion in the presence of a rb i t ra ry human / envi ronmenta l forcing by feedforward cancellat ion and preserves energetic passivity with a power scaling. In order to enforce ener- getic passivity robustly, the fictitious energy storage is used to genera te the feedforward cancellat ion and the negat ive semi-definite s t ruc ture is utilized for controller implementa- tion. By achieving perfect coordinat ion (n-DOF holonomic constraints) and preserving the passivity, the 2n-DOF tele- opera tor is reduced to the n -DOF passive robotic sys tem interact ing with a human opera tor and environments .

REFERENCES

[1] D. Lee and P. Y. Li. Passive tool dynamics rendering for nonlinear bilateral teleoperated manipulators. In 2002 IEEE International Conf. on Robotics and Automation, 2002.

[2] J. Doubler and D. Childress. An analysis of extended physiolog- ical proprioception as a prosthesis control technique. Journal of Rehabilitation Research and Development, 21(1):5-18, 1984.

[3] B. Hannaford. A design framework for teleoperators with kines- thetic feedback. IEEE transactions on Robotics and Automation, 5(4):426-434, 1989.

[4] Y. Yokokohji and T. Yoshikawa. Bilateral control of master- slave manipulators for ideal kinesthetic coupling - formulation and experiment. IEEE Transactions on Robotics and Automa- tion, 10(5):605-620, 1994.

[5] R. J. Anderson and M. W. Spong. Bilateral control of tele- operators with time delay. IEEE Transactions on Automatic Control, 34(5):494-501, 1989.

[6] J. E. Colgate B. Miller and R. Freeman. Guaranteed stability of haptic systems with nonlinear virtual environments. IEEE Transactions on Robotics and Automation, 16(6):712-719, 2000.

[7] P. Y. Li. Passive control of bilateral teleoperated manipulators. In Proceedings of the 1998 American Control Conference, pages 3838-3842, 1998.

[8] P. Y. Li and D. Lee. Passive feedforward approach to bilateral teleoperated manipulators. In Proceedings of the 2000 A S M E IMECE Symphosium on Haptic Interfaces for Virtual Reality and TeIeoperator Systems, 2000.

[9] R. M. Murray Z. Li and S. S. Sastry. A mathematical introduction to robotic manipulation. CRC, Boca Ranton, FL, 1993.

3283