Hybrid Stiff/Compliant Workspace Control for Robotized Minimally Invasive Surgery

Transcription

1 204 5th IEEE RAS & EMBS Intenational Confeence on Biomedical Robotics and Biomechatonics (BioRob) August 2-5, 204. São Paulo, Bazil Hybid Stiff/Compliant Wokspace Contol fo Robotized Minimally Invasive Sugey Pål Johan Fom, Jang Ho Cho, Andes Robetsson, Tomohio Nakano, Mahdi Ghazaei, and Rolf Johansson Abstact This pape pesents a novel contol achitectue fo hybid stiff and compliant contol fo minimally invasive sugey which satisfies the constaints of zeo lateal velocity at the enty point fo seial manipulatos. Fo minimally invasive sugey it is uied that thee is no sideways motion at the point whee the obots ente the abdomen. This is necessay to avoid any damage to the patient s body when the obot moves. We solve this at a kinematic level, i.e., we find a Jacobian matix that maps the velocities in joint space to the end-effecto velocities and at the same time guaantees that cetain velocities at the enty point ae zeo. Because the new velocity vaiables ae defined in the end-effecto wokspace we can use these fo hybid motion/foce contol. The appoach is veified expeimentally by implementing hybid stiff and compliant contol of the end effecto and we show that the insetion point constaints ae always satisfied. I. INTRODUCTION Minimally invasive telesugical systems allow fo sugical pocedues to be pefomed with less patient tauma and isk, in addition to shote patient ecovey times. This is achieved by inseting the sugical instuments into the body though small insetion points called tocas, often togethe with a camea fo visual feedback. Such systems thus povide a safe envionment fo sugeons to pefom telesugey, eithe in-house o emotely though a communication channel []. Robot-assisted telesugical systems also allow fo moe dexteous sugical pocedues than classic lapaoscopic sugey in addition to enhanced oveall pefomance, fo example by scaling and filteing out hand temo [2]. When inseting the obotic tool into the patient it is cucial to avoid any lateal motion. This can be obtained in one of thee ways: i) The emote cente of motion (RCM) can be obtained mechanically by using a paallel device that keeps the RCM fixed, such as in the DaVinci obot fom Inuitive Sugical []. This is a safe solution, but not flexible when it comes to changing the RCM duing opeation. ii) The RCM can also be implemented using two passive joints that fom a univesal joint togethe with fou active joints that geneates the desied 4-DoF motion. iii) Finally a softwaebased RCM can be implemented by contolling the obot so This wok was funded by the Swedish Reseach Council though the Linnaeus Cente LCCC, the Industial Stategic technology development pogam (#00468) funded by the Ministy of Tade Industy and Enegy (MI, South Koea), and the Nowegian Reseach Council. J. H. Cho, A. Robetsson, T. Nakano, M. Ghazaei, and R. Johansson ae with the Depatment of Automatic Contol, Lund Univesity, PO Box 8, SE Lund, Sweden. jangho@contol.lth.se P. J. Fom is with the Depatment of Mathematical Sciences and Technology, Nowegian Univesity of Life Sciences, Postbox 5003, 432 Ås, Noway. Duing this wok he was visiting the Depatment of Automatic Contol, Lund Univesity. that the lateal motion at the toca is eliminated. This is the appoach discussed in this pape. Because the sugical instuments ae located vey close to the patient s intenal ogans, tissues, and bones, a compliant behavio at the end effecto is desiable to avoid inflicting damage on the patient. On the othe hand, if the sugeon is to pefom inteaction tasks such as cutting and sutuing, a compliant behavio is not appopiate, at least fo some diections of the end-effecto wokspace. Fo cutting pocedues, fo example, the sugeon should be able to move the knife in the cutting diection and also apply foces in this diection. In othe diections such as the diections othogonal to the motion, one might opt fo a guided motion and compliant behavio in ode to keep the sugeon on the ight tajectoy, o to potect othe intenal ogans. These uiements call fo a hybid contol with stiff motion contol in some diections of the end-effecto wokspace and compliant behavio in othe diections. Such a contol scheme is only possible when the state space is witten in tems of the end-effecto vaiables, which is not staight fowad when constaints ae pesent in the kinematic chain. Hybid contol in the end-effecto space has been studied in detail by many authos [3]. Mason [4] intoduced natual constaints which coespond to the degees of feedom whee the envionment imposes position o foce constaints on the end-effecto motion and atificial constaint which ae the constaints imposed by the contolle. Because the natual constaints ae othogonal to the atificial constaints, the end-effecto space can be divided into two subspaces whee only the latte can be contolled. Fo an appopiate choice of efeence fame, selection matices can be defined fo hybid position/foce contol [5], [6], [7]. A simila but moe geometic appoach was pesented in Lipkin and Duffy [8] whee the concept of ecipocity is used to define the two subspaces. In the setting of minimally invasive sugey Deal and Newman [9], [0] pesent a method fo stiff contol at the enty point and compliant contol at the end effecto. A combination of hybid foce/position contol and Natual Admittance Contol (NAC) is used to satisfy the potal constaints and at the same time allows fo compliant behavio at the end-effecto. The esulting contolle divides the contol effots into a 2-DoF stiff contol at the enty point and a 4- DoF NAC contolle at the end effecto. The appoach thus allows fo a stiff enty point and a compliant behavio at the end effecto. The appoach can not, howeve, be diectly extended to also allow fo hybid contol of the 6-DoF endeffecto motion, as all the degees of feedom at the end /6/4/$ IEEE 345

2 effecto will have a compliant behavio. In this pape we popose a novel appoach which in addition to satisfy the enty point constaints also allows fo hybid contol of the end-effecto, i.e., some diections can take on a compliant behavio while othes ae stiff/position contolled. The constaints imposed by the enty point ae often efeed to as the Remote Cente of Motion (RCM). Seveal eseaches have addessed the poblem of imposing the RCM constaints on the obot motion by modifying the contolle. Ealy esults solved the motion constaints as an optimization poblem, fo example in Funda et al. [] and Li et al. [2]. In Otmaie and Hizinge [3] the RCM kinematics is deived and used to estimate the position of the enty point fo a obot with passive joints. The passive joints guaantee that no foces ae exeted to the enty point. In Locke and Patel [4] the kinematic model is used to deive an optimization technique that allows isotopy of the sugical tool to be evaluated subject to the RCM constaint. Toca kinematics is also discussed in Lenačič and Galletti [5]. Azimian et al. [6] uses the concept of task pioity and esticted Jacobian to deive the constained motion in tems of the toca and manipulato geomety. The end-effecto motion is found in the standad way fom the manipulato Jacobian, which is taken fom the null space of the constaint Jacobian of the enty point. The constaint Jacobian is found in the nomal way by the mapping fom the joint space velocities to the lateal linea velocities of the RCM point. The constaints at the insetion point ae given fist pioity and the end-effecto motion is given a seconday pioity as this is taken fom the null space of the fist Jacobian [7]. The appoach depends on the kinematics of both the obotic manipulato and the toca. In this pape we take a somewhat diffeent appoach in that we impose the constaints on the velocity twist of the last link of the obot diectly and ewite the mapping fom the endeffecto twists to the joint velocities so that it is guaanteed to satisfy the RCM constaints. The main advantage of the poposed appoach is that the constained system can be teated in the same way as a standad unconstained manipulato simply by eplacing the standad Jacobian with the constained Jacobian, and we can theefoe apply any conventional contol scheme used fo unconstained obots. In othe wods, when it comes to contol thee is no diffeence between constained and unconstained obotic manipulatos, except fo the Jacobian. We can fo example fomulate the contol poblem in the end-effecto space using standad contol laws, including hybid contol, and map these to the joint velocities, which ae effectuated in the nomal way by the low-level obot contolle. We thus obtain a fomulation that is independent of the obot kinematics and allows fo simple implementation as we can use existing lowlevel contolles both fo the obot and the end-effecto. II. SYSTEM OVERVIEW AND PROBLEM FORMULATION The system discussed in this pape consists of a standad o customized 6-DoF obotic manipulato with a shaft, i.e., a thin long link used fo inseting the end effecto into the body a b Wist Fame - F w Motion space: S 2 R S Robot Fame - F Motion space: SE(3) Insetion Point Fame - F p Constaints: R 2 Motion space: S 2 R S End-effecto Fame - F e Motion space: S 2 R S S 2 Fig.. The obotic setup discussed in this pape. The shaft is inseted into the body though a toca at F p. The motion spaces of the diffeent fames ae subgoups of SE(3) defined by linea motion R, cicula motion S, and the sphee S 2. though the toca. At the end of the shaft thee is a wist with two o moe additional degees of feedom and a tool. At the insetion point we will uie that the sideways velocities ae eliminated to pevent the obot fom damaging the patient s tissue. This uiement imposes a 2-DoF constaint on the shaft so that the end of the shaft has 4 DoF of motion. The additional degees of feedom in the wist give the end effecto a full 6-DoF motion. Most endoscopic wists, such as the one used in the Intuitive Sugical s da Vinci obots, have 3 degees of feedom. This pape is not concened with edundancy, so we conside the case whee the wist has 2 DoF. The system setup and the configuation spaces used in this pape ae shown in Fig.. The poblem consideed consists of maintaining a stiff contol of zeo velocity at the insetion point while allowing fo a combination of stiff and compliant contol at the end effecto. Seveal sugical tasks uie both position and foce contol in the diffeent diections of the endeffecto coodinate fame. Othe tasks do not uie the specifications of all the 6 DoF of the configuation space so the emaining diections ae often given a compliant behavio to pevent the tool fom damaging tissues o ogans. This uies a well defined wokspace epesentation that can be used to divide the wokspace into suitable othogonal o ecipocal spaces. In this pape we popose a new appoach whee the insetion point constaints ae taken cae of at a kinematic level and satisfied by defining a constained Jacobian matix that guaantees that the constaints ae satisfied, independently of the commanded maste efeence. The Jacobian gives the mapping fom the joint space to the end-effecto space on which hybid position/foce contol can be applied in the nomal way. III. CONSTRAINED KINEMATICS In this section we deive the kinematics of the obotic system when the kinematic constaints at the enty point ae satisfied. l 7 l 8 346

3 A. End-effecto Motions We will attach a fame F to the last link of the 6- DoF obotic manipulato, as illustated in Fig.. The body velocities of this fame with espect to a fixed inetial fame F 0 is epesented by 0 = [ v x v y v z ω x ω y ω z] T. () The obot velocities can be found fom the joint velocities by the Jacobian in the standad way as V0 B = JB (q ) q whee J B(q ) is the geometic Jacobian elating the joint velocities and the body twist of the last link of the obot. One of the contol objectives is to maintain zeo tanslational velocity at the enty point. We will thus also define a efeence fame F p at this point with body velocity twist 0p = [ v p x v p y v p z ω p x ω p y ω p z] T. (2) The efeence fames ae illustated in Fig.. The elation between the velocity at the last link of the obot and the enty point, i.e., the point p p = [ 0 0 a ] T in fame F, is given by the simple elation v p x v x ω x v y p = v p y + ω y 0 0 a +aωy = v y aω x (3) while the angula velocities ae identical: ω B 0p = ωb 0. If we assume that the uiement of zeo velocity at the insetion point is satisfied, fo example by a simple position contol law o a physical constaint, we see fom Equation (3) and the popeties of igid bodies that the velocity at this point can be witten in tems of the velocities at F as 0p = [ 0 0 v z ω x ω y ω z] T. (4) At the end of the shaft we attach the wist fame F w. The wist fame has only fou degees of feedom and can thus be witten in tems of the velocities at the last obot link (o altenatively the enty point) as w 0 0 b 0 w 0 b 0 0 0w = w ωx w ω w y ω w z = ω x ω y ω z. (5) Finally the velocity of the end-effecto fame F e is found by adding the velocity of the wist fame with espect to the inetial fame to the velocity of the end effecto with espect to the wist fame [8]: 0e = Ad gew 0w + we. (6) To simplify the expessions we assume that the last two joints otate about the x-axis and we set l 8 = 0. The body velocity at the end effecto then becomes [9] bωy +l 7cosq 7 ωy +l 7sinq 7 ω z sinq 78 (bcosq 78 +l 7 cosq 8 )ωx l 7 cosq 8 q 7 V0e B = cosq 78 +(bsinq 78 +l 7 sinq 8 )ωx +l 7sinq 8 q 7 ωx + q 7 + q 8 cosq 78 ωy +sinq 78 sinq 78 ωy +cosq 78 (7) whee we have used that R ew = R e and q 78 = q 7 +q 8. Fo most telesugical systems the wist is close to a spheical joint so we can assume that l 7,l 8 << b and we get bω y sinq 78 bcosq 78 ω x V0e B cosq 78 +bsinq 78ωx ωx + q 7 + q 8. (8) cosq 78 ωy +sinq 78 sinq 78 ωy +cosq 78 B. Constained Jacobian Matix With the fomulation above we have assumed that the lateal velocity at the enty hole is zeo. To guaantee this, we need a design that satisfies this uiement and these vaiables theefoe need to be included in the state space epesentation. The state space can then be witten as a vecto in : v p = [ v p x v p y v p z ω p x ω p y ω p z q 7 q 8 ] T. (9) This choice of state vaiables is vey useful when contolling the velocity at the enty point (the two fist vaiables) to zeo. It is also convenient because it can be found diectly fom the obot kinematics (fist 6 vaiables) and the end-effecto kinematics (last 2 vaiables), i.e., v = [ v x v y v z ω x ω y ω z q 7 q 8 ] T (0) and we can find Equation (9) diectly fom Equation (3). On the othe hand, the end-effecto velocities witten in this way ae not paticulaly useful because these ae not the velocities that we want to contol. A moe appopiate choice of state vaiables fo ou poblem is theefoe found as [ ] v v e p = p V0e B = [ T. p p e e e ωx e ωy e ] e () This epesentation of the velocity vecto is suitable fo both stiff contol at the enty point and hybid contol of the end effecto. The velocity vecto v e elates to the velocities v by v e = J e v. (2) Note that this is a mapping fom one epesentation using body velocities to anothe epesentation also in body velocities. The coodinate tansfomation matix J e can be found as in (3). 347

4 p a p 0 0 a e (a+b)+l 7 cosq 7 l 7 sinq e e = 0 cosq 78 sinq 78 (a+b)cosq 78 l 7 cosq l 7 cosq sinq 78 cosq 78 (a+b)sinq 78 +l 7 sinq l 7 sinq 8 0 ωx e e cosq 78 sinq e sinq 78 cosq ωx ωy q7 q 8 (3) The mapping between the wokspace velocities and the joint velocities ae found by the geometic Jacobian J B : [ ] [ ][ ] v p p Je,a 0 V0e B = 2 2 V B 0p J e, J e,2 q w [ ][ ][ ] Je,a 0 = 2 2 J B 0 q J e, J e,2 0 I q w [ ][ ] Je,a J = B q J e, J B. (4) J e,2 q w We will denote this matix J and wite v e = J q. (5) C. Minimal Repesentation with Insetion Point Constaints Fom (3) we see that the velocities at the enty point can be witten in tems of the obot velocities as v p x = v x +aω y (6) v p y = v y aω x (7) and the constaint of zeo velocity can theefoe be cast into the following simple fom v x = aω y (8) v y = aω x (9) whee we need to know the distance fom the last link of the obot to the enty point. We can incopoate these constaints in the kinematics (3) by intoducing new vaiables v and v 2 such that v x = v ω y = a v (20) v y = v 2 ω x = a v 2. (2) Substituting this into (3) and eliminating the enty point velocities that ae now known to be zeo gives Equation (22). This is suitable fo wokspace contol and at the same time guaantees that the enty point velocity constaints ae satisfied. In the contolle v and v 2 ae ealized though the expessions found in Equations (20) and (2). We will denote the matix in Equation (22) that gives us the minimum epesentation of the end-effecto wokspace as Je m and this impotant tansfomation as V0e B = Jev m m. 0e,d J q d + ė Contolle u Robot Fig. 2. One example of how the Constaint Jacobian J can be used to obtain both contol objectives. Note that the contolle can be implemented at joint level in the nomal way. IV. HYBRID STIFF AND COMPLIANT CONTROL SCHEMES In the pevious section we found a state space epesentation well suited fo implementing a hybid contol scheme in the end-effecto space. We obtained this by the oneto-one mapping in (22) which gives the constaint obot motion fom the desied end-effecto motion by imposing the enty-point constaints. In this section we will study diffeent appoaches that can be used to obtain the uied chaacteistics at the end-effecto fo which the enty-point constaints ae satisfied. Common fo all the fomulations is that we define the task specifications in the new wokspace vaiables, and use the tansfomations deived in the pevious section to obtain the coesponding joint motions. Ou objective is to allow fo both compliant and stiff contol when the end-effecto is in contact with the envionment. At the same time stiff contol is uied at the enty point, which is solved at a kinematic level, i.e., though the Constaint Jacobian. Following the seminal wok of Mason [4] and Caig and Raibet [5] we will define othogonal wokspaces fo position and foce contol of the end effecto. Mason [4] epesented physical constaints by zeo velocity and zeo foce in cetain diections of the end-effecto wokspace and denoted these as natual constaints. Then the atificial constaints, o contol, wee defined subject to a cetain contol objective so that the natual constaints wee always satisfied. The physical and atificial constaints ae epesented in tems of selection matices S p and S f whee, fo a suitable choice of state space, we can choose S p as a diagonal matix epesenting the diections with fee motion, and thus foce cannot be applied, and S f as epesenting the diections with constained motion, which allow fo foce contol. A. Position Contol We will fist look at a simple position contol, i.e., we want the end effecto to follow a maste manipulato, in addition to satisfying the constaints at the insetion point. Stiff contol q 348

5 e v e a (b+l 7cosq 7 ) 0 0 l 7 sinq v y 0 e a (bcosq 78 +l 7 cosq 8 ) sinq 78 0 l 7 cosq 8 0 ωx e = 0 a (bsinq v l 7 sinq 8 ) cosq 78 0 l 7 sinq 8 0 v z 0 e a 0 0 e a cosq ω. (22) z sinq q7 a sinq cosq q 8 of this kind is necessay in many applications whee the end effecto is to follow a efeence path as closely as possible. The appoach descibed hee will also seve as a benchmak fo the appoach pesented in the next section. With this appoach we do not want to contol the inteaction foces, but athe foce the end-effecto to follow the position dictated by the opeato. If natual constaints ae pesent, this will be communicated to the opeato though foce feedback. Position contol thus fits into the famewok descibed above by choosing S p = I and S f = 0. The contol law can also be deived in joint space as shown in Fig. 2. Fist wite the manipulato dynamics as M q (q) q +C q (q, q) q = τ. (23) Hee M q is the obot inetia matix, C q (q, q) epesent the Coiolis and centipetal foces, τ is the joint toques, and M q (q) = J T MJ ( ) C q (q, q) = J T C MJ J J whee M and C epesent the dynamics in v e. In this case we use the Jacobian that we found in (5) to obtain the dynamics in joint vaiables. An invese dynamics contol law is then given by whee we choose y as τ = M q (q)y (24) y = q d +K D ( q d q)+k P (q d q) (25) which guaantees that the eo conveges to zeo [20]. Note that this contol law is a kind of computed-toque contol method. We see that the Constaint Jacobian J allows us to efomulate the contol poblem into a standad contol law in joint vaiables which guaantees that the insetion point constaints ae satisfied (solved at a kinematic level) and that the maste efeence is followed (solved by the contolle (24-25)). A joint-space impedance contol can also appea in the wist contol with a modification of Eqs. (24-25). Intoduce the contol law τ = M q (q)y +J T F F M q(q)f (26) whee JF T is the Jacobian with espect to the foce-sensing position. In addition to the computed-toque tem, thee is one foce-compensating tem and one tem that povides impedance contol with a computed-toque-like impedance dynamics in joint space: M q (q)( q +K D q +KP q F) = 0. (27) Passivity can be shown fo the mapping fom F to q whee q = q d q. B. Impedance Contol with Insetion Point Constaints Impedance contol fo minimally invasive sugey is challenging because the impedance contol needs to be implemented in the end-effecto space while the constaints on the obot motion needs to be so that the velocities at the insetion point ae zeo. One solution to this poblem is shown in Fig. 3. The desied motion is given by the maste velocitiesv0e,d B. Fo impedance contol we define a compliant famef c which gives the position and oientation of the end effecto when it is in contact with the envionment, i.e., the deviation fom the desied fame F d due to the sensed end-effecto foces [3]. When the end effecto is in contact with the envionment it will thus follow the fame F c which elates to the desied tajectoy fame F d by M c p dc +D c ṗ dc +K c p dc = F e (28) which gives a new desied motion epesented by the fame F c wheneve the obot is in contact with the envionment. We also need to guaantee that the velocities at the insetion point ae zeo. This is guaanteed by intoducing the vaiables v and v 2 as in Equation (22). The matix (Je m) thus gives us the motion of the manipulato am fo which the constaints ae satisfied. This is given by the fou degees of feedom velocity vecto v = [ T. v v 2 ] The six degees of feedom motion of the manipulato am is thus found by v ωx = v 2 0 a 0 0 v. (29) z a We now give this as input to the obot am, togethe with the wist motion, also found by the invese of (22), and we sepaate the feedback loops fo the manipulato am and the wist, as shown in Fig. 3. We note that we have obtained compliant contol in the end-effecto wokspace and we also guaantee that the insetion point constaints ae satisfied, as uied. If hybid contol is desied, we intoduce selection matices S p and S f in the nomal way in the hybid impedance contolle in Fig. 3. Note also that we only use the velocity vaiables in the contolle. This is not a poblem in teleopeation, as the position vaiables ae nomally compensated fo by the 349

6 F e v v 2 0e J C v v 2 ωx = v z a v 2 a v ė Robot am contol τ Robot am 0e 0e,d Impedance V0e,c B contol (J m e) v m p v p ] [ q7 q 8 ė w R 2 Wist contol τ w R 2 Wist τ F Fig. 3. Hybid contol scheme with insetion point constaints taken cae of at a kinematic level. opeato, and we ae mainly inteested in following the velocity efeence. In the impedance contolle, howeve, we need both the acceleation and position vaiables. We theefoe need to include a memoy in the impedance contolle so that the position can be ecoveed wheneve sping foces ae uied. V. EXPERIMENTS In this section, the poposed contol scheme is tested expeimentally. To evaluate the applicability of the poposed method to obotic telesugey, a teleopeation system is implemeted. Fo simplicity, we conside the wist as the end effecto which means the wist does not have any DoF. A. Expeimental Set-up The expeimental set-up consists of a maste device and a slave obot. The Omega 7 haptic device fom Foce Dimension which is a paallel haptic device is used as the maste device. An ABB IRB40 industial obot with a foce/toque senso at the tip, the 00M40 fom JR3, is used as the slave obot. The poposed contol scheme is implemented by MAT- LAB Simulink and Real-Time Wokshop. Input and output signals ae eceived and sent though Ethenet fom/to maste and slave devices. The contolle takes velocities and positions of the maste device and foce signals of the slave obot as inputs and geneates desied joint velocities of the slave obot as output signals. The slave obot is contolled by its intenal contolle to follow the desied motions of each joint, details can be found in [2]. B. Expeimental Results Seveal expeiments wee pefomed including a stiff position contol and an impedance contol. In Fig. 4, an image is ceated by ovelaying the motions of the slave obot duing the expeiments. The insetion point is vitually imposed in the middle of end effecto and it can be ecognized in Fig. 4. Fo clea illustation of the velocity constaints on the insetion point, we daw the taces of the end-effecto of the expeiment in Fig. 5. The colo of the end-effecto changes fom gay to gayish ed-violet with time. It is clealy shown Fig. 4. Image ovelay of the slave obot duing the expeiment. that the poposed contol scheme satisfies the zeo-velocity constaints on the vitual insetion point. When noise is emoved, the lateal motions that we measue at this point is zeo, as expected. We have also conducted expeiments with impedance contol. Fo clea illustation of the impedance contol, the impedance contolle is implemented along with the insetion diection which is the z-axis in obot fame and the inputs of the contol scheme, V0e,d B, ae also tansfomed to obot fame F. To see the compliance along the z-axis, positions of the maste device and the slave obot ae dawn in Fig. 6. The contact along the z-axis has a duation of about 29s. As a esult of the impedance contol, we see that the desied velocities of the slave obot ae modified, as expected. It is notewothy that the impedance contolle is implemented in pioi to the kinematic constaints as descibed in Fig. 3 which enables us to estict the velocities at the insetion point. VI. CONCLUSION In this pape, we popose a novel contol achitectue fo hybid stiff and compliant contol fo minimally invasive sugey which satisfies the constaints of zeo lateal velocity at the enty point. These constaints ae handled on a 350

7 400 Z Positions (mm) Y Positions (mm) X Positions (mm) Fig. 5. Taces of the end effecto duing the expeiment, illustated in Catesian space coodinate. Positions (mm) time (sec) Fig. 6. Z-axis positions of the maste device and the slave obot in obot fame F. Note that compliant motions ae allowed though the poposed contol scheme. kinematic level by a Jacobian matix that maps the velocities in joint space to the end-effecto velocities and at the same time guaantees that the velocities at the enty point ae zeo. Both stiff position contol and hybid stiff/compliant contol can be easily implemented in the end-effecto wokspace. The poposed appoach is veified though expeimental veification via stiff position contol and hybid contol and we show that the insetion point constaints ae satisfied in both cases. x m x s [8] H. Lipkin and J. Duffy, Hybid twist and wench contol fo a obotic manipulato, Jounal of Mechanisms, Tansmissions and Automation in Design, vol. 0, pp , 988. [9] A. Deal, D. L. Chow, and W. Newman, Hybid natual admittance contol fo lapaoscopic sugey, in Poc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, Vilamoua, Algave, Potugal, pp , 202. [0] W. S. Newman and Y. Zhang, Stable inteaction contol and coulomb fiction compensation using natual admittance contol, Jounal of Robotic Systems, vol., no., pp. 3, 994. [] J. Funda, R. Taylo, B. Eldidge, S. Gomoy, and K. Guben, Constained catesian motion contol fo teleopeated sugical obots, Robotics and Automation, IEEE Tansactions on, vol. 2, no. 3, pp , 996. [2] M. Li, A. Kapoo, and R. Taylo, A constained optimization appoach to vitual fixtues, in IEEE/RSJ Int. Conf. onintelligent Robots and Systems, Albeta, Canada, pp , [3] T. Otmaie and G. Hizinge, Catesian contol issues fo minimally invasive obot sugey, in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, Takamatsu, Japan, vol., pp vol., [4] R. Locke and R. Patel, Optimal emote cente-of-motion location fo obotics-assisted minimally-invasive sugey, in IEEE Int. Conf. on Robotics and Automation, Rome, Italy, pp , [5] J. Lenacic and C. Galletti, Kinematics and modelling of a system fo obotic sugey, in On Advances in Robot Kinematics, Spinge, [6] H. Azimian, R. Patel, and M. Naish, On constained manipulation in obotics-assisted minimally invasive sugey, in IEEE RAS and EMBS Int. Conf. on Biomedical Robotics and Biomechatonics, Tokyo, Japan, pp , 200. [7] Y. Nakamua, Advanced obotics: edundancy and optimization. Addison-Wesley seies in electical and compute engineeing: Contol engineeing, Addison-Wesley Longman, Incopoated, 99. [8] P. J. Fom, K. Y. Pettesen, and J. T. Gavdahl., Vehicle-manipulato systems - modeling fo simulation, analysis, and contol. London, UK: Spinge Velag, 203. [9] P. J. Fom, On the kinematics of obotic-assisted minimally invasive sugey, Modeling, Identification and Contol, vol. 34, no. 2, pp , 203. [20] L. Sciavicco and B. Siciliano, Modelling and Contol of Robot Manipulatos. Spinge, [2] A. Blomdell, I. Dessle, K. Nilsson, and A. Robetsson, Flexible application development and high-pefomance motion contol based on extenal sensing and econfiguation of ABB industial obot contolles, in Poc. IEEE Int. Conf. on Robotics and Automation, St. Paul, MN, USA, 200. REFERENCES [] G. Guthat and J. Salisbuy J, The intuitive TM telesugey system: oveview and application, in Poc. IEEE Int. Conf. Rob. Aut., pp , [2] D. Nio, R. Balm, S. Maatense, M. Guijt, and W. Bemelman, The efficacy of obot-assisted vesus conventional lapaoscopic vascula anastomoses in an expeimental model, Eu. J. Vasc. Endovasc., vol. 27, no. 3, pp , [3] C. Natale, Inteaction Contol of Robot Manipulatos: Six-degeesof-feedom Tasks. Spinge Tacts in Advanced Robotics, Spinge, [4] M. T. Mason, Compliance and foce contol fo compute contolled manipulatos, IEEE Tans. Systems, Man and Cybenetics, vol., pp , june 98. [5] J. Caig and M. Raibet, A systematic method of hybid position/foce contol of a manipulato, in Poc. Compute Softwae and Applications Confeence,, pp , 979. [6] A. Abbati-Maescotti, C. Bonivento, and C. Melchioi, On the invaiance of the hybid position/foce contol, Jounal of Intelligent and Robotic Systems, vol. 3, no. 4, pp , 990. [7] H. Buyninckx and J. De Schutte, Specification of foce-contolled actions in the task fame fomalism -a synthesis, IEEE Tans. Robototics, vol. 2, pp , aug