On the Kinematics of Robotic-assisted Minimally Invasive Surgery

Transcription

1 , Vol. 34, No. 2, 203, , ISSN On Kinmatics of Robotic-assistd Minimally Invasiv Sugy Pål Johan Fom Datmnt of Mamatical Scincs and Tchnology, Nogian Univsity of Lif Scincs, 432 Ås, Noay. Abstact Minimally invasiv sugy is chaactizd by instion of sugical instumnts into human body though small instion oints calld tocas, as oosd to on sugy hich quis substantial cutting of skin and tissu to giv sugon dict accss to oating aa. To avoid damag to skin and tissu, zo latal vlocity at instion oint is cucial. Enting human body though tocas in this ay thus adds constaints to obot kinmatics and nd-ffcto vlocitis cannot b found fom joint vlocitis using siml lation givn by standad Jacobian matix. W fo div a n Jacobian matix hich givs lation btn joint vaiabls and ndffcto vlocitis and at sam tim guaants that vlocity constaints at instion oint a alays satisfid. W dnot this n Jacobian Rmot Cnt of Motion Jacobian Matix (RCM Jacobian). Th main contibution of this a is that addss oblm at a kinmatic lvl and that though RCM Jacobian can guaant that instion oint constaints a satisfid hich again allos fo contoll to b imlmntd in nd-ffcto oksac. y liminating kinmatic constaints fom contol loo can div contol la in nd-ffcto sac and a fo abl to aly Catsian contol schms such as comliant o hybid contol. Kyods: Minimally Invasiv Sugy, Robotic-assistd Minimally Invasiv Sugy, Robot Kinmatics, Constaind Jacobian Matics, Rmot Cnt of Motion. Intoduction Th us of obots fo sugical ocdus has gon into on of most omising alications of obotic tchnology in halth ca. On of asons fo this is us of obots in minimally invasiv sugy (MIS), hich is chaactizd by instion of tools though small hols in atint s body. MIS lads to lss atint tauma, shot covy tims and lo ovall isk comad to convntional on sugy. Robotic-assistd minimally invasiv sugy (RAMIS) is fomd ith a obotic maniulato hos nd ffcto is attachd to a long and thin shaft usd to ntat skin though a toca. To avoid damaging atints tissus at instion oint, it is vital that latal dislacmnts at this oint is kt to a minimal. This adds a kinmatic constaint to stuctu, oftn fd to as Rmot Cnt of Motion (RCM). Ths kinmatic constaints can b imlmntd i mchanically o in softa. Mchanical constaints can b imosd by dsigning a aalll kinmatic stuctu fo hich shaft ivots about RCM (Lock and Patl, 2007; Sun and Yung, 2007; Hannafod t al., 203). Altnativly, kinmatic constaints can b imlmntd in contoll. Fo standad on-chain maniulatos s constaints cannot b addd though mchanical dsign, so only ay to guaant that constaints a satisfid is though contoll. Th main advantag of this doi:0.473/mic c 203 Nogian Socity of Automatic Contol

2 aoach is mo flxibility hn it coms to changing RCM quickly, and it also allos obot to follo motion of a moving RCM, fo xaml if atint is bathing. Th main disadvantag is that additional safty systms nd to b imlmntd to guaant that RCM constaints a not violatd in cas of systm bakdon. In both cass, h constaint is assiv o activ, maing fom joint sac to Catsian sac is ndd fo contol uoss. Sval sachs hav addssd oblm of imosing RCM constaints on obot motion by modifying contoll. Ealy sults solvd motion constaints as an otimization oblm, fo xaml in Funda t al. (996) and Li t al. (2005). In Otmai and Hizing (2000) RCM kinmatics is divd and usd to stimat osition of nty oint fo a obot ith assiv joints. Th assiv joints guaant that no focs a xtd to nty oint. In Lock and Patl (2007) kinmatic modl is usd to div an otimization tchniqu that allos isotoy of sugical tool to b valuatd subjct to RCM constaint. Toca kinmatics is also discussd in Lnačič and Galltti (2004). Azimian t al. (200) and Azimian (202) us conct of task ioity and stictd Jacobian to div constaind motion in tms of toca and maniulato gomty. Th nd-ffcto motion is found in standad ay fom maniulato Jacobian, hich is takn fom null sac of constaint Jacobian of nty oint. Th constaint Jacobian is found in nomal ay by maing fom joint sac vlocitis to latal lina vlocitis of RCM oint. Th constaints at instion oint a givn fist ioity and nd-ffcto motion is givn a sconday ioity as this is takn fom null sac of fist Jacobian (Nakamua, 99). Th aoach dnds on kinmatics of both obotic maniulato and toca. In this a tak a somhat diffnt aoach in that imos constaints on vlocity tist of last link of obot dictly and it maing fom nd-ffcto tists to obot tists so that it is guaantd to satisfy RCM constaints. Th contol oblm is thus fomulatd in nd-ffcto sac and vlocitis a n mad to obot tist, hich is ffctuatd in nomal ay by lo-lvl obot contoll. W thus obtain a fomulation that is indndnt of obot kinmatics hich allos fo siml imlmntation as can us xisting lo-lvl contolls both fo obot and ist/nd-ffcto. It is ll knon that location of toca and os of obot a citical factos hn it coms otimizing maniulability, dxtity, achability, and visibility of obot Sun and Yung (2007). As s factos can significantly nhanc ovall fomanc of systm, it is dsiabl to obtain a systm in hich toca location and obot os can b chosn activly both bfo and duing sugy. W fo ot fo an analytical aoach, as oosd to null-sac aoach discussd abov in od to b abl to contol s factos mo activly. Intaction contol duing obotic sugy is a citical ocdu du to isk of damaging atint s tissu and ogans. Th comlxity of sugical oations oftn quis hybid contol schms alying a combination of motion and foc contol to allo fo both stiff o dict foc contol in dictions associatd ith task and comliant and indict foc contol in o dictions. This ill sult in an fficint and at sam tim a saf contol schm ll suitd fo obotic sugy. Th oosd contol schm is dvlod so that contol la can b imlmntd in Catsian sac. This allos fo fficint imlmntation of hybid contol schms (Cho t al., 202) hil instion oint constaints a takn ca of by modifid maniulato Jacobian dnotd Rmot Cnt of Motion Jacobian Matix (RCMJ). W a fo abl to aly standad aoachs fo Catsian contol usd fo convntional obotic maniulatos found in litatu. Th a is oganizd as follos: A systm ovvi of obotic systm discussd and oblm fomulation a sntd in Sction 2. In Sction 3 kinmatics of systm subjct to Rmot Cnt of Motion constaints a divd. Th dynamics of systm is discussd bifly in Sction 4 and som commnts gading contol oblm a sntd in Sction 5. Concluding maks a sntd in Sction 6. 2 Systm Ovvi and Poblm Fomulation Th systm discussd in this a consists of a standad o customizd 6-DoF obotic maniulato ith a shaft, i.., a thin long link usd fo insting ndffcto into body though toca. At nd of shaft is a ist ith to o mo additional dgs of fdom and a tool. At instion oint ill qui that sidays vlocitis a liminatd to vnt obot fom damaging atint s tissu. This quimnt imoss a 2-DoF constaint on shaft so that nd of shaft has 4 DoF of motion. Th additional dgs of fdom in 70

3 P.J. Fom, On Kinmatics of Robotic-assistd Minimally Invasiv Sugy ist giv nd ffcto a full 6-DoF motion. Most ndoscoic ists, such as on usd in Intuitiv Sugical s da Vinci obots, hav to o mo additional dgs of fdom. This a is not concnd ith dundancy, so consid cas h ist has 2 DoF. Th systm stu and configuation sacs usd in this a a shon in Figu. Th oblm considd in this a consists of diving kinmatics of obotic systm subjct to kinmatic constaints at nty oint. This can n b usd to obtain a stiff contol of zo vlocity at instion oint hil alloing fo a combination of stiff and comliant contol at nd-ffcto. This kind of Catsian contol schms qui stat sac to b ittn in tms of nd-ffcto vaiabls and kinmatic constaints nd to b liminatd fom contol la. In this a thus ndavo to convy a maing fom nd-ffcto sac, in hich high-lvl contoll is divd, to joint sac of lo-lvl contol, hich satisfis Rmot Cnt of Motion constaints. 3 Constaint Kinmatics Th main objctiv of this sction is to find a oksac sntation suitd fo contolling nd-ffcto motion and hich at sam tim guaants zo latal vlocity at instion oint. To obtain a unifid aoach satisfying both contol objctivs ill qui both vlocitis at instion oint as ll as a ll-dfind st of nd-ffcto vlocitis to b includd in stat sac fomulation. W ill s that du to kinmatic constaints at instion oint, it is not staight foad to find a stat sac sntation suitd fo contol. caus maing fom joint vaiabls to nd-ffcto sac also nds to tak constaints into account cannot us Jacobian in standad ay. W ill fo intoduc n vlocity vaiabls that do not hav a staight foad gomtic inttation, but allo us to includ constaints in Jacobian and find a maing fom joint vlocity sac to constaind nd-ffcto vlocity sac. 3. Notation W ill dnot a maing fom intial fam F 0 to a moving fam F a by homognous tansfomation matix R0a (t) g 0a (t) 0a (t) R 4 4 () 0 h R 0a is otation matix that givs ointation of F a ith sct to F 0, and 0a is vcto fom F 0 to F a. Th vlocity of F a ith sct to F 0 is givn in body coodinats as a tist ] [ˆω ˆV 0a 0a v0a g 0 0a ġ0a R 4 4 (2) h ˆω 0a R 3 3 is sk-symmtic sntation of angula vlocitis ω0a R 3 and v0a R 3 is lina vlocity. ˆV 0a R 4 4 is thus matix sntation of an lmnt of Li algba. Th vcto sntation of sam tist is dnotd V0a. Fo a thid fam F b that lats to F a though homognous tansfomation matix g ab can find its motion ith sct to intial fam F 0 by g 0b g 0a g ab. Th body vlocity V0b of fam F b ith sct to F 0 is givn by V 0b Ad gba V 0a + V ab (3) h Ad gba is Adjoint ma givn by Rba ˆ Ad gba ba R ba 6. (4) 0 R ba W f to Fom t al. (203) fo mo dtails on this toic. 3.2 Robot Am Motions W ill attach a fam F to last link of 6-DoF obotic maniulato, as illustatd in Figu. Th body vlocitis of this fam ith sct to a fixd intial fam F 0 is sntd by V 0 [ v x v y v z ω x ω y ω z] T. (5) Th obot vlocitis can b found fom joint vlocitis by Jacobian in standad ay as V0 J (q ) q h J (q ) is gomtic Jacobian lating joint vlocitis and body tist of nd ffcto. On of contol objctivs is to maintain zo tanslational vlocity at nty oint. W ill thus also dfin a fnc fam F at this oint ith sam ointation as F, i.., R I. W dnot body vlocitis of this fam as V 0 [ v x v y v z ω x ω y ω z] T. (6) Th fnc fams a illustatd in Figu. Th lation btn vlocity at last link of obot and nty oint, i.., oint T 0 0 a in fam F is givn by siml lation v x v x ω x 0 v y v y + ω y 0 a + aωy v y aωx (7) 7

4 Robot Fam - F Motion sac: SE(3) a b x y z Instion Point Fam - F Constaints: R 2 Motion sac: S 2 R S End-ffcto Fam - F Motion sac: S 2 R S S 2 y x l 8 x Wist Fam - F Motion sac: S 2 R S l 7 z y z Figu Figu : : Systm stu stufofo Robotic-assistd Minimally Invasiv Sugy. Th configuation sacs at nt fams a a shon in intms of oflina motion R and otational motion S. diffnt hilw also angula attach vlocitis a ist fam a idntical: F at ω nd of 0 ω0. W shaft, also i.., attach at a ist that is fam locatd F insid at nd body, of and shaft, dnot i.., body at vlocitis as V0 [ that is locatd insid body, and dnot body vlocitis ] vy vas z ωx ωy T. (8) Similaly V0 vvlocitis x vy vat z ωx ndω yffcto ω T z is. givn (8) by adding motion of shaft and last joints of Similaly nd ffcto vlocitis and a at dnotd nd ffcto is givn by adding motion V0 [ of shaft and last vy ωx ωy ] T joints of nd ffcto and a dnotd. (9) 3.3 Constaind 0 [ vy End-ffcto ωx ωy Motion ] T. (9) W can study nd-ffcto motions und assumtion that instion oint constaints a satis- 3.3 Constaind End-ffcto Motion Wfid. caninstudy this sction nd-ffcto ill div motions nd-ffcto und assumtion assuming that instion oint oint constaints a a satis- sat- mofidisfid, In this andsction in nxt illsction div ill nd-ffcto constainmo- tion nd-ffcto assuming vlocity instion by including oint constaints instion a oint sat- isfid, constaints. and Thnxt sults sction in this sction ill constain a thus usful nd-ffcto hn analyzing vlocity andby diving including contol las instion fo mchanically constaind Th sults maniulatos in this sction such a as thusdausful Vinci oint constaints. hn andanalyzing Ravn sugical and diving obots (??). contol las fo mchanically constaind maniulatos such as da Vinci 72 4 V andth Ravn instion sugical oint obots constaints (Sun and canyung, b satisfid 2007; i Hannafod though t aal., contol 203). la, fo xaml a siml osition contol la, o by a mchanical dvic. In any cas vlocity Th instion at instion oint constaints oint can can b b ittn satisfid in tms i though of vlocitis a contol at la, F as fo xaml a siml osition contol la, o by a mchanical dvic. 0 In any cas vlocity at instion voint y can 0b ittn in tms of vlocitis at V0 F ωx as ωx. (0) ωy vy ωx ωy ωy 0 At nd of shaft attach ist fam F. Th ist fam has only fou dgs 0 of fdom and can thus b ittn V in tms of vlocitis at 0 v z last obot link (o altnativly ω. (0) xnty oint) as ω y 0 0 b ω 0 vy z 0 b 0 0 v V0 z ωx ωx ω. () ωy y At nd of shaft 0 attach 0 0 ist ω z fam F. Th ist fam has only 0 fou 0 dgs 0 of fdom and can thus b ittn in tms of vlocitis at

5 P.J. Fom, On Kinmatics of Robotic-assistd Minimally Invasiv Sugy last obot link (o altnativly nty oint) as V 0 vy ωx ω y ω z 0 0 b 0 0 b ωx ω y ω z. () h b is distanc fom toca to nd of shaft locatd insid body. Finally vlocity of nd-ffcto fam F is found by adding vlocity of nd ffcto ith sct to ist fam to vlocity of ist fam ith sct to intial fam, h both nd to b xssd in nd-ffcto fam, as ant body vlocitis: V 0 Ad g V 0 + V. (2) To simlify xssions assum that last to joints volut about x-axis and st l 8 0. W fist it hich givs g 0 cq 78 sq 78 l 7 sin q 7 0 sq 78 cq 78 l 7 cos q 7 (3) l 7 cq 7 l 7 sq 7 R T ˆ 0 cq 78 sq 78 l 7 cq sq 78 cq 78 l 7 sq l 7 cos q 7 l 7 sin q 7 l 7 cos q (4) l 7 sin q h hav usd shot hand notations s and c fo sin and cos sctivly, and that l 7 cos q 7 cos q 78 + l 7 sin q 7 sin q 78 l 7 cos (q 7 + q 8 q 7 ) and l 7 cos q 8 (5) l 7 cos q 7 sin q 78 +l 7 sin q 7 cos q 78 (l 7 sin q 78 cos q 7 l 7 cos q 78 sin q 7 ) l 7 sin (q 7 + q 8 q 7 ) l 7 sin q 8. W can no it R Ad g Ad g T R T ˆ 0 R T l 7 cq 7 l 7 sq 7 0 cq 78 sq 78 l 7 cq sq 78 cq 78 l 7 sq cq 78 sq sq 78 cq 78 (6) Th contibution of last to links of ist can b found staight foad by obsving that (call that assum l 8 0) q 8 only contibuts to angula motion and q 7 contibuts to angula vlocity in sam ay as q 8 and lina vlocity hich nds to tak into account ointation of last link. W can find this by body gomtic Jacobian: J m,g (q) Ad g7 X7 7 Ad g8 X l 7 cos q 8 0 l 7 sin q 8 0. (7) H X7 7 and X8 8 a constant vlocity tists of joint 7 and 8, sctivly, as sn fom joint fams (Fom t al., 203). Th body vlocity at nd ffcto is n givn by Equation (9) on nxt ag h q 78 q 7 +q 8. Fo most tlsugical systms ist is clos to a shical joint so can assum that l 7, l 8 << b and gt bω y sin q 78 b cos q 78 ω x V0 cos q 78 + b sin q 78 ω x ωx + q 7 + q 8. (20) cos q 78 ωy + sin q 78 sin q 78 ωy + cos q Comlt Stat Sac Rsntation In vious sction lookd at nd-ffcto motion assuming instion oint constaints satisfid. In this sction ill includ instion oint vlocitis in stat sac fomulation to b abl to cancl this motion to zo. Th stat sac can n b ittn as a vcto in : v [ v x v y v z ω x ω y ω z q 7 q 8 ] T. (2) This fomulation lads to kinmatic and dynamic saations as shon in Figu 2. This choic of stat 73

6 V 0 vy ωx ω y ω z l 7 cos q 7 l 7 sin q b cq 78 sq 78 l 7 cos q b 0 0 v z l 7 cos q sq 78 cq 78 l 7 sin q ω x ω + l 7 sin q 8 0 q7 y q cq 78 sq sq 78 cq b + l 7 cos q 7 l 7 sin q sin q 78 b cos q 78 l 7 cos q v z l 7 cos q 8 0 cos q 78 b sin q 78 + l 7 sin q ω x ωy + l 7 sin q 8 0 q7 q cos q 78 sin q sin q 78 cos q b + l 7 cos q 7 l 7 sin q sin q 78 b cos q 78 l 7 cos q l 7 cos q 8 0 ω x cos q 78 b sin q 78 + l 7 sin q l 7 sin q 8 0 ω y ω (8) z 0 0 cos q 78 sin q q7 0 0 sin q 78 cos q q 8 bωy + l 7 cos q 7 ωy + l 7 sin q 7 sin q 78 (b cos q 78 + l 7 cos q 8 )ωx l 7 cos q 8 q 7 cos q 78 + (b sin q 78 + l 7 sin q 8 )ωx + l 7 sin q 8 q 7 ωx + q 7 + q 8 (9) cos q 78 ωy + sin q 78 sin q 78 ωy + cos q 78 vaiabls is vy usful hn contolling vlocity at nty oint to zo. It is also convnint bcaus it can b found dictly fom obot kinmatics (fist 6 vaiabls) and nd-ffcto kinmatics (last 2 vaiabls). Fumo, kintic ngy can b stimatd to K (V 2 0 ) T q T T M (q m ) 0 V 0 0 M (q ) q (22) h q T q q 6 and q T. q 7 q 8 In gnal a som couling tms, but considing lativly lo vlocitis nomally usd duing obotic sugy and lo ight of ist comad to obot, this dynamic dcouling is a good aoximation. In this sns this is a good choic of stat vaiabls. On o hand, nd-ffcto vlocitis as ittn in this ay a not aticulaly usful bcaus s a not vlocitis that ant to contol. A mo aoiat choic of stat vaiabls fo ou oblm is fo found as v v V0 [ vy vy ωx ωy ] T. (23) H hav saatd nty oint vlocitis fo hich ant zo vlocity and vlocity of nd-ffcto on hich ant to div ou contol la. This sntation of vlocity vcto is fo suitabl fo both stiff contol at nty oint and fo xaml hybid contol of nd ffcto. W nd to find nty-oint and nd-ffcto vlocitis in tms of obot and ist vlocitis, i.., v V 0 [ v q x vy ωx ωy q 7 ] T q 8. (24) Th contibution of ist is idntical to vious sction hil obot vlocity lats to instion oint and ist fam vlocitis by Equation (7), hich, hn mov constaints at instion 74

7 P.J. Fom, On Kinmatics of Robotic-assistd Minimally Invasiv Sugy Kinmatic saation: Dynamic saation: v x v y v z ω x ω y ω z Robot kinmatics Robot dynamics. q 7 q 8. End-ffcto kinmatics. End-ffcto dynamics Kinmatic saation: Dynamic saation: vy ωx ωy. q 7 q 8 RCM motion. Dsid nd-ffcto motion Robot dynamics. End-ffcto dynamics Kinmatic saation: Dynamic saation: vy RCM motion Robot dynamics. vy ωx ωy.. Dsid nd-ffcto motion. Robot and nd-ffcto dynamics Figu 2: Saation of oksac vaiabls. Th fist sntation ith V0 is convnint as obot and nd-ffcto kinmatics and dynamics a saatd, but fomulation is not suitd fo contol bcaus nd-ffcto vaiabls a not snt. Th last fomulation of oksac vaiabls is mo suitabl fo contol as it saats nty oint vlocitis and nd-ffcto vlocity vaiabls, and s can b tatd indndntly. oint, bcoms V 0 vy ωx ωy (a + b) (a + b) ωx ω y ω z. (25) Th body vlocity at nd ffcto hn no constaints a snt a found by V 0 Ad g V 0 + V. (26) Th dtails a found in Equation (27). W ill dnot lation found in Equation (27) as V0 J v. (28) W not that if ant to us ou modl to contol both nty oint and nd-ffcto vlocity, nd to us tansfomation in Equation (27), hil fo analysis of nd-ffcto motion assuming constaints a satisfid, it is sufficint to us Equation (8). Fo contol uoss it is convnint to kno maing fom nd-ffcto oksac to joint sac. Th maing btn oksac vlocitis and joint vlocitis a n found by gomtic Jacobian J g,: v V V0 J 0 q J J g, q q q J q. (29) q W ill dnot this matix J q and it v J q q. (30) Whnv nty oint vlocitis a zo can lav s out and it V 0 J q. (3) Fo ou uos, hov, it is mo convnint to us obot fam F as a fnc fo contoll instad of joint vlocitis dictly, as this bcoms comutationally fast and can us convsion to lo-lvl contolls alady availabl in obot contoll. This is discussd mo in Sction 5. 75

8 V 0 Ad g V0 + V l 7 cos q 7 l 7 sin q (a + b) 0 0 cq 78 sq 78 l 7 cos q (a + b) 0 0 v y 0 sq 78 cq 78 l 7 sin q v z ω x cq 78 sq ω y sq 78 cq l 7 cos q l 7 sin q 8 0 q7 q (a + b) + l 7 cos q 7 l 7 sin q cos q 78 sin q 78 (a + b) cos q 78 l 7 cos q v y l 7 cos q sin q 78 cos q 78 (a + b) sin q 78 + l 7 sin q v z ω + l 7 sin q 8 0 q7 x q cos q 78 sin q 78 ω y sin q 78 cos q (a + b) + l 7 cos q 7 l 7 sin q v y 0 cos q 78 sin q 78 (a + b) cos q 78 l 7 cos q l 7 cos q 8 0 v z 0 sin q 78 cos q 78 (a + b) sin q 78 + l 7 sin q l 7 sin q 8 0 ω x ω. y cos q 78 sin q ω z sin q 78 cos q q7 q 8 (27) 76

9 P.J. Fom, On Kinmatics of Robotic-assistd Minimally Invasiv Sugy 3.5 Minimal Rsntation ith Instion Point Constaints Fom Equation (7) s that vlocitis at nty oint can b ittn in tms of obot vlocitis as v x v x + aω y (32) v y v y aω x (33) and constaint of zo vlocity can fo b cast into folloing siml fom v x aω y (34) v y aω x (35) h nd to kno distanc fom last link of obot to nty oint, hich may b tim vaying. W can incooat s constaints in kinmatics by intoducing n vaiabls v and v 2 such that v x v ω y a v (36) v y v 2 ω x a v 2. (37) Substituting this into Equation (27) givs Equation (38). No that hav guaantd that vlocitis at nty oint a zo can mov s and gt sntation ll suitd fo oksac contol givn in Equation (39). This sntation is suitabl fo oksac contol and at sam tim guaants that nty oint vlocity constaints a satisfid. W ill dnot matix in Equation (39) that givs us minimum sntation of nd-ffcto oksac as J m and this imotant tansfomation as V 0 J m v m. 3.6 Imlmntation Th fist st is to find n joint vlocity vaiabls fom dsid nd-ffcto motion. This is found by invs of tansfomation in Equation (39). Onc n vaiabls in Equation (39) a found, s nd to b tansfomd into a fnc fo last obot link F. In contoll v and v 2 a alizd though xssions found in Equations (36) and (37). This lation can b ittn as a matix such that vy v v 2 ωx ωy 0 a v z a ω. (40) z q7 q q 8 q This xssion, tog ith Equation (39) thus givs a maing fom dsid nd-ffcto vlocitis to obot and nd-ffcto vlocitis. Ths a asily imlmntd i by standad contol intfac of obots o by finding joint vlocitis though invs Jacobian and fding this to lo-lvl contolls. It is in fact this imlmntation of n vlocity vaiabls v and v 2 into obot vlocitis V0 that guaants that instion oint constaints a alays satisfid. It is imotant to mmb that a(t) and b(t) a tim vaying and should thus b calculatd fom maniulato foad kinmatics. 4 Dynamic Equations To find dynamic quations fist it kintic ngy of ach of igid bodis in systm as K i ( ) V T 2 0i Ii V0i 2 qt J T i Ad T g i0 I i Ad gi0 J i q 2 2 ( q T Ji T + q J T i T ) Ad T I gi0 i Ad (J gi0 i q + J i q ) [ q T q T ] Mi [ q q ] 2 vt M i (q)v (4) ith M i (q) R (6+2) (6+2) givn as [ J T M i (q) i Ad T g I i0 i Ad J gi0 i Ji T AdT g I i0 i Ad J ] gi0 i Ji T AdT g i0 I i Ad gi0 J i Ji T AdT g i0 I i Ad gi0 J i (42) snting intia of ach link and total intia matix is givn by M(q) 8 i M i(q). Fu dnot by C(q, q) Coiolis matix of systm. Th dynamics of hol systm can no b ittn as [M ] (q ) M (q) q M(q) T M (q ) q C (q + ) C (q) q τ. (43) C (q) C (q ) q τ 77

10 0 0 0 a v vy x 0 0 a v y (a + b) + l 7 cos q 7 l 7 sin q v vy z 0 cos q 78 sin q 78 (a + b) cos q 78 l 7 cos q l 7 cos q 8 0 ω x 0 sin q 78 cos q 78 (a + b) sin q 78 + l 7 sin q l 7 sin q 8 0 ω ωx y ω ω y z cos q 78 sin q q sin q 78 cos q q a v 0 0 a v (a + b) + l 7 cos q 7 l 7 sin q v z 0 cos q 78 sin q 78 (a + b) cos q 78 l 7 cos q l 7 cos q 8 0 a 0 sin q 78 cos q 78 (a + b) sin q 78 + l 7 sin q l 7 sin q 8 0 v 2 a v ω z cos q 78 sin q q sin q 78 cos q q a a v 0 0 a a a (a + b) a l v 2 7 cos q 7 l 7 sin q cos q 78 sin q 78 a (a + b) cos q 78 a l v z 7 cos q l 7 cos q sin q 78 cos q 78 a (a + b) sin q 78 + a l v 2 7 sin q l 7 sin q 8 0 v a a cos q ω z 78 sin q q a sin q 78 cos q q 8 a a a a a (a + b) a l v 7 cos q l 7 sin q cos q 78 a (a + b) cos q 78 a l v 2 7 cos q 8 sin q 78 0 l 7 cos q sin q 78 + a (a + b) sin q 78 + a l v z 7 sin q 8 cos q 78 0 l 7 sin q 8 0 ω z 0 a 0 0 a cos q q sin q q 8 a sin q cos q a (b + l v 7 cos q 7 ) 0 0 l 7 sin q a (b cos q v l 7 cos q 8 ) sin q 78 0 l 7 cos q a (b sin q v z 78 + l 7 sin q 8 ) cos q 78 0 l 7 sin q 8 0 ω (38) z 0 a 0 0 a cos q q sin q q 8 a sin q cos q v a (b + l 7 cos q 7 ) 0 0 l 7 sin q y 0 a (b cos q 78 + l 7 cos q 8 ) sin q 78 0 l 7 cos q 8 0 ωx 0 a (b sin q 78 + l 7 sin q 8 ) cos q 78 0 l 7 sin q ω y a 0 0 a cos q sin q a sin q cos q v v 2 q7 q 8. (39) 78

11 P.J. Fom, On Kinmatics of Robotic-assistd Minimally Invasiv Sugy W s that hav a total of 8 vaiabls (6 fo obotic maniulato and 2 fo ist). Hov, fo contol uoss ant to dfin only 6 vaiabls snting nd-ffcto vlocity. W thus nd tansfomation in fom V 0 J (q) q. (44) To find an xlicit xssion us Equation (2) and find W no it V 0 J (q) q, V 0 Ad g V 0 + V q J (q)v0, Ad g J (q ) q + J (q ) q [ Ad g J (q ) J (q ) ] q q J (q) q. (45) V 0 J (q) q + J (q) q q J (q) ( V 0 J (q) q ) (46) and gt dynamics ( ) MJ V 0 + C MJ J J V0 τ. (47) Ths quations giv us motion of nd ffcto in and constaint focs that duc oksac fom to. If a only intstd in motion of nd ffcto obtain this by ( ) J T MJ V 0 + J T C MJ J J V0 J T τ. (48) At this stag it is intsting to look at ho motion in is ducd to a motion in, as this is not a dundant systm ith intnal motion. Fo systm at hand stict motion at nty oint so that to dundant dgs of fdom a ducd by imosing a stiction on vlocity at this oint. This constaint is sn in Equation (40) hich ducd dgs of fdom of systm fom ight to six by imosing constaints in Equation (36-37). Th sulting quations in nd-ffcto vaiabls V0 a n givn by Equation (48). W also not that fomulation of dynamics in this ay quis maniulato Jacobian. Thus, a lo-lvl contol schm of this kind is dndnt of maniulato kinmatics, hich is of cous alays cas, hil kinmatic fomulation sntd in vious sction, and also studid in nxt sction, bcoms a high-lvl contoll and is thus indndnt of maniulato kinmatics, as ill s. In this cas invs kinmatics is handld by obot contoll. 5 Catsian Contol In Sction 3 found a stat sac sntation ll suitd fo imlmnting diffnt contol schms at nty oint and nd ffcto by saating oksac vaiabls. This can fo xaml b usd to obtain a stiff bhavio at nty oint and comliant o hybid contol at nd ffcto. In this sction ill bifly look at ho kinmatic fomulation divd in this a can b usd in contol. 5. Position Contol W ill fist look at a siml osition contol schm, i.., ant nd ffcto to follo a -dfind ath, in addition to satisfying constaints at instion oint. Stiff contol of this kind is ncssay in many alications h nd ffcto is to follo a fnc ath as closly as ossibl. Th asist ay to us RCM Jacobian is to simly tansfom dsid nd-ffcto motion into a joint tajctoy hich is guaantd to satisfy nty oint constaints. This is obtaind though q d J V0,d. (49) A joint sac contoll can n b imlmntd in nomal ay. This contol la is shon in Figu 3. Fo tajctoy folloing an out loo should b addd fo obustnss uoss. Fo tloation, hov, this is nomally not quid, as oato comnsats fo dift in osition. 5.2 Hybid Contol If a foc snso is availabl at nd ffcto can simly dsign a contoll that minimizs intaction focs, lik Natual Admittanc Contoll (NAC) sntd by Nman and Zhang (994) o o nd-ffcto foc contol schms. Using foc masumnts can n gnat on contol signal to minimiz intaction focs and on signal to follo dsid tajctoy (Nman and Zhang, 994). Combining this ith kinmatic constaints at nty oint ill sult in an xtnsion of 4 DoF aoach sntd in Dal t al. (202) to a comlt 6-DoF sntation of nd-ffcto oksac. Altnativly an imdanc contoll can b alid, i.., dfin dsid chaactistics fo intaction focs in tms of a mass-sing-dam systm. Similaly, a standad hybid contol la can b imlmntd as in Figu 4. Sval tasks in obotic tlsugy qui nd ffcto to intact ith nvionmnt, fo xaml in cutting and sutuing. 79

12 Fom, Kinmatics of Minimally Invasiv Robotic Sugy V 0,d J q d + ė Contoll u Robot q Figu 3: Stiff contol by us of RCM Jacobian matix Figu 3: Stiff contol by us of RCM Jacobian matix joint tajctoy hich is guaantd to satisfy nty oint constaints. This is obtaind though This ill in many cass qui combination of stiff and comliant contol in diffnt dictions of nd-ffcto fam. q d J V0,d. (49) Fo this kind of contol schms fist dfin a st ofa constaints, joint sac hich contoll cosond can ntob imlmntd natual constaints nomal of Caig ay. and This Raibt contol (979) la isand shon Mason in Figu (98). 3. in Ths Fostict tajctoy folloing allod motion an out of loond-ffcto should b ith addd fo objctiv obustnss notuoss. to ham Fo tloation, atint. Thn, hov, thisgnatd nomallyby not quid, oato as is tatd oatoas com- an fnc atificial nsatsconstaint, fo dift ini.., osition. motion is f but guaantd to alays satisfy natual constaints. On xaml of hybid contol is shon in Figu 4. W f 5.2 tohybid Caig and Contol Raibt (979) and Mason (98) fo If amo focdtails. snso is availabl at nd ffcto can simly dsign a contoll that minimizs intaction focs, lik Natual Admittanc Contoll 5.3 (NAC) Imdanc sntd by Contol? o o ith nd-ffcto Instion foc contol Point schms. Constaints Using foc masumnts can n gnat on contol signal to minimiz intaction focs and on signal to follo dsid Imdanc contol fo minimally invasiv sugy is challnging bcaus imdanc contol nds to b tajctoy (?). Combining this ith kinmatic constaints at nty oint ill sult in an xtnsion imlmntd in nd-ffcto sac hil constaints on obot motion nds to b so that vlocitis at instion oint a zo. On solution to of 4 DoF aoach sntd in? toacomlt6- DoF sntation of nd-ffcto oksac. Altnativly this oblm an is imdanc shon in Figu contoll 5. Th can dsid b alid, motion i.., is givn dfin by dsid mast chaactistics vlocitis V0,d fo. Fo imdanc intaction contol focs in to tms b usful of a it mass-sing-dam nds to b imlmntd systm. in oksac Similaly, vaiabls. a standad W hybid ill dfin contol a comliant la can b fam imlmntd c hich givs F as in Figu osition 4. and Sval ointation tasks of in obotic nd ffcto tlsugy hn qui it is in contact nd ffcto ith to nvionmnt, intact ith i.., nvionmnt, dviation fo fom xaml dsid in cutting fam and F d sutuing. du to This snsd ill innd-ffcto many cassfocs qui(natal combination (2003)). of Whn stiff andnd comliant ffctocontol is in contact in diffnt ith dictions nvionmnt of it ill nd-ffcto thus follo fam. fam F c hich lats to dsid Fo tajctoy this kindfam of contol F d by schms fist dfin a st of constaints, hich cosond to natual constaints of? Mand c dc?. + Ths D c ṗ dc stict + K c dc Fallod motion (50) of nd-ffcto ith objctiv not to ham h atint. dc Thn, is osition fnc of Fgnatd c ith sct by tooato F d and Mis c, tatd D c, and as Kan c atificial dfin constaint, mass, sing, i.., andmotion dam is systm f but foguaantd a foc F. tothis alays givs satisfy a n dsid natual constaints. by Onfam xaml F c hn of hybid obot contolisisinshon contact in givn ith Figu 4. nvionmnt. W f to? W and s? fo that mo hn dtails. is no contact hav dc 0 and gt F d F c. as xctd. 5.3 Imdanc Contol ith Instion W also nd to guaant that vlocitis at instion Point oint Constaints a zo. This is guaantd by intoducing Imdanc vaiabls contol fo v and minimally v 2 as ininvasiv Equation sugy (39). Th is matix challnging (J) m bcaus thus givs imdanc us motion contol ofnds maniulato imlmntd am foin hich nd-ffcto constaints saca hil satisfid. con- W to b no staints givon this asobot inutmotion to obot nds am, to btog so that ith vlocitis ist motion, at instion also found oint by Equation a zo. (39) On solution Not to that this oblm hav saatd is shon infigu fdback 5. Th loosd- sidmaniulato motion is givn am and by mast ist, as vlocitis shon inv 0,d Fig-. fo u Fo 5. imdanc W hav contol obtaind tocomliant b usful it contol ndsinto b ndffctolmntd oksac in oksac hich also vaiabls. guaants W ill thatdfin in- a imstion comliant oint fam constaints F c hicha givs satisfis, osition as quid. and ointation Notofalso that nd ffcto onlyhn us it isvlocity in contact vaiabls ith in nvionmnt, contoll. i.., This isdviation not a oblm fom in dsid tloation, fam as F d duosition to snsd vaiabls nd-ffcto a nomally focs comnsatd (?). Whn fo by nd ffcto us, is and in contact a mainly ith intstd nvionmnt in folloing it ill thus vlocity follo fnc. fam F c In hich lats imdanc to contoll, dsid hov, tajctoy fam nd F d both by acclation and osition vaiabls. W Mfo c dc + D c ṗnd dc + Kto c includ dc F a mmoy (50) in imdanc contoll so that osition can b covd h dc hnv is osition sing offocs F c ith asct quid. to F d and M c, D c,andk c dfin mass, sing, and dam systm fo a foc F. This givs a n dsid motion givn by fam F c hn obot is in contact 6 Conclusion ith nvionmnt. W s that hn is no contact hav dc 0andgtF d F c. as xctd. This a divd kinmatics of obotic maniulatos subjct to mot cnt of motion (RCM) con- W also nd to guaant that vlocitis at instion oint a zo. This is guaantd by intoducing vaiabls v and v 2 as in Equation (39). Th staints. Th contol la is divd in Catsian nd-ffcto matix (J m sac hich allos fo convntional contol ) thus givs us motion of maniulato am schms such fo as hich comliant constaints and hybid a contol satisfid. to b W imlmntd no giv this also as in inut snc to obot of RCM am, constaints. tog ith W solvd ist motion, constaints also at found a kinmatic by Equation lvl, (39) i.., find a Jacobian Not that matix hav that saatd mas nd-ffcto fdback loos vlocitis fo into maniulato a n st of vlocity am andvaiabls ist, onas hich shon inimos Fig- stuctu 5. W hav thatobtaind guaants comliant that contol mot in cnt nd- of au motion ffcto constaints oksac hich a satisfid. also guaants This modifid that Jaco- bian stion matix, oint dnotd constaints a RCM satisfis, Jacobian, as quid. can thus b in- usd Not to also find that dsid onlyjoint us vlocitis vlocity fom vaiabls a dsid in nd-ffcto contoll. vlocity This isand not a oblm toca location. in tloation, caus as n osition vlocity vaiabls vaiabls a nomally a asilycomnsatd mad to fo obot by and us, ist andjoint a sac mainly intstd dsid tajctoy in folloing can b imlmntd vlocity fnc. using standad In contolls imdancavailabl contoll, fo most commcially availabl maniulatos. 80

13 P.J. Fom, On Kinmatics of Robotic-assistd Minimally Invasiv Sugy V 0 v d + ė S ė Fom, Kinmatics of Minimally Invasiv Robotic Sugy T Jq Position Contol ė v J q Position Contol τ τ v τ Robot V 0 F F d + F I S F J T q Foc Contol τ F F Figu Figu 4: 4: Comliant Comliant contol contol by by us us of of RCM RCM Jacobian Jacobian matix matix hov, nd both acclation and osition F vaiabls. W fo nd to includ a mmoy vin x v imdanc contoll so that osition can b v 2 V vy 0 covd hnv sing focs a quid. J C V 0,d Imdanc contol V 0,c 6 Conclusion (J m ) v m ω x ω y ω z 7 Futu Wok Th v 2 algoithms sntd in a hav bn imlmntd ė τ v z Robot V0 and hav vifid Robot that instion oint a vlocitis v2 a v R a 6 contol R kt at zo. 6 am R It ould b intsting 6 to look mo into hybid contol la and s if and ho RCM constaints duc maniulability v and thus also fomanc of hybid contol in nd-ffcto fam. W should also look at ho singulaitis a affctd and h a dundant systm is quid in od to guaant that singulaitis ė τ q7 do not ais in Wist τ F oksac. q This a divd kinmatics of obotic 8 maniulatos subjct to mot cnt of motion (RCM) con- R 2 contol Wist R 2 staints. Th contol la is divd in Catsian Acknoldgmnt nd-ffcto sac hich allos fo convntional contol Figu schms Figu5: 5: such An Animdanc as comliant contol contol and hybid schm schm contol hich hich tofist fist b imlmntd vaiabls vaiabls imoss Th autho dsid acknoldgs imdanc contol suot on of nd-ffcto Nogian also in and and snc n, n, though of though RCM constaints. constaind W Jacobian Rsachmatix Council (J (Jfo ) i funding and suot. ) m guaants that instion solvd oint oint constaints constaints at a a kinmatic asatisfid. lvl, Not Not i.., that that both find both obot obotand andist can can b b contolld using standad a Jacobian contolls contolls matix that availabl availabl mas ith ith nd-ffcto most most industial industial vlocitis maniulatos. maniulatos. into a n st of vlocity vaiabls on hich imos a stuctu that guaants that mot cnt of 7motion Futu constaints Wok a satisfid. This modifid Jacobian matix, dnotd RCM Jacobian, can thus b singulaitis a affctd and h a dundant sys- nd-ffcto fam. W should also look at ho usd to find dsid joint vlocitis fom a dsid tm is quid in od to guaant that singulaitis Th algoithms sntd in a hav bn imlmntd and hav vifid that instion oint nd-ffcto vlocity and toca location. caus do not ais in oksac. n vlocity vaiabls a asily mad to vlocitis a kt at zo. It ould b intsting to obot and ist joint sac dsid tajctoy can look mo into hybid contol la and s if and b imlmntd using standad contolls availabl fo ho RCM constaints duc maniulability most commcially availabl maniulatos. and thus also fomanc of hybid contol in v 2 8

14 Acknoldgmnt Th autho acknoldgs suot of Nogian Rsach Council fo i funding and suot. Rfncs Azimian, H. Poativ Planning of Robotics- Assistd Minimally Invasiv Cadiac Sugy Und Unctainty. Ph.D. sis, Univsity of Wstn Ontaio - Elctonic Thsis and Disstation Rositoy, 202. Azimian, H., Patl, R., and Naish, M. On constaind maniulation in obotics-assistd minimally invasiv sugy. In iomdical Robotics and iomchatonics (iorob), 200 3d IEEE RAS and EMS Intnational Confnc on. ags , 200. doi:0.09/ioro Cho, J. H., Fom, P. J., Annstdt, M., Robtsson, A.,, and Johansson, R. Dsign of an intmdiat lay to nhanc oato aanss and safty in tlsugical systms. Pocdings of IEEE/RSJ Intnational Confnc on Intllignt Robots and Systms, Vilamoua, Potugal, 202. doi:0.09/iros Caig, J. and Raibt, M. A systmatic mthod of hybid osition/foc contol of a maniulato. In Comut Softa and Alications Confnc, 979. Pocdings. COMPSAC 79. Th IEEE Comut Socity s Thid Intnational. ags , 979. doi:0.09/cmpsac Dal, A., Cho, D. L., and Nman, W. Hybid natual admittanc contol fo laaoscoic sugy. In Pocdings of IEEE/RSJ Intnational Confnc on Intllignt Robots and Systms doi:0.09/iros Fom, P. J., Pttsn, K. Y., and Gavdahl., J. T. Vhicl-maniulato systms - modling fo simulation, analysis, and contol. Sing Vlag, London, UK, 203. Funda, J., Taylo, R., Eldidg,., Gomoy, S., and Gubn, K. Constaind catsian motion contol fo tloatd sugical obots. Robotics and Automation, IEEE Tansactions on, (3): doi:0.09/ Hannafod,., Rosn, J., Fidman, D., King, H., Roan, P., Chng, L., Glozman, D., Ma, J., Kosai, S., and Whit, L. Ravn-ii: An on latfom fo sugical obotics sach. iomdical Engining, IEEE Tansactions on, (4): doi:0.09/tme Lnačič, J. and Galltti, C. Kinmatics and modlling of a systm fo obotic sugy. In On Advancs in Robot Kinmatics. Sing, Li, M., Kaoo, A., and Taylo, R. A constaind otimization aoach to vitual fixtus. In Intllignt Robots and Systms, (IROS 2005) IEEE/RSJ Intnational Confnc on. ags , doi:0.09/iros Lock, R. and Patl, R. Otimal mot cntof-motion location fo obotics-assistd minimallyinvasiv sugy. In Robotics and Automation, 2007 IEEE Intnational Confnc on. ags , doi:0.09/root Mason, M. T. Comlianc and foc contol fo comut contolld maniulatos. Systms, Man and Cybntics, IEEE Tansactions on, 98. (6): doi:0.09/tsmc Nakamua, Y. Advancd obotics: dundancy and otimization. Addison-Wsly sis in lctical and comut ngining: Contol ngining. Addison-Wsly Longman, Incooatd, 99. URL htt://books.googl.no/books?id h4qaqaamaaj. Natal, C. Intaction Contol of Robot Maniulatos: Six-dgs-of-fdom Tasks. Sing Tacts in Advancd Robotics. Sing, URL htt: //books.googl.no/books?idmsyqxs5ci4sc. Nman, W. S. and Zhang, Y. Stabl intaction contol and coulomb fiction comnsation using natual admittanc contol. Jounal of Robotic Systms, 994. ():3. doi:0.002/ob Otmai, T. and Hizing, G. Catsian contol issus fo minimally invasiv obot sugy. In Intllignt Robots and Systms, (IROS 2000). Pocdings IEEE/RSJ Intnational Confnc on, volum. ags vol., doi:0.09/iros Sun, L. and Yung, C. Pot lacmnt and os slction of da vinci sugical systm fo collisionf intvntion basd on fomanc otimization. In Intllignt Robots and Systms, IROS IEEE/RSJ Intnational Confnc on. ags , doi:0.09/iros