PASSIVE FEEDFORWARD APPROACH TO BILATERAL TELEOPERATED MANIPULATORS Perry Y. Li † and Dongjun Lee Department of Mechanical Engineering, University of Minnesota, 111 Church St. SE, Minneapolis MN 55455. E-mails: pli,djlee @me.umn.edu ABSTRACT A control scheme for linear dynamically similar bilateral teleoperated manipulator system which ensures that the closed loop system is energetically passive is proposed. Energetic pas- sivity implies that the teleoperated manipulator system is safe to interact with, and that the coupling between the system and any strictly passive environment is stable. The control objective is for the two manipulators in the system to behave in unison under the influence of both the operator and the work environ- ment, while maintaining energetic passivity. The dynamics of the system in unison and its response to the operator and work environment can be specified as kinematic and power scalings. To maintain energetic passivity with feedback and feedforward actions, the proposed control makes use of two fictitious internal energy storages. The result is that when the internal state of the storage elements are suitably initialized, the teleoperated manip- ulator system achieves asymptotic locking even in the presence of external (bounded) forcing from the operator and work envi- ronment. 1 INTRODUCTION A bilateral teleoperated manipulator system interacts with both the work environment and the human operator. To be suc- cessful, the motions of the slave and master manipulators must mimic each other in a manner determined by the kinematic scal- ing. Thus, their motions must be coordinated. The coordination control for a bilateral teleoperator must be designed not to inhibit the effect of the work environment and operator’s forces on the movement of the overall system to obtain kinesthetic coupling in a natural manner. Since a teleoperated manipulator simultaneously interacts with two environments (the human operator, and the work en- RESEARCH SUPPORTED BY NATIONAL SCIENCE FOUNDATION UNDER GRANT CMS-9870013 † Corresponding Author vironment), an a-priori requirement must be that a broad class of environments with which the system interacts do not destabilize the system. It is particularly important when the teleoperator is required to provide force or power amplification / attenuation. If the system is rendered passive with respect to a supply function related to the mechanical power input, the interaction stability with any strictly passive work environment and human operator can be guaranteed using the familiar passivity theorem [1], i.e. the interconnection between a passive and a strictly passive sys- tem is necessarily stable. Our control philosophy for bilateral teleoperator is that the teleoperated system should present itself as a passive mechanical tool to both the human operator and to the work environment. We refer to the tool as the locked mechanical system since it is the system that one gets when the master robot and the slave robot are perfectly coordinated (locked). In [2], a passive control methodology, using kinematic (po- sition and velocity) feedback only, was proposed for linear, dynamically similar bilateral teleoperated manipulator systems. The development of the control law makes use of the desirable property that a pair of n DOF dynamically similar master and slave manipulators (total 2n-DOF) can be decomposed, both dy- namically and energetically, into two n DOF mechanical sys- tems. The decomposed systems represent the coordination and the gross motion aspects respectively. Comparing with other ap- proaches [3, 4], the main contribution of this control law is in that it controls the two aspects of teleoperation separately and achieves passivity of the whole system by passifying the decom- posed systems individually. The passivity of this control law is also based on time domain analysis extensible to general nonlin- ear cases. This control law, which is based on the passive ve- locity field control (PVFC) technique [5, 6, 7], has the following features: 1) It enables the teleoperator to be coordinated accord- ing to a prescribed linear kinematic scaling; 2) It provides for bilateral power amplification / attenuation; 3) It ensures that the closed loop system is energetically passive; 4) The dynamics of
8
Embed
PASSIVE FEEDFORWARD APPROACH TO BILATERAL …lixxx099/papers/LiLeeIMECE.pdf · PASSIVE FEEDFORWARD APPROACH TO BILATERAL TELEOPERATED ... The surprising aspect of the coordinate transformation
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
PASSIVE FEEDFORWARD APPROACH TO BILATERAL TELEOPERATEDMANIPULATORS
�Perry Y. Li †and Dongjun Lee
Department of Mechanical Engineering,University of Minnesota,
111 Church St. SE,Minneapolis MN 55455.
E-mails:�pli,djlee � @me.umn.edu
ABSTRACTA control schemefor linear dynamically similar bilateral
teleoperatedmanipulatorsystemwhich ensuresthat the closedloop systemis energeticallypassive is proposed.Energeticpas-sivity implies that the teleoperatedmanipulatorsystemis safeto interactwith, and that the couplingbetweenthe systemandany strictly passive environmentis stable.Thecontrolobjectiveis for the two manipulatorsin the systemto behave in unisonunderthe influenceof both the operatorandthe work environ-ment, while maintainingenergetic passivity. The dynamicsofthe systemin unisonandits responseto the operatorandworkenvironmentcanbe specifiedaskinematicandpower scalings.To maintainenergetic passivity with feedbackandfeedforwardactions,theproposedcontrolmakesuseof two fictitious internalenergy storages.Theresultis thatwhenthe internalstateof thestorageelementsaresuitablyinitialized, theteleoperatedmanip-ulator systemachievesasymptoticlocking even in the presenceof external(bounded)forcing from theoperatorandwork envi-ronment.
both thework environmentandthehumanoperator. To besuc-cessful,the motionsof the slave andmastermanipulatorsmustmimic eachotherin a mannerdeterminedby thekinematicscal-ing. Thus,their motionsmustbecoordinated.Thecoordinationcontrolfor abilateralteleoperatormustbedesignednotto inhibitthe effect of the work environmentandoperator’s forceson themovementof theoverallsystemto obtainkinestheticcouplinginanaturalmanner.
Since a teleoperatedmanipulatorsimultaneouslyinteractswith two environments(the humanoperator, and the work en-�
RESEARCH SUPPORTED BY NATIONAL SCIENCE FOUNDATIONUNDER GRANT CMS-9870013
†CorrespondingAuthor
vironment),ana-priori requirementmustbethatabroadclassofenvironmentswith which thesysteminteractsdo not destabilizethesystem.It is particularlyimportantwhenthe teleoperatorisrequiredto provide forceor power amplification/ attenuation.Ifthesystemis renderedpassive with respectto a supplyfunctionrelatedto the mechanicalpower input, the interactionstabilitywith anystrictly passive work environmentandhumanoperatorcanbe guaranteedusingthe familiar passivity theorem[1], i.e.theinterconnectionbetweena passive anda strictly passive sys-temis necessarilystable.
Our control philosophy for bilateralteleoperatoris that theteleoperatedsystemshouldpresentitself asapassivemechanicaltool to boththehumanoperatorandto thework environment.Werefer to the tool asthe locked mechanicalsystemsinceit is thesystemthat onegetswhenthe masterrobot andthe slave robotareperfectlycoordinated(locked).
In [2], a passive controlmethodology, usingkinematic(po-sition and velocity) feedbackonly, was proposedfor linear,dynamicallysimilar bilateral teleoperatedmanipulatorsystems.The developmentof the control law makesuseof the desirablepropertythata pair of n � DOF dynamicallysimilar masterandslavemanipulators(total 2n-DOF)canbedecomposed,bothdy-namicallyandenergetically, into two n � DOF mechanicalsys-tems. The decomposedsystemsrepresentthe coordinationandthegrossmotionaspectsrespectively. Comparingwith otherap-proaches[3, 4], the main contribution of this control law is inthat it controlsthe two aspectsof teleoperationseparatelyandachievespassivity of thewholesystemby passifyingthedecom-posedsystemsindividually. The passivity of this control law isalsobasedon timedomainanalysisextensibleto generalnonlin-earcases.This control law, which is basedon the passive ve-locity field control(PVFC)technique[5, 6, 7], hasthefollowingfeatures:1) It enablestheteleoperatorto becoordinatedaccord-ing to a prescribedlinear kinematicscaling; 2) It provides forbilateralpower amplification/ attenuation;3) It ensuresthat theclosedloop systemis energeticallypassive; 4) Thedynamicsof
the locked systemin responseto the operatorandthe environ-mentforcesis asprescribed.
Unfortunately, thecontrolschemein [2] suffersfrom thefactthat the coordinationperformancedegradesin the presenceofunmatchedoperatorandenvironmentforcing. In this paper, weremedythisdrawback.Forcesensorsareusedto measurethehu-manoperatorandthework environmentforces,andthena feed-forward action is designedto compensatefor any mismatchedoperator/ environmentforce.Thechallengeof feedforwardcom-pensationis that it candestroy thepassivity propertyof thesys-tem. This difficulty is overcomeby the useof fictitious energystorages.The kinematicfeedbackportion of the controllerhasalsobeenredesignedto achieveabetterconvergencerate.
Thepaperis organizedasfollows. In section2, thecontrolproblemis formulated.In section3, we review resultsfrom [2]thatfor lineardynamicallysimilarteleoperatedmanipulators,thesystemdynamicscanbedecomposedinto two parts.Thecontrolof thegrossmotion(lockedsystem)is discussedin section4 andthecoordinationcontrolis discussedin section5. For thecoordi-nationcontrol, novel passive feedbackstabilizationandpassiveexternal force compensationschemesareutilized. Experimentresultsarepresentedin Section6. Section7 containssomecon-cludingremarks.
2 PROBLEM FORMULATION2.1 PLANT
Considera linear teleoperatedmanipulatorsystemconsist-ing of two n � degreeof freedommanipulatorswith dynamicsgivenby:
M1q1 � T1 � F1 � M2q2 � T2 � F2 (1)
whereT1 � T2 � ℜn are the control forces,F1 � F2 � ℜn are theenvironmentforcesthatthemasterandslave systemsencounter,andM1 andM2 � ℜn n aretheinertiamatricesfor themanipu-lators.
Let α � ℜn n bethedesiredbijective linearkinematicscal-ing sothatideally, wewould like
αq1 t � � q2 t �sothatthesystemachievestheperfectcoordination(locking).
As in [8], weassumethattheslaveandmastermanipulatorsaredynamically similar with respect to α in thesensethatthereexistsascalarζ � 0, s.t.
ζM1 � αTM2α Any oneDOF linearmechanicalsystem,suchastheexperimentsetupin Figure1, satisfiesthis property.
SLAVE
MASTER
Figure 1. Teleoperator with Two Single-DOF Planar Manipulators in
MIML. The master manipulator is equipped with force sensor to measure
human force.
2.2 PASSIVITY WITH POWER SCALINGTo ensurethat both the humanand the work environment
caninteractwith theteleoperatedmanipulatorin stablefashions,we requirethatthetwo port teleoperatorsystembeenergeticallypassive. In addition,we arealso interestedin the teleoperatorsystemproviding power scalingρ. An appropriatedefinitionofthesupplyrateis:
sρ q1 � q2 � F1 � F2 � � ρFT1 q1 � FT
2 q2 (2)
whichis thesumof thepowerexertedby theforceF2 andρ timesof thepower exertedby theforceF1. We requirethattheclosedloop controlledteleoperatedsystembe passive with respecttothis supplyrate,i.e.� t
for somec � ℜ which would dependon the initial conditionatt � 0. Notice that sinceρ � 0, if the two port systemis ener-geticallypassive with respectto thesupplyratein (2), it is alsoenergetically passive asoneport systems(i.e. whenoneof theportsis openwith F2 � 0 or F1 � 0) in theusualsense:� t
0FT
1 q1dt ��� c21; whenF2 � 0� t
0FT
2 q2dt ��� c22; whenF1 � 0
Thesupplyratedefinedin (2) which incorporatespower scalinginspiresthe following definitionof scaledkinetic energy for the
teleoperatedmanipulatorsystem:
κρ q1 � q2 � � ρ2
qT1 M1q1 � 1
2qT
2 M2q2 � 0 (4)
Theobjectiveof this work is to designthecontrolactionT1
andT2 so that the teleoperatorsystemis coordinated,the grossmotionmimicssomedesireddynamics,andat thesametime theenergeticpassivity propertytowardsthehumanoperatorandtheenvironmentis preserved.
3 SHAPE AND LOCKED SYSTEM DECOMPOSITIONIn this section,we decomposethedynamicsof theteleoper-
atorsysteminto two systemsconcernedwith thecoordinationofthetwo manipulatorsandthegrossmotionof thesystem,respec-tively. It turnsoutthatfor lineardynamicallysimilarteleoperatedmanipulators,thetwo resultingsystemscanbeindividually con-trolled, andas long asthe individual controller is energeticallypassive, the combinedteleoperatorsystemis alsoenergeticallypassive.
Webegin by defining
E � ζρ � ζ �αq1 � q2 �
to be thecoordinationerror. Now considertheconstantcoordi-natetransformationof thevelocity space(i.e. tangentbundle)togetbothdynamicalandenergeticdecomposition:�
VL
E � � ζρ � ζ
� ρζ α IαI � I �� ��� �S
�q1
q2 � (5)
Notice that when the teleoperatoris perfectly coordinated,i.e.αq1 � q2, thenVL � q2 � αq1. Similarly, considerthecompat-ible transformationof theforces:�
TL
TE � � S � T
�ρT1
T2 � � �FL
FE � � S � T
�ρF1
F2 � (6)
Under thesetransformations,the dynamicsof the teleoperatorsystem(1) block diagonalizesinto:
MLVL � TL � FL � ML � ζ � ρζ
M2 (7)
and
MEE � TE � FE � ME � ρ ζ � ρ �ζ2 M2 (8)
Wemayconsider(8) asan-DOFmechanicalsystemwith config-urationcoordinatesE, inertiaME, controlinputTE andenviron-mentforceFE; and(7) asaseparaten � DOFmechanicalsystemwith velocity VL, inertiaML, controlTL andenvironmentforceFL. We shall referto (8) astheshapesystemsinceit determinestherelativeconfiguration(coordination)of thetwo manipulators.Similarly, (7) will bereferredto asthe lockedsystemsinceit de-terminesVL, which is theaveragevelocity (grossmotion)of thetwo manipulators.
The surprisingaspectof the coordinatetransformation(5)is that thescaledkinetic energy of 2n-DOF teleoperatorsystemin Eq.(4)canbe written asa sumof the kinetic energiesof theshapeandof the lockedsystems.:
Proposition 1 If the locked systemin (7) and the shapesystemin (8) are individually controlled usingTL and TE respectively,such that each systemis individually passive: i.e. � cL � cs s.t.�
FL � FE, and�t � 0,� t
0FT
L VLdτ ��� c2L � � t
0FT
EEdτ ��� c2s � (10)
then, the teleoperator system(1) is passivewith respectto thesupplyratesρ q1 � q2 � F1 � F2 � in (2) (i.e. (3) is satisfied).
Proof: Noticethat
sρ � q1 q2 F1 F2 !#" FTL VL $ FT
EE
wheres� q1 q2 F1 F2 ! is thescaledsupplyratedefinedin (2), Thus,wehave
ddt
κρ � q1 q2 !#" sρ � q1 q2 F1 F2 !which givesriseto thepassivity inequalityin (3) on integration.%
Thus,thecontrolof the2n � DOF system(1) reducesto theindependentcontrolof thetwo n-DOF lockedandshapesystems,(7)-(8), while maintainingtheir individual passivity properties.Theobjective for theshapesystemis to regulate E � E � at 0 � 0� ;while theobjective for the lockedsystemis to mimic thedesiredtarget lockedsystemdynamicsgivenasbelow.
4 LOCKED SYSTEM CONTROLAs mentionedearlier, ourcontrolphilosophy is thatthetele-
operatedsystemshouldappearto bea n � DOF passive mechan-ical tool with which both the work environment and the hu-manoperatorcommonlyinteract. The mechanicaltool, whichis achievedwhenthemasterandslave robotsareperfectlycoor-dinated,is referredto asthe lockedsystem.
Supposethat the two manipulatorsin the systemare per-fectly coordinated,we cantake thecoordinateof the lockedsys-tem,qL � ℜn to beeitherq1 or q2. If wetakethecoordinateto beqL � q2, wewould liketheresultinglockedsystem(expressedinthe units anddimensionof robot 2) to behave accordingto thetarget lockedsystemdynamicsasgivenby:
MLqL � CL qL � qL � qL � ρα � TF1 � F2 (11)
whereρ � 0 is the desiredpower scalingto amplify / attenuatetheoperator’s forceandpower, andML, asdefinedin (7), is theapparentinertia thatappearsto theenvironmentof robot2. Then & n skew symmetricmatrix
CL qL � γqL � � � CL qL � γqL � T � γCL qL � qL � �specifiesthe unforceddynamicsof the target locked systembyformally defininga connection[9] of the targetsystem.For ex-ample,thegeodesicsof theconnectioncanbeusedto prescribethe preferreddirection of travel for the target system. The re-quirementthatCL qL � qL � shouldbeskew symmetricis to ensurethat thetarget lockedsystemis a passive systemwith respecttoa supplyrate that respectsthe desiredpower scaling. For pathguidanceapplications,CL qL � qL � maybedesignedusingpassivevelocity field control techniqueasappliedto contourfollowingproblems[6].
Notice from (6) that FL � ρα � TF1 � F2 which is exactlythesecondtermon theRHSof (11), thetargetsystemdynamics.Moreover, when E � E � �� 0 � 0� , thenVL � αq1 � q2. Therefore,if we definethe lockedconfigurationof thetwo manipulators(intherobot2 unitsanddimensions)to be
qL � ζζ � ρ ' ρζ αq1 � q2 (
thenapossiblecontrolfor the lockedsystemis:
TL � � CL qL � VL � VL � (12)
which duplicatesthetarget lockedsystemdynamics(11). In ad-dition, if indeedE t �*) 0, then the dynamicsof robot 2 alsoconvergesto (11).
Since CL q2 � q2 � is skew-symmetric, by considering12VT
L t � MLVL t � to be a storagefunction candidate,the lockedsystem(7) underthecontrol(12) caneasilybeshown to bepas-sive in thesenseof (10).
5 SHAPE SYSTEM CONTROLTheshapesystemcontrolshouldmake themasterandslave
whereTE is the control torqueand FE is the transformeddis-turbanceforces from the humanoperatorand the work envi-ronment. We wish to designa control law for TE so that theshapesystemdynamicsis passivewith respectto thesupplyratesE FE � E � � FT
EE, andalsothatthecoordinationerrorE ) 0.The controllerstructurebelow ensuresthat passivity is en-
forced. First, we augmentthesystemwith two fictitious energystorageswhich areto beimplementedwithin thecontroller. Oneis associatedwith thepotentialenergy in a fictitious springwithapositivedefinitespringconstantK � ℜn n. Theotheris associ-atedwith thekinetic energy of a fictitious flywheelwith dynam-ics
-.andthetotal energy of theaugmentedsystemis givenas:
κE E � E � xf � � 12
ETMEE � 12
ETKE � 12
M f x2f (13)
Considerthecontrollaw of theform:
' TE
T f( � ' � S f b t �0� K � U f b t �
UTf b t � 0 0 (213 E
Exf
45� p xf � E � E � ' � S f f t � 0 � U f f t �
UTf f t � 0 0 (213 E
Exf
45(14)
wherep xf � E � E � is aswitchingfunctionto turnon/off thefeed-forwardaction,S f b t � andU f b t � arepositivesemi-definitepos-itive matricesto give dampingeffect in feedbackcontrol, andS f f t � � � ST
f f t � andU f f t � areresponsiblefor generatingfeed-forward cancellationof mismatchedhuman and environmentbasedon forcemeasurement.Thesematricesaredefinedbelow.
If S f b t � andU f b t � aredefineds.t.
S f b t � E � U f b t � xf � BE �thena constantdampingeffect is achieved in feedback,whereB � ℜn n is astrictly positivedefinitedampingmatrix. Onepos-sibility is to define:
U f b t � � g xf t ��� BE t � � S f b t � �6 1 � g xf t ��� xf t ��� B (15)
where
g xf � �879: 9;1x f < xf < � f01f0
sign xf � 0 =� < xf <?> f01f0 < xf < � 0
(16)
and f0 � 0 is a thresholdon theflywheelspeedxf to ensurethatU f b t � is bounded.
Using only the feedbackaction cannotensurethat E ) 0in the presenceof mismatchedoperator/ environment forceFE. If FE is completelycanceled,thenthe dampingeffect willcauseE ) 0 exponentially, so we assumethat the teleoperatoris equippedwith forcesensorssothatFE canbecomputedfrom(6) and canceledout with a feedforward action. However, anordinaryfeedforwardactioncanleadto destroying thepassivity.
To achieve passivefeedforward, the matricesS f f t � andU f f t � aredesignedsothat,� � S f f t �0� U f f t �
UTf f t � 0 � � E
xf � � p xf � E � E �A@ � FE1x f
FTEE B (17)
whereS f f t � � ℜn n is a skew symmetricmatrix to ensurethatthefeedforwardactionpreservesthepassivity, andp xf � E � E � isaswitchingfunction:
p xf � E � E � �DC 1 if xf � E � E � �FE0 otherwise
where EHG � xf � E � E � � ℜ2nI 1 � ,asin Figure3, is an invariantregionhaving some”good” convergencepropertieswith feedfor-ward action. The region E will be determinedlater. Typically
xf � E � E � would belongto E if < xf < is sufficiently large,and/ ortheerror E � E � aresufficiently small. This preventsthefeedfor-wardportionof thecontrol law (14) from becomingunbounded.When xf � E � E � is indeedin E , then the feedforward action isactivatedandtheenvironment/ operatorforceFE is exactlycan-celedout.
With both the feedbackand feedforward control (14), thecoordinationerror(shapesystem)dynamics(8) become:
13 ME 0 00 K 00 0 M f
45 13 EExf
45 �J13 � S f b t �0� K � U f b t �K 0 0
U f b t � T 0 0
45 13 EExf
45� p xf � E � E � 13 � S f f t � 0 � U f f t �
0 0 0UT
f f t � 0 0
45 13 EExf
45 � 13 FE
00
45(18)
This controllerdoesindeedpreserve passivity of theshapesys-tem even when it usesboth feedbackand feedforward control(14)because,
ddt
κE K t LNM FTE O E PRQ ET ET xf SATUWV S f b V S f f V K V U f b V U f f
K 0 0UT
f b P UTf f 0 0 XY TU E
Exf XYM FT
E O E V ETS f bE Z sK FE [ E LwhereκE E t � � E t � � xf t ��� denotesthetotalenergy functionde-finedin (13)evaluatedat time t. Utilizing thefactthatκE t �\� 0,thepassivity propertyof theshapesystemunderthecontrol(14)is obtainedon integration.
We now analyzetheshapesystemcontrolpresentedabove,anddeterminetheinvariantregion E .
Proposition 2 Without feedforward cancellation,theshapesys-tem variables E t � and E t � are ultimately bounded withboundedenvironment/ operator forceFE. Moreover, theenergyin thefictitiousflywheel,12M f x2
To seethat the kinetic energy of the flywheel is non-decreasing,observe from theshapesystemdynamicswith thecontrollaw that
ddt ] 12M f x
2f ^ " xf g � xf ! ETBE _ 0 ` %
Unlikeregulardamping,thedampingeffect in thecontrolleris at leastpartially implementedusinga flywheel. Theextentofthis dependson thecurrentvaluexf . Thus,insteadof a damper
With Feedforward
Cancellation
Environments
Shape
System
Fictitious
Flywheel
via FictitiousDamping
Without Feedforward
Cancellation
Environments
Shape
System
Flywheel
Fictitious
Damping
Untimate Bound
via Fictitious
Figure 2. Energy flow with or without feedforward cancellation in the
augmented shape system. With feedforward cancellation, energy flows
between the environments and the fictitious flywheel without affecting the
real shape system. Without feedforward cancellation, energy flows only
into the fictitious flywheel to increase flywheel energy monotonically so
that the system enters into the invariant region E .
simply dissipatingtheenergy to theenvironment,theenergy de-velopedin thedamperis storedin theflywheel. Thestoredfly-wheelenergy will be usedto provide the feedforward cancella-tion of the mismatchedoperator/ environmentforce FE with-out violating energeticpassivity. Energy flow in theaugmentedshapesystemwith or without feedforward cancellationis illus-tratedin Figure2.
Remark 5.1 We computetheultimateboundusinga Lyapunovfunction. For the exponentiallystablesystem(19), we can de-finepositivedefinematricesP � ℜ2n 2n andQ � ℜ2n 2n, andaLyapunov function,
V t � � 12 a ET ET b P � E
E � (20)
sothat for someγ � 0 andζ � 0,
V t � � � a ET ET b Q �EE � � a ET ET b P � 0
In n � FE> � γ V t � � ζV12 t �dc FE c (21)
where γ is theexponentialconvergencerate(which maybeesti-matedfromγ � σ eQ f
σ e P f � 0, whereσ �hg � andσ �ig � denotetheminimumand maximumsingular valuesof their argumentsrespectively).From(21), theultimateboundfor theLyapunov functionis foundto be
V � ' ζγ Fmax( 2
(22)
where c FE t �dc > Fmax. Therefore, if V12 0� > ζ
γ Fmax, then
V12 t � > ζ
γ Fmax for all t � 0. Otherwise, for anyV12 0� , andfor
anyε � 0, thereexistsT � 0, sothatV12 t � > ζ
γ Fmax � ε whenevert � T.
Definefirst a subregion E 1 GjEkG ℜ2nI 1 in Figure3 to be:
E 1 : � � xf � E � E � <V 12 > γ δ
2Fmax ' 12M f � x2f � f 2
0 � ( � � (23)
where V t � is the Lyapunov function (20) and f0 � 0 is thethresholdin (16).
Proposition 3 Let xf � E � E � �lE 1 at t � t1, thenwecanensurethat xf � E � E � �mE 1
�t � t1 with thefeedforward cancellationi.e.
p xf � E � E � � 1 in (14).
Proof: Supposefor amomentthat � xf E E !onqp 1 andp � xf E E !W" 1at t " t1, thentheshapesystemdynamicsis givenby:
MEE $ BE $ KE " 0
becausethefeedforwardterm(17) is ableto exactly cancelout FE. Us-ing thesameLyapunov functionin Remark5.1,
V � t !r" 12 s ET ET t P u E
E v _ δ2 w E w 2for someδ x 0, sothat
V � t !#"ly s ET ET t Q u EE vkz y γ V � t ! `
Continuingwith the suppositionthat p � xf E E !{" 1 andconsidertheenergy storedin theflywheel,we have
ddt
M f s xft 2
2 " Tf xf " UTf bExf $ FT
EE" xf g � xf ! ETBE $ FTEE _ y w FE
w|w E w _ y Fmax
δV
12 � t !
Therefore,by integratingtheabove relation,we canensurethat} s ET ET t P2u E
E v " V � t ! 12 z γ δ
2Fmax ] 12M f ~ xf � t ! 2 y f 20 �i^ �
t _ t1, sinceV � t ! z e� γtV � 0! .%Define f1 to bethethresholdfor theflywheelenergy s.t.:
γ δ2Fmax ' 12M f � f 2
1 � f 20 ��(�� V
12 � ε � ζ
γFmax � ε
whereε � 0 is asmallnumber. Therationalefor thedefinitionoff1 is that theboundaryof E 1 intersectswith theultimatebound
Kf(f0) Kf(f1)Figure 3. � V t � versus K f xf t ��� � 1
2M f x2f where V t � is the Lya-
punov function in (20), K f xf � is the kinetic energy in the flywheel. Also
shown are 4 sample paths of K f xf t ��� � � V t ��� . In Paths 1 and 2,
V t ��) 0, and in Path 3, V t � becomes ultimately bounded by V . In
Path 4, V t ��) 0 but it is not guaranteed.
in Remark5.1whenxf � f1. Theregions E , E 1, andtherelation-shipsto thethresholdf1, andtheultimateboundV areillustratedin Figure3.
We arenow readyto definetheinvariantregion E , which isusedto define(17), to beE : ��E 1 � � xf � E � E � < xf � f1 �� (24)
Theorem 1 Considertheshapesystem(8) underthecontrollerstructure, the dampingeffectsgiven by (15),(16), and feedfor-ward effectsprovidedby (17) with theswitching region E givenin (24).
1. Theclosedloop shapesystemis passivewith respectto thesupplyratesE FE � E � � FT
EE;2. Supposethat c FE t �dc > Fmax
�t � 0. For any initial condi-
tion xf 0� � E 0� � E 0��� , theLyapunov functionV t � in (20)will beultimatelyboundedbyV in (22).
3. Supposeagain that c FE t �dc > Fmax for all t � 0. Thentheregion E definedin (24) is invariant.
4. If furthermore, xf 0� � E 0� � E 0��� ��E , then E t ��) 0,E t ��) 0, andxf t ��� f0
�t � 0.
5. If xf 0�*=� 0 andV t ��=) 0, thenE 0��) 0.
Proof:
1. Passivity hasalsoalreadybeendemonstratedusingthetotalenergyκE � t ! in (13)asstoragefunctioncandidate.
2. Notice that the feedforward actioneithercancelsor leavesalonetheenvironment/ operatorforceFE. Thustheeffective boundonFE doesnot increaseover the casewhenonly dampingeffect isused.ThusProposition2 andRemark5.1apply.
3. Propositionin 3 showsthatthesubregion p 1 is invariant.If theini-tial statebelongsto p but doesnotbelongto p 1, then1) xf � t ! _ f1
for all t _ 0; 2) eventually,V � t ! z V $ ε. The1stresultis dueto thefact that the flywheel energy is non-decreasingwhenthe feedfor-wardtermis not used(Prop. 2). The2ndresultis dueto ultimateboundedness.Becauseof theway that f1 is defined,thestatemusteventuallyenterp 1 andremainsthere.
4. Since eventually, the state enters p 1, FE is canceled out,E � t !� E � t !\� 0 exponentially. The definition of p 1 ensuresthatxf � t ! will notbedepletedbelow f1.
5. If V � t !��� 0, thenthestatemustnothaveenteredp . FromProposi-tion 2, this meansthatV � t ! would beultimatelybounded,andtheenergy in the flywheel would be non-decreasing.Sincethe statedoesnot enter p , xf is boundedby f1 andso xf � t ! mustconverge.This impliesthatE � 0. %Roughlyspeaking,theshapesystemcontrolpresentedabove
consistsof spring/ dampertermfor stabilization,anda feedfor-ward termto cancelout theeffect of FE. The innovationof thecontrollerlies in that fact that it makesuseof theenergy storedin thefictitiousflywheelto ensurethatcontrolsystemis energet-ically passive.
In Theorem1, we canonly show that E ) 0 if the systemstateenterstheregion E . Otherwise,E wouldonly beultimatelybounded.In reality, however, thestateis likely to eventuallytoenter E or E ) 0 and E ) 0 if the shapesystemis constantlybeingexcitedby timevaryingmismatchedenvironment/ humanforce FE t � . The reasonis that if FE t � excitesthe systemper-petually, it is not likely that E ) 0 which by Theorem1 item 5is a necessityfor E =) 0 or E =) 0. Thus,anoperatorcanshakeup theteleoperatorfirst in orderto causethestateto enter E be-fore using the teleoperatorfor manipulation. Hereafter, if themeasurementof FE is accurate,then E will beinvariant,andtheteleoperatorwill behaveexactly like a lockedsystem.
6 EXPERIMENTAL RESULTSExperimentsare performedon the teleoperatedsystemas
shown in Figure1. The mastermanipulatoris equippedwith aforcesensorandexcitedby humanandanexternalenvironment.Both kinematicandpower scalingsaresetto be1 for simplicity.With differentpower scaling,thehumanoperatorfeelsdifferentinertia of the tool representedby the locked system. We con-sidertwo setsof initial conditions:1) zeroflywheelenergy andcoordinationerror(from origin in Figure4); 2) outsideof thein-variantregion E with nonzeroflywheelenergy andcoordinationerror. Thetrajectoriesrepresentingenergy flow and E areshownin Figure4 andtheactualangle(position)trajectoriesareshownin Figure5 for thetwo initial conditionsrespectively.
With the first initial condition, the humanoperatorshouldexcite the systemto generatesufficient energy so that the sys-tementersinto E , andfeedforwardcancellationis activated.Be-fore thesystementersinto E , thefeedbackpartis usedalone,sothe coordinationperformanceis poor (with error around5 deg)
0 5 10 15 20 25 30 35 40 45 500
1
2
3
4
5
6
7
8
9
10
Kinetic Energy in Flywheel Kf(t)
Squ
are
Roo
t of L
yapu
nov
Func
tion
V(t)
Square Root of Lyapunov Function V(t) vs Kinetic Energy in Flywheel Kf(t)
Initial Condition 1:from Origin Initial Condition 2:from Outside C
Switching Line
Invariant Region C
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.80
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
Switching Line
Invariant Region C
Enlarged Diagram of Initial Condition 1
Figure 4. Trajectories for two sets of initial conditions traced out in the� V t � - flywheel energy diagram and the invariant region E . See figure
3 for interpretation. Result of the first initial condition is enlarged and
shown separately. Note the ripples after entering E with the first condition
due to real implementation.
dueto mismatchedoperator/environmentforce. Whenthe sys-tem getsenoughenergy from humanoperator, the feedforwardcancellationis turn on andnearperfectcoordination(with errorlessthan0 5 deg) is achieved.
Thesecondinitial conditionhasnonzeroinitial flywheelen-ergy andcoordinationerror. Thesystemstartsoutsideof E , butit entersinto theinvariantregion E quickly (after0 2second) andachievesthenearperfectcoordinationafterthat.Thisquicklock-ing is mainlycausedby theenergyflow via thefictitiousdampingasin Figure2.
Thebandwidthof the feedforwardcancellationdegradesinreal implementation.The ripples in E in Figure4 with the fistinitial conditionshowstheeffectsof noise,discretizationandac-tuator limitation. The ultimateboundis too large to be shownin Figure 4. This large ultimate bound, which representstheguaranteedcoordinationperformanceof feedbackcontrolalone,highlightstheimportanceof feedforwardcancellation.
namicallysimilar teleoperatorsystems.Thecontrol law coordi-natesthe two robotsin the systemandensuresthat the closedloop systemis energeticallypassive. Thecontrol law makesuseof fictitious energy storagesto generatebothfeedbackandfeed-forward control baseduponboth kinematicandforce measure-ments. By the useof feedforward compensation,the teleoper-atedsystembecomesperfectlycoordinateddespitethepresenceof mismatchin environment/ operatorforce. Experimentalim-plementationvalidatesthe proposedcontrol scheme.Although
2 3 4 5 6 7 8 9 10−75
−50
−25
0
25
50
75
100
125
Time[sec]
Pos
ition
[deg
]
Slave Master
2 3 4 5 6 7 8 9 10−10
0
10
20
30
40
50
60
70Master & Slave Position [deg]
Time[sec]
Pos
ition
[deg
]
Slave Master
Initial Condition 1 : Zero Initial Energy & Zero Initial Error
Initial Condition 2 : from Outside of Invariant Region C
Figure 5. Real angle trajectories of master and slave manipulators with
two initial conditions. Note that the system achieves locking after getting
energy from environments (human operator) in the first initial condition.
theideaof decompositioninto shapeandlockedsystemdoesnotapply to genericnonlinearteleoperatorsystems,the main con-ceptof passive feedbackandfeedforwardcontrol shouldbeap-plicable.
REFERENCES[1] M. Vidyasagar. Analysisof nonlineardynamicsystems. Prentice