On the Manipulability of Velocity-constrained Serial Robotic Manipulators

Transcription

1 Preprints of the 9th World Congress The Interntionl Federtion of Automtic Control On the Mnipulilit of Velocit-constrined Seril Rootic Mnipultors Pål John From Anders Roertsson, nd Rolf Johnsson Deprtment of Mthemticl Sciences nd Technolog, Norwegin Universit of Life Sciences, 432 Ås, Norw. Deprtment of Automtic Control, Lund Universit, PO Bo 8, SE Lund, Sweden Astrct: This pper presents performnce metric for the mnipulilit of constrined seril mnipultors. The mnipulilit mesure nd mnipulilit ellipsoid re found from the mnipulilit Jcoin, which is esil found for seril mnipultors s the stndrd Jcoin mtri, or for prllel mnipultors s the Jcoin tht gives the mpping from the ctive joint velocities to the end-effector velocities. For seril mnipultors with constrints on the kinemtic chin, however, this mesure is not strightforwrdl found ecuse the mnipulilit Jcoin cnnot e found in the sme w s for the cses ove. In this pper we propose to use the Constrined Jcoin Mtri, i.e., n nlticl mpping etween the end-effector nd joint velocities tht lso tkes the chin constrints into ccount. This nlticl representtion of the Constrined Jcoin Mtri llows us to find performnce metric descriing the mnipulilit of seril mnipultor with chin constrints in the sme w s for non-constrined kinemtic chins nd prllel mnipultors. The pproch llows us to compre the mnipulilit of the end effector of non-constrined seril mnipultors nd constrined seril mnipultors nd we stud the importnt cse of rootics-ssisted minimll invsive surger where the rm is constrined t the entr point. Kewords: Root Mnipultor, Kinemtic Constrints, Surgicl Rootics.. INTRODUCTION Root control lws re often designed or evluted sed on performnce metrics. One such metric is wht is referred to s the mnipulilit of the rootic rm. The mnipulilit of rootic mnipultor tells us in wht directions the end effector cn move nd how much effort it requires to move in ech direction. The mnipulilit is often mesured the condition numer or the mimum singulr vlue of the Jcoin mtri, or more illustrtivel the mnipulilit ellipsoid. For redundnt roots the ojective is generll to tke dvntge of the null-spce of the root to find trjector for which the mnipulilit remins high, or t lest ove some limit, while for non-redundnt roots the ojective is to void singulr configurtions modifing the trjector. The concept of mnipulilit of seril mnipultors ws introduced in [?]. For the si degrees of freedom of the end-effector spce the mnipulilit ws found nd 6-DoF ellipsoid ws constructed to define the mnipulilit. When motion in one direction of the ellipsoid cnnot e relied, the length of this is ecomes ero nd the mnipulilit ellipsoid hs no volume. This is often referred to s singulr configurtion. The mnipulilit cn lso e found for prllel mnipultors [??]. In this cse the length in one or more directions of the mnipulilit ellipsoid cn ecome either ero or infinite. If one direction is ero it mens tht velocit cnnot e relied The uthors re memers of the LCCC Linneus Center nd the elliit Ecellence Center t Lund Universit. The work is prtill finnced the Norwegin Reserch Council. in this direction, this is often referred to s n unmnipulle singulrit. On the other hnd, if the length pproches infinit the mnipultor cnnot prevent the end effector to move in certin direction. In this cse the pssive joints will move even though ll the ctive joints re locked, which is referred to s n unstle singulrit [??]. For seril mnipultors the mnipulilit Jcoin is defined s the mpping from joint spce velocities to the velocit of the end effector, i.e., the stndrd nlticl or geometric Jcoin mtri. For prllel mnipultors the mnipulilit Jcoin is normll found s the mpping from the ctive joints to the end-effector velocities. This pper endevors to define similr metric for seril mnipultors with velocit constrints on one or more points in the kinemtic chin. This is different from prllel roots with closed-loop constrints ecuse the constrints re imposed on point on the chin so tht the joints fter the constrint cn move freel. We thus get two different sets of joints where one is constrined nd the other one is free. This is n importnt clss of roots tht needs to e treted differentl from the two cses descried ove. One importnt emple of such mechnisms is roots used for minimll invsive surger where the point t which the root is inserted into the od does not llow for lterl motion to void dmging the ptient s skin. Other emples re seril roots in the presence of ostcles or virtul velocit constrints, or snke-like roots with severl joints tht need to enter into smll openings, for emple through cr window or into uilding. Copright 204 IFAC 0934

2 The mnipulilit of seril nd prllel mnipultors hve een studied etensivel in literture. There does not, however, seem to e corresponding nlsis of the mnipulilit of seril mnipultors suject to chin constrints. For these roots the mnipulilit cnnot e determined the stndrd Jcoin mtri s this reltion does not give the mpping from joint to end-effector spce when kinemtic constrints re present in kinemtic chin. Constrined seril mnipultors differ from seril unconstrined mnipultors in tht the stndrd geometric or nlticl Jcoins do not give the mpping from joint to end-effector velocit spce ecuse of the constrint, nd lso differ from prllel mnipultors ecuse the end-effector motion of the seril root is free while the chin motion is constrined. In this pper we propose to use modified Jcoin to determine the mnipulilit. More specificll we propose the Constrined Jcoin Mtri (CJM) introduced in [?] s cndidte for nling the mnipulilit of seril rootic mnipultor suject to one or more velocit constrints on the kinemtic chin. The CJM finds the mpping etween the end-effector velocities nd new set of velocit vriles which gin hs one-to-one reltion to the joint velocities. It is thus 2-step mpping from the joint velocities to the end-effector velocities which stisfies the velocit constrints. We suggest tht this mpping cn e used to find the mnipulilit mesure of seril rootic mnipultor with velocit constrints on the chin. The pper is orgnied s follows: Section 2 gives n overview of the prolem discussed nd sets the frmework. Section 3 riefl introduces the most relevnt concepts of differentil kinemtics nd introduce the Constrined Jcoin Mtri. The new mnipulilit inde for constrined seril mnipultors is introduced in Section 4 nd simple emple relted to rootics-ssisted minimll invsive surger is shown in Section SYSTEM OVERVIEW AND PROBLEM FORMULATION The sstem discussed in this pper consists of seril rootic mnipultor with one or more velocit constrints on the kinemtic chin. The mnipultor rm is normll redundnt to mintin the full moilit of the end effector under chin constrints. The sstem setup for these constrints together with the most importnt configurtion spces used in this pper re shown in Fig.. We denote the frme of the joint locted efore the constrint t F c in the chin F nd the joint tht is locted fter the constrint is denoted F. The desired endeffector motion is given the frme F e. In this pper we re concerned with the mnipulilit of the frme F e. We will denote the velocit vriles representing the velocities of F e with respect to the inertil frmef 0 in the following w V B 0e = [ v e v e v e ω e ω e ω e ] T () nd similrl for the other frmes. The superscript B stte tht the vriles re written in od coordintes [?]. We consider rootic mnipultors with n joints so tht the unconstrined motion is n n-dof motion. Furthermore we ssume tht the constrined motion hs m degrees of freedom so the constrint hs dimension n m. The end-effector spce hs dimension r. Denote the joints so tht the joint closest to the se is joint nd in incresing order until we rech the lst joint nd the end effector. We will divide the joints into two groups where the first group q consists of ll joints tht come efore the constrint F c in the kinemtic chin nd the second group q consists of ll joints fter the constrints. The prolem considered in this pper is to find the mnipulilit of these kinds of kinemtic structures, i.e., seril rootic mnipultors with free end-effector motion which mens there re no constrints imposed on the end effector itself ut with one or more kinemtic constrints on the kinemtic chin. Intuitivel, this kind of constrints will potentill reduce the moilit of the sstem drsticll nd thus leds to reduced performnce. In this pper we derive performnce metric for thorough nlsis of these sstems nd stud in detil the effects tht these constrints hve on the mnipulilit of constrined seril mnipultor. 3. DIFFERENTIAL KINEMATICS Differentil kinemtics is the stud of how joint velocities relte to the link velocities, nd in prticulr the end-effector velocities. Mnipulilit cn e thought of s mesure of how well-defined this mpping is for given configurtion of the root rm. 3. Mnipultor Jcoin In this section we will find the reltion etween the desired end-effector velocities nd the corresponding joint velocities for seril mnipultors. The stndrd od Jcoin mtri gives the mpping from the joint velocities q to the end-effector velocities V0e B in od coordintes which is the mpping [?] V0e B = Je B (q) q. (2) The od Jcoin mtri Je B (q) is found representing the twist of ech joint i in the end-effector frme, i.e., Je B (q) = [ X X 2 ] X n (3) = [ Ad g e X Ad ] g 2e X2 2 Xn n R n 6 where Xi i is the constnt twist in frme F i nd Ad g ie is the Adjoint mtri tht trnsforms Xi i from frme F i to X i represented in the end-effector frme F e. The od Jcoin mtri cn lso e found for other links thn the end effector, in which cse it is denoted Ji B which gives the velocities of link i. Prticulrl, the Jcoin mtri tht gives the velocit of the linkf locted efore the constrint is denoted J B. 3.2 Constrined Jcoin Mtri In this section we will find the od Jcoin mtrices, s ove, ut suject to the constrints, i.e., we find the mpping from the joint velocit vriles to the end effector suject to constrint on the velocit t the constrint frme F c. For lrge clss of constrints the Constrined Jcoin Mtri cn e written in the form [?] [ J e B = αi X i αj X j αk X k] R m 6 (4) for some (n m)-dimensionl constrint. The r in J B e distinguishes the Constrined Jcoin Mtri from the stndrd JcoinJ B e.x i re the mnipultor twists (represented inf ) while α i re configurtion-dependent functions of the mnipultor nd constrint kinemtics. 0935

3 End-effector Frme - F e S 2 R S S 2 End-effector Frme - F e S 2 R 2 S S 2 Mnipulilit Hole Constrint -F c Constrints:R 2 Motion spce: S 2 R S Plne Constrint - F c Constrints:R Motion spce: S 2 R 2 S Mnipulilit Fig.. We stud the mnipulilit of the end-effector frme F e in the presence of constrint t F c. The figure shows two emples of the constrints discussed in this pper: on the left, hole constrint which prevents n lterl motion of specific point on the mnipultor chin; nd on the right, plne constrint tht restricts the liner motion of point to given direction in the plne. The motion spces of the different frmes re sugroups of SE(3) defined liner motion R, circulr motion S, nd the sphere S 2. Given the end-effector velocit we wnt to find the free nd constrined velocit vriles of the root. We will find the Constrined Jcoin Mtri J e B which gives the reltion V0e B = J e B vm nd the required velocitiesvm re found from the desired end-effector velocities the inverse of the Constrined Jcoin Mtri. These new velocit vriles nd the form of the Constrined Jcoin Mtri depends on the tpe of constrint. For plne nd hole constrints, for emple, the new velocit vriles re found s v v v v v v v v v v 2 ω = ω v v v ω = ω ω ω v 2, (5) ω ω v ω respectivel. We cn now write the reduced velocit vriles s V 0 B = [ v v v ω ω ] T, B V 0 = [v v 2 v ω] (6) respectivel. v i for i =,2 re the constrined vriles while the remining vriles re denoted the free vriles. We now present in rief the min ide of how to find the new velocit vriles nd derive the Constrined Jcoin Mtri. The following is tken from [?]: The min ide is to find the velocit V B 0 in terms of set of new velocit vriles prmetried in such w tht these vriles cn e chosen freel nd t the sme time gurntee tht the constrints t F c re stisfied. This mens tht certin directions in the velocit spce re reduced from higher to lower-dimensionl spce represented new velocit vriles v i. As our min ojective is to follow desired end-effector motion V0e B we need to find the mpping fromv0e B to the free vriles, i.e.,v0 B with the reduction in dimensionlit represented v i. This is otined in the following w: () Define desired end-effector velocit V0e B. (2) Given constrint tf c, define the velocities t this point which stisf the constrints, i.e., the velocities t the previous jointf re given the free vriles {v, v, v, ω, ω, ω}, nd the constrined vriles {v, v 2, v 3,...}. The free vriles re the ones tht cn e chosen freel nd do not ffect the constrint. The constrined vriles require specific form nd structure for the constrints to e stisfied. We will therefore replce some of the free vriles with the constrined vriles which gives us the required structure. These vriles thus represent freedom, ut in spce with reduced dimensionlit tht stisfies the constrint. The constrined vriles re thus written in terms of the free vriles v s V0 B = V0(v). B (7) (3) Eliminte the redundnt vriles tht rise s result of the reduced dimensionlit nd denote the miniml representtion of the velocit vriles V 0. B The vriles will now tke the form shown in (6). (4) Find mpping from the end-effector velocities V0e B to the new reduced velocit vriles V 0, B which will tke the form v m = ] B [ V 0 = q [ ] constrined vriles free vriles. (8) joint velocities The mpping is given the Constrined Jcoin Mtri J e B tht gives the importnt reltion V0e B = J e B vm, i.e., the trnsformtion from the new reduced velocit vriles vm to the desired end-effector velocities V0e B. The joint velocities represent the joints tht re determined the end-effector velocit V0e B onl nd do not depend on the constrints. These re tpicll the joints q tht re situted etween the constrint nd the end effector. The free vriles re the velocities of F tht do not depend on the constrint, ut differentl from the joint velocities, the depend on the joints q etween the se 0936

4 nd the constrint. Finll, the constrined vriles re constrint dependent nd give the velocit t F the required structure so tht the constrints re stisfied. (5) From the new vriles, find the root velocit t F. We refer to [?] for detils on this topic. We note tht there re two min steps: First we need to find suitle representtion of the velocit vriles nd define the Constrined Jcoin Mtri which gives us V 0 B nd q. Then we find V0 B from V 0 B nd finll the joint velocities q from V0 B the stndrd Jcoin mtri. The kinemtics of seril rootic mnipultor with chin constrints re now found the two kinemtic reltions V B 0e = J B e (q)v m (9) V B 0 = J B (q) q (0) where J B e (q) is the Constrint Jcoin Mtri nd J B (q) is the stndrd od geometric Jcoin tht gives the mpping from the (first m) joints to joint F. We will see emples of these mtrices in Section MANIPULABILITY In this section we riefl review the mnipulilit mesure of roots s presented in literture. Given the mnipulilit Jcoin J M R r n, the mnipulilit mtri is given W = J M JM T () nd the mnipulilit mesure s w = det JM JM T. (2) Let the dimension of the tsk spce r e the sme s the moilit of the mnipultor n, i.e., non-redundnt roots with r = n. In this cse the mnipulilit mesure is given simpl w = detj M. (3) The mnipulilit ellipsoid is found the eigenvlues nd eigenvectors of the mnipulilit mtri M. Let the singulr vlue decomposition of J M e written s [?] where J M = UΣV T (4) σ Σ = 0 σ (5) σ. r 0 with σ σ 2 σ r. The mnipulilit mesure cn now e written s w = σ σ 2 σ 3 σ r. (6) Furthermore the mnipulilit ellipsoid is the ellipsoid with principl esσ u, σ 2 u 2,..., σ r u r whereu i re the columns of U. In this section we will find the mnipulilit mesure for constrined rootic rms. The mnipulilit mesure is found (2), so it onl remins to find the mnipulilit Jcoin, which is found in the following w: Seril mnipultors: The stndrd Jcoin mtrij B e (q). Prllel mnipultors: The Jcoin mp from the ctive joint velocities to the end-effector velocities. Constrined seril mnipultors: The Jcoins J B e (q) nd J B (q) found in Eqs. (9) nd (0). The two first cses re studied in detil in literture. In the net section we will stud in more detil the mnipulilit of constrined seril mnipultor rms. 4. The Mnipulilit Mesure for Constrined Kinemtic Chins The mnipulilit of constrined seril mnipultors needs to e found in two steps. We first look t the mnipulilit of the end effector ssuming the first joints q cn generte the required motion. This mpping depends onl on the geometr of the constrint nd the links q locted fter the constrint. This is given (9). The Constrint Jcoin Mtri J e B (q) tells us whether it is possile to generte n end-effector velocit V 0e B using the new velocit vriles vm, i.e., whether the constrined velocities V0 B nd the joints fter the constrint cn generte the desired end-effector velocities. The condition numer nd singulrit nlsis of this mtri thus gives the effects of the constrints on the mnipulilit. This depends onl on the kinemtics of the constrint itself nd the kinemtics of the joints fter the constrint. The mnipulilit ellipsoid is given choosing J M = J B e (q), nd thus gives the mnipulilit in the directions of the end-effector frmev B 0e. The interprettion of this is s follows: It gives us the moilit of V0e B given V0 B nd q. We see tht ecuse we usev0 B the moilit depends on the constrint. We thus interpret this s the constrint-dependent mnipulilit of the sstem, nd denote this the Constrined Mnipulilit Mesure (CMM). In the ove we hve ssumed tht the root cn lws generte the desired motion V0. B This is of course not lws the cse. We thus need to find the sme performnce mesure for V0. B This is similr to the stndrd mnipulilit mesure for non-constrined seril mnipultors. The mnipulilit of the stndrd geometric Jcoin J B (q) is mesure of how efficientl the mnipultor cn generte the motionv0 B required to generte the desired end-effector motion V0e B. This is thus equivlent to the stndrd mnipulilit nlsis of seril mnipultor, ut with respect to joint F s opposed the the end effector F e. We will denote this the Mnipultor Mnipulilit Mesure (MMM). The CMM nd MMM together give mesure of the mnipulilit of constrined seril mnipultor. If the mnipulilit ellipsoid of oth these re non-vnishing we know tht n ritrr motion cn e relied in the end-effector spce, even though there re constrints on the kinemtic chin. Furthermore, the mnipulilit ellipsoids gives us vlule informtion out the cuse of the singulrit nd how it cn e resolved. 5. CASE STUDY MANIPULABILITY OF A SERIAL MANIPULATOR WITH HOLE CONSTRAINTS YZ-Mnipultor Arm Consider n 8-DoF mnipultor with 6 joint efore nd 2 joints fter hole constrint. Assume further 0937

5 F End-effector Frme - F e S 2 R S S 2 Hole Constrint -F c Mnipulilit Fig. 2. An emple of root with two links fter hole constrint Intersecting es ω nd q 7 intersect tht the lst two joints rotte round the - nd the -es, respectivel. An overled photo of such root with imposed hole constrint on point on the rm is showed in Fig. 2 nd illustrted in Fig. 3. For the hole constrints the Constrined Jcoin Mtri cn e written the reltion α cq 8 sq 7 β l 8 sq 7 sq 8 l 8 cq 8 0 v e v e sq 7 cq v v e α 2 sq 7 sq 8 β 2 l 8 sq 7 cq 8 l 8 sq 8 0 v = sq cq v 7cq 8 sq 7 cq 8 sq 8 0 ω e ω e ω e where 0 0 sq 7 cq 7 0 cq 8 0 cq 7sq 8 sq 7 sq 8 scq 8 0 α = cq 8(l 8 +cq 7 ), α 2 = sq 8(l 8 +cq 7 ), β = sq 8(+l 8 cq 7 ), β 2 = cq 8(+l 8 cq 7 ), ω q7 q 8 (7) is the distnce from the constrint to the lst joint nd q 8 is the length of the lst link. The condition of this mtri tells us whether the desired velocities t the end effector cn e relied or not. As we cn see, the mtri onl depends on the link geometr (length) of the lst link, the joint ngles q nd the distnce etween the frmes F, F c, nd F given the vriles nd. If we plot the mnipulilit mesure with respect toq 7 ndq 8 it will look like the plot in Fig. 4 where it is plotted for two different vlues of. We note tht the mnipulilit mesure onl depends on the first joint q 7 right fter the constrint. The mnipulilit mesure vnishes forq 7 = 0,±π. Note tht this singulrit is different from the well known singulrit of the 2-DoF plnr root which lso hs singulrit when it is stretched out (second joint equls ero). For the plnr cse this singulrit is limit singulrit which rises when the mnipultor is t the limit of its workspce, so tht no motion in the direction of the stretched Hole Constrint -F c Mnipulilit Fig. 3. The root hs links q tht rottes out the - nd - es, nd hs singulrities t q 7 = 0,±π (ottom figure). Mnipulilit q Fig. 4. The mnipulilit mesure of the Constrined Jcoin Mtri with respect to the joint vriles q = q 7, q 8 for two different distnces etween F nd F c given s = 0.4 (the surfce with the highest mnipulilit) nd = 0.5 (the surfce with the lowest mnipulilit). The link length is set tol 8 =. out rm cn e relied. In our cse, however, the singulrit is due to the lignment of two velocit vriles, nmel ω nd q 7. It is importnt to note tht this is different from singulrities tht rise in unconstrined mnipultors where two of the es 2 q

6 Hole Constrint -F c F End-effector Frme -F e S 2 R S S 2 Mnipulilit Fig. 5. For this root oth links q rotte out the -is nd the mnipulilit mesure vnishes for ll vlues of q 7 nd q 8. re ligned. In our cse the direction one of the free velocit vriles ω is ligned with the is of q 7, which is different from ligning two es of the mnipultor in tht it depends on the geometr of the constrint, nd not the geometr of the mnipultor joints q. YY-Mnipultor Arm In Fig. 5 we see nother emple of mnipultor geometr where oth joints q rotte round the sme is, nmel the -is. In this cse the Constrined Jcoin Mtri tkes the form α v e 0 0 l 7 sq β sq 78 0 l 7 cq 8 0 v e 0 β 2 cq 78 0 l 7 sq 8 0 v v v e 2 ω e = v ω e ω (8) ω e cq sq q7 q8 sq cq where we hve defined α = (+l 7cq 7 ), cn give ver different results when it comes to mnipulilit. This cn e used to identif geometric structures for which the moilit is lost/mintined, nd thus importnt in mnipultor design nd control in the presence of ostcles. We prticulrl focus on rootic setups such s the ones found in rooticsssisted minimll invsive surger. As future work we wnt to generlie the pproch to include ostcles with different geometr nd lso stud how the dnmic mnipulilit of the mnipultor is ffected the constrint. REFERENCES P. J. From. On the kinemtics of rootic-ssisted minimll invsive surger. Modeling, Identifiction nd Control, 34 (2):69 82, 203. P. J. From nd J. T. Grvdhl. On the moilit nd fult tolernce of closed chin mnipultors with pssive joints. Modeling, Identifiction nd Control, 29(4):5 65, P. J. From, K. Y. Pettersen, nd J. T. Grvdhl. Vehiclemnipultor sstems - modeling for simultion, nlsis, nd control. Springer Verlg, London, UK, 204. F.C. Prk nd J. W. Kim. Mnipulilit nd singulrit nlsis of multiple root sstems: geometric pproch. In Rootics nd Automtion, 998. Proceedings. 998 IEEE Interntionl Conference on, volume 2, pges vol.2, 998. doi: 0.09/ROBOT J. Wen nd L.S. Wilfinger. Kinemtic mnipulilit of generl constrined rigid multiod sstems. Rootics nd Automtion, IEEE Trnsctions on, 5(3): , 999. ISSN X. doi: 0.09/ T. Yoshikw. Dnmic mnipulilit of root mnipultors. In Rootics nd Automtion. Proceedings. 985 IEEE Interntionl Conference on, volume 2, pges , 985. doi: 0.09/ROBOT T. Yoshikw. Singulr-vlue decomposition. Foundtions of Rootics:Anlsis nd Control, MIT Press, β = (cq 78 +l 7 cq 8 ), β 2 = (sq 78 +l 7 sq 8 ). The mnipulilit mesure is lws vnishing, so we hve n ill-conditioned mpping for ll joint positions q. The reson for this cn e seen from geometric point of view: Consider the -plne s seen from the frme F. Then we see tht we use the four vriles v, v, q 7, nd q 8 to generte the 3- DoF motion of this plne. It thus onl remins two degrees of freedom to generte the remining 3-DoF, which is the reson for the vnishing mnipulilit mesure for ll q 7 nd q CONCLUSION In this pper we hve proposed new mnipulilit mesure for constrined seril mnipultors. We hve shown tht the Constrined Jcoin Mtri previousl proposed the uthors cn e used to find the mnipulilit of seril mnipultors with velocit constrints on the chin. We show through two simple emples how the pproch cn e used to nle the mnipulilit of two different rootic rms with hole constrints on the chin nd show how two different joint setups 0939