The Delta Parallel Robot: Kinematics Solutions Robert L. Williams II, Ph.D., Mechanical Engineering, Ohio University, April 2015

Transcription

1 he Delt rllel Robot: Knemtcs Solutons Robert L. Wllms II, h.d., Mechncl Engneerng, Oho Unversty, Aprl 05 Clvel s Delt Robot s rgubly the most successful commercl prllel robot to dte. he left mge below shows the orgnl desgn from Clvel s U.S. ptent, nd the rght photogrph below shows one commercl nstntton of the Delt Robot. Delt Robot Desgn A Flexcker Delt Robot he Delt Robot hs 4-degrees-of-freedom (dof), -dof for XYZ trnslton, plus fourth nner leg to control sngle rottonl freedom t the end-effector pltform (bout the xs perpendculr to the pltform). he remnder of ths document wll focus only on the -dof XYZ trnslton-only Delt Robot snce tht s beng wdely ppled by D prnters nd Arduno hobbysts. resented s descrpton of the -dof Delt Robot, followed by knemtcs nlyss ncludng nlytcl solutons for the nverse poston knemtcs problem nd the forwrd poston knemtcs problem, nd then exmples for both, snpshots nd trjectores. he velocty equtons re lso derved hs s presented for both revolute-nput nd prsmtc-nput Delt Robots. For referencng ths document, plese use: R.L. Wllms II, he Delt rllel Robot: Knemtcs Solutons, Internet ublcton, Aprl 05. R. Clvel, 99, Concepton d'un robot prllèle rpde à 4 degrés de lberté, h.d. hess, EFL, Lusnne, Swtzerlnd. R. Clvel, 990, Devce for the Movement nd ostonng of n Element n Spce, U.S. tent No. 4,976,58.

2 ble of Contents REVOLUE-INU DELA ROO... REVOLUE-INU DELA ROO DESCRIION... REVOLUE-INU DELA ROO MOILIY... 7 RACICAL REVOLUE-INU DELA ROOS... 9 REVOLUE-INU DELA ROO KINEMAICS ANALYSIS... 0 Inverse oston Knemtcs (IK) Soluton... Forwrd oston Knemtcs (FK) Soluton... Revolute-Input Delt Robot Velocty Knemtcs Equtons... 5 REVOLUE-INU DELA ROO OSIION KINEMAICS EXAMLES... 6 Inverse oston Knemtcs Exmples... 6 Forwrd oston Knemtcs Exmples... 9 RISMAIC-INU DELA ROO... RISMAIC-INU DELA ROO DESCRIION... RACICAL RISMAIC-INU DELA ROOS... 5 RISMAIC-INU DELA ROO KINEMAICS ANALYSIS... 6 Inverse oston Knemtcs (IK) Soluton... 7 Forwrd oston Knemtcs (FK) Soluton... 9 rsmtc-input Delt Robot Velocty Knemtcs Equtons... RISMAIC-INU DELA ROO OSIION KINEMAICS EXAMLES... 4 Inverse oston Knemtcs Exmples... 4 Forwrd oston Knemtcs Exmples... 7 ACKNOWLEDGEMENS AENDICES... 4 AENDIX A. HREE-SHERES INERSECION ALGORIHM... 4 Exmple... 4 Imgnry Solutons... 4 Sngulrtes... 4 Multple Solutons AENDIX. SIMLIFIED HREE-SHERES INERSECION ALGORIHM... 45

3 Revolute-Input Delt Robot Revolute-Input Delt Robot Descrpton As shown below, the -dof Delt Robot s composed of three dentcl RUU legs n prllel between the top fxed bse nd the bottom movng end-effector pltform. he top revolute jonts re ctuted (ndcted by the underbr) v bse-fxed rottonl ctutors. her control vrbles re,,, bout the xes shown. In ths model re mesured wth the rght hnd, wth zero ngle defned s when the ctuted lnk s n the horzontl plne. he prllelogrm 4-br mechnsms of the three lower lnks ensure the trnslton-only moton. he unversl (U) jonts re mplemented usng three non-collocted revolute (R) jonts (two prllel nd one perpendculr, sx plces) s shown below. Delt rllel Robot Dgrm dpted from: elmomc.com/cpbltes

4 he three-dof Delt Robot s cpble of XYZ trnsltonl control of ts movng pltform wthn ts workspce. Vewng the three dentcl RUU chns s legs, ponts,,, re the hps, ponts A,,, re the knees, nd ponts,,, re the nkles. he sde length of the bse equlterl trngle s s nd the sde length of the movng pltform equlterl trngle s s. he movng pltform equlterl trngle s nverted wth respect to the bse equlterl trngle s shown, n constnt orentton. 4 Delt Robot Knemtc Dgrm he fxed bse Crtesn reference frme s {}, whose orgn s locted n the center of the bse equlterl trngle. he movng pltform Crtesn reference frme s {}, whose orgn s locted n the center of the pltform equlterl trngle. he orentton of {} s lwys dentcl to the orentton of {} so rotton mtrx RI s constnt. he jont vrbles re Θ, nd the Crtesn vrbles re x y z. he desgn shown hs hgh symmetry, wth three upper leg lengths L nd three lower lengths l (the prllelogrm four-br mechnsms mjor lengths). he Delt Robot fxed bse nd pltform geometrc detls re shown on the next pge.

5 5 Delt Robot Fxed se Detls Delt Robot Movng ltform Detls

6 6 he fxed-bse revolute jont ponts re constnt n the bse frme {} nd the pltform-fxed U-jont connecton ponts re constnt n the bse frme {}: 0 0 w 0 w w 0 w w 0 0 u 0 s w 0 s w he vertces of the fxed-bsed equlterl trngle re: 0 s w b 0 0 u b 0 s w b where: 6 w s u s 6 w s u s nme menng vlue (mm) s bse equlterl trngle sde 567 s pltform equlterl trngle sde 76 L upper legs length 54 l lower legs prllelogrm length 44 h lower legs prllelogrm wdth w plnr dstnce from {0} to ner bse sde 64 u plnr dstnce from {0} to bse vertex 7 w plnr dstnce from {} to ner pltform sde 65 u plnr dstnce from {} to pltform vertex he model vlues bove re for specfc commercl delt robot, the A Flexcker IR 60-/600, scled from fgure (new.bb.com/products). hough Delt Robot symmetry s ssumed, the followng methods my be dpted to the generl cse.

7 7 Revolute-Input Delt Robot Moblty hs secton proves tht the moblty (the number of degrees-of-freedom) for the Delt robot s ndeed -dof. Usng the sptl Kutzbch moblty equton for the prevous Delt Robot fgure: M 6( N) 5J 4J J where: M s the moblty, or number of degrees-of-freedom N s the totl number of lnks, ncludng ground J s the number of one-dof jonts J s the number of two-dof jonts J s the number of three-dof jonts For the s-desgned Delt Robot, we hve: J one-dof jonts: revolute nd prsmtc jonts J two-dof jonts: unversl jont J three-dof jonts: sphercl jont N 7 J J J 0 0 M M 6(7 ) 5() 4(0) (0) 9 dof As often hppens, the Kutzbch equton fls becuse the result must obvously be -dof. hs result predcts the Delt s severely overconstrned sttclly ndetermnte structure, whch s ncorrect. he Kutzbch equton knows nothng bout specl geometry n the Delt Robot cse, there re three prllel four-br mechnsms. he overll robot would work knemtclly dentclly to the orgnl Delt Robot f we removed one of the long prllel four-br mechnsm lnks, long wth two revolute jonts ech. Wth ths equvlent cse, the Kutzbch equton yelds: N 4 J J J M M 6(4 ) 5(5) 4(0) (0) dof whch s correct. An lterntve pproch to clcultng the Delt Robot moblty s by gnorng the three prllel four-br mechnsms, replcng ech wth sngle lnk nsted. In ths we must count unversl jont t ether end of ths vrtul lnk. hs pproch follows the smplfed Delt Robot nmng conventon -RUU. he Kutzbch equton for ths cse lso succeeds:

8 8 N 8 J J J 6 0 M M 6(8) 5() 4(6) (0) dof Ether of the second two pproches works. he uthor prefers the former snce t s closer to the ctul Delt Robot desgn.

9 9 rctcl Revolute-Input Delt Robots Delt Chocolte-Hndlng Robot st.epfl.ch Delt ck-nd-lce Robots en.wkped.org/wk/delt_robot Delt Robot D rnter en.wkped.org/wk/delt_robot Sketchy Delt Robot en.wkped.org/wk/delt_robot Novnt Flcon Hptc Interfce mges.bt-tech.net Fnuc Delt Robot robot.fnucmerc.com

10 0 Revolute-Input Delt Robot Knemtcs Anlyss From the knemtc dgrm bove, the followng three vector-loop closure equtons re wrtten for the Delt Robot: where L l R,, R I snce no rottons re llowed by the Delt Robot. he three pplcble constrnts stte tht the lower leg lengths must hve the correct, constnt length l (the vrtul length through the center of ech prllelogrm): l l L,, It wll be more convenent to squre both sdes of the constrnt equtons bove to vod the squre-root n the Euclden norms: nd l l l l l,, x y z Agn, the Crtesn vrbles re x y z were gven prevously. he vectors Θ :. he constnt vector vlues for ponts L re dependent on the jont vrbles 0 L Lcos Lsn L L cos L cos Lsn L L cos L cos Lsn Substtutng ll bove vlues nto the vector-loop closure equtons yelds: x l ylcos l y L c l z Lsn x L cos b cos z Lsn x L cos b y L cos c z Lsn

11 where: w u s b w cw w And the three constrnt equtons yeld the knemtcs equtons for the Delt Robot: Ly ( )coszlsnx y z Lyl 0 L xb yc zl x y z b c L xb ycl ( ( ) )cos sn 0 L x b y c zl x y z b c L xb yc l ( ( ) )cos sn 0 he three bsolute vector knee ponts re found usng A L,,, : A w 0 Lcos Lsn A ( w L cos ) ( w L cos ) Lsn A ( w L cos ) ( w L cos ) Lsn Inverse oston Knemtcs (IK) Soluton he -dof Delt Robot nverse poston knemtcs (IK) problem s stted: Gven the Crtesn x y z, clculte the three poston of the movng pltform control pont (the orgn of {}), requred ctuted revolute jont ngles Θ. he IK soluton for prllel robots s often strghtforwrd; the IK soluton for the Delt Robot s not trvl, but cn be found nlytclly. Referrng to the Delt Robot knemtc dgrm bove, the IK problem cn be solved ndependently for ech of the three RUU legs. Geometrclly, ech leg IK soluton s the ntersecton between known crcle (rdus L, centered on the bse trngle R jont pont ) nd known sphere (rdus l, centered on the movng pltform vertex ). hs soluton my be done geometrclly/trgonometrclly. However, we wll now ccomplsh ths IK soluton nlytclly, usng the three constrnt equtons ppled to the vector loop-closure equtons (derved prevously). he three ndependent sclr IK equtons re of the form: where: E cos F sn G 0,,

12 E L( y) F zl G x y z L yl E L( ( xb) yc) F zl G x y z b c L ( xb yc) l E L( ( xb) yc) F zl G x y z b c L ( xb yc) l he equton Ecos Fsn G 0 ppers lot n robot nd mechnsm knemtcs nd s redly solved usng the ngent Hlf-Angle Substtuton. If we defne t tn then t t cos nd sn t t Substtute the ngent Hlf-Angle Substtuton nto the EFG equton: t t E F 0 G E( t ) F( t) G( t ) 0 t t ( G E) t ( F) t ( G E) 0 qudrtc formul: t, F E F G G E Solve for by nvertng the orgnl ngent Hlf-Angle Substtuton defnton: tn ( t) wo solutons result from the n the qudrtc formul. oth re correct snce there re two vld solutons knee left nd knee rght. hs yelds two IK brnch solutons for ech leg of the Delt Robot, for totl of 8 possble vld solutons. Generlly the one soluton wth ll knees knked out nsted of n wll be chosen. Forwrd oston Knemtcs (FK) Soluton he -dof Delt Robot forwrd poston knemtcs (FK) problem s stted: Gven the three ctuted jont ngles Θ control pont (the orgn of {}),, clculte the resultng Crtesn poston of the movng pltform x y z. he FK soluton for prllel robots s generlly very dffcult. It requres the soluton of multple coupled nonlner lgebrc equtons, from the three constrnt equtons ppled to the vector loop-closure equtons (derved prevously). Multple vld solutons generlly result.

13 hnks to the trnslton-only moton of the -dof Delt Robot, there s strghtforwrd nlytcl soluton for whch the correct soluton set s esly chosen. Snce Θ re gven, we clculte the three bsolute vector knee ponts usng A L,,,. Referrng to the Delt Robot FK dgrm below, snce we know tht the movng pltform orentton s constnt, lwys horzontl wth R I, we defne three vrtul sphere centers A v A,,, : A 0 w Lcos u Lsn v A s ( cos ) w L ( w L cos ) w Lsn v A s ( cos ) w L ( w L cos ) w Lsn v nd then the Delt Robot FK soluton s the ntersecton pont of three known spheres. Let sphere be referred s vector center pont {c} nd sclr rdus r, ({c}, r). herefore, the FK unknown pont s the ntersecton of the three known spheres: ( A v, l) ( A v, l) ( A v, l). Delt Robot FK Dgrm

14 4 Appendx A presents n nlytcl soluton for the ntersecton pont of the three gven spheres, from Wllms et l. hs soluton lso requres the solvng of coupled trnscendentl equtons. he ppendx presents the equtons nd nlytcl soluton methods, nd then dscusses mgnry solutons, sngulrtes, nd multple solutons tht cn plgue the lgorthm, but ll turn out to be no problem n ths desgn. In prtculr, wth ths exstng three-spheres-ntersecton lgorthm, f ll three gven sphere centers A v hve the sme Z heght ( common cse for the Delt Robot), there wll be n lgorthmc sngulrty preventng successful soluton (dvdng by zero). One wy to fx ths problem s to smple rotte coordntes so ll A v Z vlues re no longer the sme, tkng cre to reverse ths coordnte trnsformton fter the soluton s ccomplshed. However, we present nother soluton (Appendx ) for the ntersecton of three spheres ssumng tht ll three sphere Z heghts re dentcl, to be used n plce of the prmry soluton when necessry. Another pplcble problem to be ddressed s tht the ntersecton of three spheres yelds two solutons n generl (only one soluton f the spheres meet tngentlly, nd zero solutons f the center dstnce s too gret for the gven sphere rd l n ths ltter cse the soluton s mgnry nd the nput dt s not consstent wth Delt Robot ssembly). he spheres-ntersecton lgorthm clcultes both soluton sets nd t s possble to utomtclly mke the computer choose the correct soluton by ensurng t s below the bse trngle rther thn bove t. hs three-spheres-ntersecton pproch to the FK for the Delt Robot yelds results dentcl x y z gven Θ. to solvng the three knemtcs equtons for R.L. Wllms II, J.S. Albus, nd R.V. ostelmn, 004, D Cble-sed Crtesn Metrology System, Journl of Robotc Systems, (5): 7-57.

15 5 Revolute-Input Delt Robot Velocty Knemtcs Equtons he revolute-nput Delt Robot velocty knemtcs equtons come from the frst tme dervtve of the three poston constrnt equtons presented erler: Lycos L( y ) sn Lzsn Lz cos xx( y ) y zz 0 L( x y)cos L( ( x b) y c) sn Lzsn Lz cos ( x b) x( y c) y zz 0 L( x y)cos L( ( xb) yc) sn Lz sn Lz cos ( x b) x( y c) y zz 0 Re-wrtten: xx( y ) y Lycos zzlz sn L( y ) sn Lz cos ( xb) x( yc) y L( x y)cos zz Lzsn L( ( xb) yc) sn Lz cos c) sn Lzcos ( xb) x( yc) yl( x y)cos zzlzsn L( ( xb) y Wrtten n mtrx-vector form: AX = Θ x ylcos zlsn x b 0 0 ( x b) Lcos ( y c) Lcos ( z Lsn ) y 0 b 0 ( x b) Lcos 0 0 ( y c) Lcos ( z Lsn ) z b where: b L ( y)sn zcos b L( ( x b) y c)sn zcos b L( ( x b) y c)sn zcos

16 Revolute-Input Delt Robot oston Knemtcs Exmples For these exmples, the Delt Robot dmensons re from the tble gven erler for the A Flexcker IR 60-/600, s = 0.567, s = 0.076, L = 0.54, l =.44, nd h = 0. (m). Inverse oston Knemtcs Exmples Snpshot Exmples Nomnl oston. Gven results re (the preferred knked out soluton): Θ Generl oston. Gven knked out soluton): m, the clculted IK m, the clculted IK results re (the preferred Θ Generl Inverse oston Snpshot Exmple

17 7 IK rjectory Exmple he movng pltform control pont {} trces n XY crcle of center 0 0 m nd rdus 0.5 m. At the sme tme, the Z dsplcement goes through complete sne wve motons centered on Z m wth 0. m mpltude. hs IK trjectory, t the end of moton, s pctured long wth the smulted Delt rllel Robot, n the MALA grphcs below. XY Crculr rjectory wth Z sne wve

18 8 Commnded IK Crtesn ostons Clculted IK Jont Angles

19 Forwrd oston Knemtcs Exmples Snpshot Exmples Nomnl oston. Gven Θ , the clculted FK results re (the dmssble soluton below the bse, usng the equl-z-heghts spheres ntersecton lgorthm): m Generl oston. Gven Θ m, the clculted FK results re (the dmssble soluton below the bse, usng the non-equl-z-heghts spheres ntersecton lgorthm): m Generl Forwrd oston Snpshot Exmple Crculr Check Exmples All snpshot exmples were reversed to show tht the crculr check vldton works;.e.: When gven , the IK soluton clculted Θ nd when gven , the IK soluton clculted Θ Θ , the FK soluton clculted Θ , the FK soluton clculted When gven nd when gven

20 0 FK rjectory Exmple he IK soluton s more useful for Delt Robot control, to specfy where the tool should be n XYZ. hs FK trjectory exmple s just for demonstrton purposes, not yeldng ny useful robot moton. Smultneously the three revolute jont ngles re commnded s follows: () t mxsn() t () t mxsn() t () t sn() t mx where mx 45 nd t proceeds from 0 to n 00 steps. hs closed FK trjectory, t the end of moton, s pctured long wth the smulted Delt rllel Robot, n the MALA grphcs below FK rjectory

21 Commnded FK Jont Angles Clculted FK Crtesn ostons

22 rsmtc-input Delt Robot rsmtc-input Delt Robot Descrpton he rsmtc-input Delt Robot s fundmentlly smlr to the orgnl Revolute-Input Delt Robot. he mjor dfference s tht the three nputs now re drven by three lner-sldng prsmtc jonts nsted of three revolute jonts. hs desgn chnge smplfes the knemtcs equtons nd the IK nd FK equtons nd solutons sgnfcntly, becuse the three prsmtc nputs re lgned wth the {} frme Z xs, nd there re no snes nor cosnes requred s n the revolute-nput cse. As shown below, the -dof prsmtc-nput Delt Robot s composed of three dentcl UU legs n prllel between the top fxed bse nd the bottom movng end-effector pltform. he control vrbles re L,,,. In ths model postve chnge n L s downwrd, n the Z drecton. he three-dof Delt Robot s gn cpble of XYZ trnsltonl control of ts movng pltform wthn ts workspce. he jont vrbles re L L L L, nd the Crtesn vrbles re x y z. Delt Robot Knemtc Dgrm rsmtc Inputs hs robot s lso known s the Lner Delt Robot or the Lner-Rl Delt Robot. he constnt geometry (bse ponts, pltform vertces, etc.) presented erler for the revolute-nput Delt robot stll pply to the prsmtc-nput Delt Robot. Also, the Moblty (dof) clcultons for the prsmtc-nput cse re dentcl to tht presented for the revolute-nput cse, yeldng M =. he rsmtc-input Delt Robot fxed bse nd pltform geometrc detls re shown on the next pge.

23 rsmtc-input Delt Robot Fxed se Detls rsmtc-input Delt Robot Movng ltform Detls

24 he fxed-bse prsmtc jont ponts re constnt n the bse frme {} nd the pltformfxed U-jont connecton ponts re constnt n the bse frme {}: 4 R R cos 0 R sn 0 R 0 0 R R cos0 Rsn0 R 0 0 R cos90 0 Rsn90 R 0 0 s w w 0 s 0 0 u 0 he vertces of the fxed-bsed equlterl trngle re: s b w b w b 0 s 0 0 u 0 where: w 6 s u s w 6 s u s nme menng vlue (mm) s bse equlterl trngle sde 4 R bse rdus from orgn to jonts ( ) 4 s pltform equlterl trngle sde 7 L mn =,, mnmum prsmtc jonts lengths 67 L mx =,, mxmum prsmtc jonts lengths 479 l lower legs prllelogrm length 64 h lower legs prllelogrm wdth 44 w plnr dstnce from {0} to ner bse sde 5 u plnr dstnce from {0} to bse vertex 49 w plnr dstnce from {} to ner pltform sde 7 u plnr dstnce from {} to pltform vertex 7 o X nozzle X offset 0 o Y nozzle Y offset 0 H frme heght 686

25 he model vlues bove re for specfc commercl prsmtc-nput delt robot D prnter, the Delt Mker (deltmker.com). hough Delt Robot symmetry s ssumed, the followng methods my be dpted to the generl cse for the prsmtc-nput delt robot. Note tht ech ndvdul prsmtc length lmts re 67 L 479 mm; however, for ll three prsmtc lengths equl, L 54 mm s the mxmum extent to vod moton through the D prntng surfce. 5 rctcl rsmtc-input Delt Robots forums.reprp.org deltmker.com

26 6 rsmtc-input Delt Robot Knemtcs Anlyss From the knemtc dgrm bove, the followng three vector-loop closure equtons re wrtten for the Delt Robot: L l R,, he three pplcble constrnts stte tht the lower leg lengths must hve the correct, constnt length l (the vrtul length through the center of ech prllelogrm): l l L,, It wll be more convenent to squre both sdes of the constrnt equtons bove to vod the squre-root n the Euclden norms: nd L erler: l l l l l,, x y z Agn, the Crtesn vrbles re x y z were gven prevously. he vectors L L L. he constnt vector vlues for ponts L re dependent on the jont vrbles ; these formuls re much smpler thn those for the rottonl-nput cse presented 0 L 0,, L Substtutng ll bove vlues nto the vector-loop closure equtons yelds: x l y l yc l z L x b z L x b yc z L where: w u s b w cw w And the three constrnt equtons yeld the knemtcs equtons for the prsmtc-nput Delt Robot:

27 7 x y z L zl yl 0 x y z b c L xb yc zl l 0 x y z b c L xb yc zl l 0 he three bsolute vector knee ponts re found usng A L,,, : 0 A w A w A L w L w w L Inverse oston Knemtcs (IK) Soluton he -dof prsmtc-nput Delt Robot nverse poston knemtcs (IK) problem s stted: x y z, Gven the Crtesn poston of the movng pltform control pont (the orgn of {}), clculte the three requred ctuted prsmtc jont ngles L L L L. he IK soluton for the prsmtc-nput Delt Robot s much smpler thn tht for the revolute-nput Delt robot presented erler nd s esly found nlytclly. Referrng to the prsmtc-nput Delt Robot knemtc dgrm bove, the IK problem cn be solved ndependently for ech of the three UU legs. Geometrclly, ech leg IK soluton s the ntersecton between vertcl lne of unknown length L (pssng through bse pont ) nd known sphere (rdus l, centered on the movng pltform vertex ). hs soluton my be done geometrclly/trgonometrclly. However, we wll now ccomplsh ths IK soluton nlytclly, usng the three constrnt equtons ndependently (derved prevously). he three ndependent sclr IK equtons re qudrtc equtons of the form: L zl C 0,, where: C x y z yl C x y z b c xb yc l C x y z b c xb yc l So we smply hve three ndependent qudrtc equtons to solve for the prsmtc-length nputs L, for ech leg ndependently, where A =, = z, nd the C re gven bove. he IK soluton smplfes qute ncely:

28 8 L z z C,, wo L solutons result from the n the qudrtc formul. hese solutons cn be referred to s knee up nd knee down for ech leg. hs yelds two IK brnch solutons for ech leg of the prsmtc-nput Delt Robot, for totl of 8 possble solutons. Generlly the one overll soluton wth ll knees up wll be chosen. When z C, the soluton for L s mgnry. hs cse should never occur n theory snce the prsmtc jont cn extend s fr s needed to mntn rel soluton for ech leg. However, n prctce, there re of course prsmtc jont lmts. When z C, the two soluton brnches (knee up nd knee down) hve become the sme soluton. he IK nput xyz s for the movng pltform geometrc center. When the desred control pont s offset from the center (s n the cse of mny D prnters), n ntl trnsformton s requred pror to mplementng the IK soluton: NN where 0 0 OX 0 0 O Y N nd 0 0 O X 0 0 O Y N hs s very smple trnsformton snce the prsmtc-nput Delt Robot llows only trnsltonl moton, wth R NR I. o sve lot of computtons wth s nd 0s, smply subtrct O X nd O Y from the x nd y components, respectvely, of the gven (xyz) N to obtn the IK movng-pltformcenter nput xyz. N stnds for nozzle n D prnter. he z component s unchnged n ths trnsformton.

29 9 Forwrd oston Knemtcs (FK) Soluton he -dof prsmtc-nput Delt Robot forwrd poston knemtcs (FK) problem s stted: Gven the three ctuted jont ngles L L L L movng pltform control pont (the orgn of {}),, clculte the resultng Crtesn poston of the x y z. he FK soluton for prllel robots s generlly very dffcult. It requres the soluton of multple coupled nonlner lgebrc equtons, from the three constrnt equtons ppled to the vector loop-closure equtons (derved prevously). Multple vld solutons generlly result. hnks to the trnslton-only moton of the -dof Delt Robot, there s strghtforwrd L re gven, we clculte the three bsolute vector knee ponts usng A L,,,. Referrng to the prsmtc-nput Delt Robot FK dgrm below, snce we know tht the movng pltform orentton s constnt, lwys horzontl wth R I, we defne three vrtul sphere centers A A,,, : nlytcl soluton for whch the correct soluton set s esly chosen. Snce L L L v 0 A v w u A v w w A v w w L s w L s w L nd then the prsmtc-nput Delt Robot FK soluton s the ntersecton pont of three known spheres. Let sphere be referred s vector center pont {c} nd sclr rdus r, ({c}, r). herefore, the FK unknown pont s the ntersecton of the three known spheres: ( A v, l) ( A v, l) ( A v, l).

30 0 Delt Robot FK Dgrm rsmtc Inputs Appendx A presents n nlytcl soluton for the ntersecton pont of the three gven spheres, from Wllms et l. 4 hs soluton lso requres the solvng of coupled trnscendentl equtons. he ppendx presents the equtons nd nlytcl soluton methods, nd then dscusses mgnry solutons, sngulrtes, nd multple solutons tht cn plgue the lgorthm, but ll turn out to be no problem n ths desgn. In prtculr, wth ths exstng three-spheres-ntersecton lgorthm, f ll three gven sphere centers A v hve the sme Z heght ( common cse for the Delt Robot), there wll be n lgorthmc sngulrty preventng successful soluton (dvdng by zero). One wy to fx ths problem s to smple rotte coordntes so ll A v Z vlues re no longer the sme, tkng cre to reverse ths coordnte trnsformton fter the soluton s ccomplshed. However, we present nother soluton 4 R.L. Wllms II, J.S. Albus, nd R.V. ostelmn, 004, D Cble-sed Crtesn Metrology System, Journl of Robotc Systems, (5): 7-57.

31 (Appendx ) for the ntersecton of three spheres ssumng tht ll three sphere Z heghts re dentcl, to be used n plce of the prmry soluton when necessry. Another pplcble problem to be ddressed s tht the ntersecton of three spheres yelds two solutons n generl (only one soluton f the spheres meet tngentlly, nd zero solutons f the center dstnce s too gret for the gven sphere rd l n ths ltter cse the soluton s mgnry nd the nput dt s not consstent wth prsmtc-nput Delt Robot ssembly). he spheres-ntersecton lgorthm clcultes both soluton sets nd t s possble to utomtclly mke the computer choose the correct soluton by ensurng t s below the bse trngle rther thn bove t. he FK soluton xyz s for the movng pltform geometrc center. When the desred control pont s offset from the center (s n the cse of mny D prnters), further trnsformton s requred fter to mplementng the FK soluton: N N where N ws gven prevously wth the IK soluton. hs s very smple trnsformton snce the prsmtc-nput Delt Robot llows only trnsltonl moton, wth R NR I. o sve lot of computtons wth s nd 0s, smply dd O X nd O Y to the x nd y components, respectvely, of the FK movng-pltform-center soluton xyz, to obtn the desred FK soluton (xyz) N. he z component s unchnged n ths trnsformton.

32 Alternte FK Soluton hs three-spheres-ntersecton pproch to the FK for the prsmtc-nput Delt Robot yelds results dentcl to solvng the three knemtcs equtons for x y z gven L L L L. Snce the three constrnt equtons for the prsmtc-nput Delt Robot re much smpler thn those for the revolute-nput Delt Robot, we now present n lterntve FK nlytcl soluton. he three constrnt equtons re repeted below: x y z L zl yl 0 x y z b c L xb yc zl l 0 x y z b c L xb yc zl l 0 Subtrctng the second equton from the thrd equton yelds lner equton, expressng x s functon of z only: x f( z) dz e where: d L L b e L L 4b Further, subtrctng the frst equton from the thrd equton nd substtutng x f( z) from bove yelds nother lner equton, expressng y s functon of z only: y g( z) Dz E where: D LL bd c E be b c L L ( c ) Substtutng both x f( z) nd y g( z) nto the frst equton yelds sngle equton n one unknown z, qudrtc polynoml: Az z C 0 where: A d D ( dedel D) C e E L El And so the lternte nlytcl FK soluton s:

33 z, 4 A AC x f ( z ) dz e,,, y gz ( ) Dz E,,, here re two possble soluton sets ( x, y, z ) nd ( x, y, z ), due to the n the qudrtc formul. Generlly only one soluton wll be used, the one where xyz s below the top-mounted bse trngle. rsmtc-input Delt Robot Velocty Knemtcs Equtons he prsmtc-nput Delt Robot velocty knemtcs equtons come from the frst tme dervtve of the three poston constrnt equtons presented erler: xx( y) y( zl) z ( zl) L ( x bx ) ( ycy ) ( zl) z ( zl) L ( x bx ) ( ycy ) ( zl) z ( zl) L Wrtten n mtrx-vector form: AX = L x y zl x zl 0 0 L x b yc zl y 0 z L 0 L x b yc zl z 0 0 zl L

34 rsmtc-input Delt Robot oston Knemtcs Exmples For these exmples, the prsmtc-nput Delt Robot dmensons re from the tble gven erler for the Delt Mker D rnter, s = 0.4, R = 0.4, s = 0.7, l = 0.64, nd h = (m). Inverse oston Knemtcs Exmples Snpshot Exmples Nomnl oston. Gven results re (the preferred lower soluton): L m Generl oston. Gven knked out soluton): m, the clculted IK m, the clculted IK results re (the preferred L m Generl Inverse oston Snpshot Exmple

35 5 IK rjectory Exmple he movng pltform control pont {} trces n XY crcle of center m nd rdus 0. m. At the sme tme, the Z dsplcement goes through complete sne wve motons centered on Z m wth 0.05 m mpltude. hs IK trjectory, t the end of moton, s pctured long wth the smulted Delt rllel Robot, n the MALA grphcs below. XY Crculr rjectory wth Z sne wve

36 6 Commnded IK Crtesn ostons Clculted IK Jont Angles

37 Forwrd oston Knemtcs Exmples Snpshot Exmples Nomnl oston. Gven 7 L m, the clculted FK results re (the dmssble lower soluton, usng the equl-z-heghts spheres ntersecton lgorthm): m Generl oston. Gven L m, the clculted FK results re (the dmssble lower soluton, usng the non-equl-z-heghts spheres ntersecton lgorthm): m Generl Forwrd oston Snpshot Exmple Crculr Check Exmples All snpshot exmples were reversed to show tht the crculr check vldton works;.e.: When gven , the IK soluton clculted L nd when gven , the IK soluton clculted When gven L , the FK soluton clculted gven L , the FK soluton clculted L nd when

38 FK rjectory Exmple he IK soluton s more useful for Delt Robot control, to specfy where the D prnter nozzle should be n XYZ. hs FK trjectory exmple s just for demonstrton purposes, not yeldng ny useful robot moton. Smultneously the three prsmtc jont lengths re commnded s follows: Lsn( t) L L() t Lmn Lsn( t) L L() t Lmn Lsn( t) L L() t Lmn where L 0. m nd t proceeds from 0 to n 00 steps. hs closed FK trjectory, t the end of moton, s pctured long wth the smulted Delt rllel Robot, n the MALA grphcs below 8 FK rjectory

39 9 Commnded FK Jont Angles Clculted FK Crtesn ostons

40 40 Acknowledgements hs work ws ntted durng the lst week of the uthor s sbbtcl from Oho Unversty t the Unversty of uerto Rco (UR), Myguez, durng Fll Semester 04. he uthor grtefully cknowledges fnncl support from Rcky Vlentn, UR ME chr, nd the Russ College of Engneerng & echnology t Oho Unversty.

41 4 Appendces Appendx A. hree-spheres Intersecton Algorthm We now derve the equtons nd soluton for the ntersecton pont of three gven spheres. hs soluton s requred n the forwrd pose knemtcs soluton for mny cble-suspended robots nd other prllel robots. Let us ssume tht the three gven spheres re ( c,r ), ( c,r ), nd ( c,r ). ht s, center vectors c x y z, c x y z, c x y z, nd rd r, r, nd r re known (he three sphere center vectors must be expressed n the sme frme, {0} n ths ppendx; the nswer wll be n the sme coordnte frme). he equtons of the three spheres re: x x y y z z r x x y y z z r x x y y z z r (A.) Equtons (A.) re three coupled nonlner equtons n the three unknowns x, y, nd z. he soluton wll yeld the ntersecton pont x y z (A.) nd combne them n wys so tht we obtn x f y nd z f y. he soluton pproch s to expnd equtons ; we then substtute these functons nto one of the orgnl sphere equtons nd obtn one qudrtc equton n y only. hs cn be redly solved, yeldng two y solutons. hen we gn use x f y nd z f y to determne the remnng unknowns x nd z, one for ech y soluton. Let us now derve ths soluton. Frst, expnd equtons (A.) by squrng ll left sde terms. hen subtrct the thrd from the frst nd the thrd from the second equtons, yeldng (notce ths elmntes the squres of the unknowns): x y z b (A.) x y z b (A.) where: x x x x y y y y z z z z b b r r r r x x y y z z x x y y z z Solve for z n (A.) nd (A.): b z x y (A.4) b z x y (A.5)

42 Subtrct (A.4) from (A.5) to elmnte z nd obtn y x f : 4 y 4 y 5 x f (A.6) where: 4 5 b b Substtute (A.6) nto (A.5) to elmnte x nd obtn y z f : y 6 y 7 z f (A.7) where: b 5 Now substtute (A.6) nd (A.7) nto the frst equton n (A.) to elmnte x nd z nd obtn sngle qudrtc n y only: where: 4 b c 5 y by c 0 (A.8) 45 x y 6 7 z x z x y z r here re two solutons for y: b b 4c y (A.9) o complete the ntersecton of three spheres soluton, substtute both y vlues y + nd y - from (A.9) nto (A.6) nd (A.7): x z 4 y 5 (A.0) 6 y 7 (A.) In generl there re two solutons, one correspondng to the postve nd the second to the negtve n (A.9). Obvously, the + nd solutons cnnot be swtched: x y z x y z (A.)

43 Exmple Let us now present n exmple to demonstrte the solutons n the ntersecton of three spheres lgorthm. Gven three spheres (c,r): 0 0 0, 0 0, 5, 4 (A.) he ntersecton of three spheres lgorthm yelds the followng two vld solutons: x y z 0 x y z (A.4) hese two solutons my be verfed by D sketch. hs completes the ntersecton of three spheres lgorthm. In the next subsectons we present severl mportnt topcs relted to ths threespheres ntersecton lgorthm: mgnry solutons, sngulrtes, nd multple solutons. Imgnry Solutons he three spheres ntersecton lgorthm cn yeld mgnry solutons. hs occurs when the rdcnd b 4c n (A.9) s less thn zero; ths yelds mgnry solutons for y, whch physclly mens not ll three spheres ntersect. If ths occurs n the hrdwre, there s ether jont ngle sensng error or modelng error, snce the hrdwre should ssemble properly. A specl cse occurs when the rdcnd b 4c n (A.9) s equl to zero. In ths cse, both solutons hve degenerted to sngle soluton,.e. two spheres meet tngentlly n sngle pont, nd the thrd sphere lso psses through ths pont. Sngulrtes he three spheres ntersecton lgorthm nd hence the overll forwrd pose knemtcs soluton s subject to sngulrtes. hese re ll lgorthmc sngulrtes,.e. there s dvson by zero n the mthemtcs, but no problem exsts n the hrdwre (no loss or gn n degrees of freedom). hs subsecton derves nd nlyzes the lgorthmc sngulrtes for the three spheres ntersecton lgorthm presented bove. Dfferent possble three spheres ntersecton lgorthms exst, by combnng dfferent equtons strtng wth (A.) nd elmntng nd solvng for dfferent vrbles frst. Ech hs dfferent set of lgorthmc sngulrtes. We only nlyze the lgorthm presented bove. Inspectng the lgorthm, represented n equtons (A.) (A.), we see there re four sngulrty condtons, ll nvolvng dvson by zero. Sngulrty Condtons (A.5)

44 44 he frst two sngulrty condtons: z z 0 (A.6) z z 0 (A.7) re stsfed when the centers of spheres nd or spheres nd hve the sme z coordnte,.e. z z or z z. herefore, n the nomnl cse where ll four vrtul sphere centers centers hve the sme z heght, ths three-spheres ntersecton lgorthm s lwys sngulr. An lternte soluton s presented n Appendx to overcome ths problem. he thrd sngulrty condton, 0 (A.8) Smplfes to: x z x z x z x z (A.9) For ths condton to be stsfed, the centers of spheres,, nd must be collner n the XZ plne. In generl, sngulrty condton les long the edge of the useful workspce nd thus presents no problem n hrdwre mplementton f the system s properly desgned regrdng workspce lmttons. he fourth sngulrty condton, Is stsfed when: (A.0) 4 6 (A.) It s mpossble to stsfy ths condton s long s 4 nd 6 from (A.6) nd (A.7) re rel numbers, s s the cse n hrdwre mplementtons. hus, the fourth sngulrty condton s never problem. Multple Solutons In generl the three spheres ntersecton lgorthm yelds two dstnct, correct solutons ( n (A.9 A.)). Generlly only one of these s the correct vld soluton, determned by the dmssble Delt Robot ssembly confgurtons.

45 45 Appendx. Smplfed hree-spheres Intersecton Algorthm We now derve the equtons nd soluton for the ntersecton pont of three gven spheres, ssumng ll three spheres hve dentcl vertcl center heghts. Assume tht the three gven spheres re ( c,r ), ( c,r ), nd ( c,r ). ht s, center vectors c x y z, c x y z, c x y z, nd rd r, r, nd r re known. he three sphere center vectors must be expressed n the sme frme, {} here, nd the nswer wll be n the sme coordnte frme. he equtons of the three spheres to ntersect re (choosng the frst three spheres): n n n ( x x ) ( y y ) ( zz ) r (.) ( x x ) ( y y ) ( zz ) r (.) ( x x ) ( y y ) ( zz ) r (.) Snce ll Z sphere-center heghts re the sme, we hve z z z z n. he unknown threespheres ntersecton pont s x y z. Expndng (-) yelds: x xx x y yy y z z n z z n r (.4) x xx x y yy y z z n z z n r (.5) x xxx y yy y z z n zz n r (.6) Subtrctng (6) from (4) nd (6) from (5) yelds: ( x x ) x( y y ) yx y x y r r (.7) ( x x ) x( y y ) y x y x y r r (.8) All non-lner terms of the unknowns x, y cncelled out n the subtrctons bove. Also, ll z- relted terms cncelled out n the bove subtrctons snce ll sphere-center z heghts re dentcl. Equtons (7-8) re two lner equtons n the two unknowns x, y, of the followng form. bx c d e y f (.9) where: ( x x ) b ( y y ) c r r x y x y d ( x x ) e ( y y ) f r r x y x y he unque soluton for two of the unknowns x, y s:

46 46 ce bf x e bd f cd y e bd (.0) Returnng to () to solve for the remnng unknown z: Az z C 0 (.) where: A z n n ( ) ( ) C z r xx y y Knowng the unque vlues x, y, the two possble solutons for the unknown z re found from the qudrtc formul: z pm, 4C (.) For the Delt Robot, ALWAYS choose the z heght soluton tht s below the bse trngle,.e. negtve z, snce tht s the only physclly-dmssble soluton. hs smplfed three-spheres ntersecton lgorthm soluton for x, y, z fls n two cses: ) When the determnnt of the coeffcent mtrx n the x, y, lner soluton (.0) s zero. ebd ( x x )( y y ) ( y y )( x x ) 0 (.) hs s n lgorthmc sngulrty whose condton cn be smplfed s follows. (.) becomes: ( x x )( y y ) ( y y )( x x ) (.4) If (.4) s stsfed there wll be n lgorthmc sngulrty. Note tht the lgorthmc sngulrty condton (.4) s only functon of constnt terms. herefore, ths sngulrty cn be voded by desgn,.e. proper plcement of the robot bse loctons n the XY plne. For symmetrc Delt Robot, ths prtculr lgorthmc sngulrty s voded by desgn. ) When the rdcnd n (.) s negtve, the soluton for z wll be mgnry. he condton 4C 0 yelds: ( x x ) ( y y ) r (.5) When ths nequlty s stsfed, the soluton for z wll be mgnry, whch mens tht the robot wll not ssemble for tht confgurton. Note tht (.5) s n nequlty for crcle. hs sngulrty wll NEVER occur f vld nputs re gven for the FK problem,.e. the Delt Robot ssembles.