ROBOTIC BACKHOE WITH HAPTIC DISPLAY

Transcription

1 Dni Modling nd Conrol Dsign of ROBOTIC BACOE WIT APTIC DISPLAY Jo Frnl orgi Insiu of Thnolog Aril 4, 3 I. Inroduion A. Bground Th rdiionl hod o onrol hdruli quin hs n olishd wih h us of nul roorionl vlvs. For l, il disl owrd rhoving vhil gnrs hdruli rssur wih u, whih is hnill drivn h ngin. Th u dlivrs hdruli owr o is ilns, suh s ho, lodr, or ugr, vi nul vlvs h r onrolld h oror. A dir hnil onnion iss wn h oror s hnd nd h sool in h vlv hrough lvr nd onning lings. Th sool us disld fro is zro osiion o llow high-rssur fluid o flow fro h u o h lindrs nd us h iln o ov. Eh lvr onrol ihr on or wo dgrs of frdo of h iln. A ho oror us onrol ulil lvrs siulnousl in ordr o rodu h dsird nd-ffor u oion, whih is sill h s i o lrn. Also, fd inforion on h fors rind h u is liid, in h for of olin in h lvrs du o lindr rssur hngs, ngin sd hngs undr lod, nd vhil virion. Th uros of his roj is o lor h viili of ling odrn onrol hniqus o hdruli rhoving quin. Th urrn dsign inluds s fd onrol nd hi for disl o h oror s hnd. I is roosd h wih ror onrol ss dsign, h iln s rforn nd usr inrf n oh irovd for grr roduivi nd shorr oror rining i, wihou signifin inrss in nufuring oss. B. Th Rooi Bho wih i Disl Figur illusrs h ss undr invsigion. Th John Dr Modl 47 ho hn is ound on 44 ror. Th sr niulor is h Prsonnl i Inrf Mhnis PANTOM, hrfr rfrrd o s h sr, dvlod MIT

2 nd rodud orill Snsl Thnologis. Addiionl roosd oonns inlud solnoid vlvs, ngl nodrs, nd PC-4 onrollr. Figur : Rooi Bho wih i Disl Dsird u osiion PANTOM Msr osiion Digging for & ring rror for Conrollr olg o oil Lin osiions lvs Posiion Snsors druli oil flow Join ngls Bho Bu osiion Figur is lo digr of h ss undr onsidrion. Th dsird u osiion is dfind s h inu nd h ul u osiion s h ouu. Svrl silifiions nd ssuions will d o h ss shown in Figur. A dni s s odl will drivd, onil onrollr will roosd, nd siulion rsuls will rsnd. II. Coonn Modling A. lv nd Clindr Figur illusrs n lrill ud roorionl vlv onnd o hdruli lindr. Figur : lv nd Clindr Sool disln s Rurn Pu s Q i A A = s w Rurn

3 Whn h vlv sool is disld fro s =, sris of vns our, whih n dsrid h following quions. Assuing inorssil fluid nd onsrvion of ss, h flow hrough h vlv ino h lindr is A A Q i whr A dnos h ross sion of h oning wn h sool nd vlv od, A = s w nd w is h sool s widh. A is h lindr r, nd r h fluid vloiis, nd Q i is h flow r. Assuing invisid fluid nd no nrg losss wn oins nd, onsrvion of nrg long srlin rquirs h s whr s nd r h sul rssur nd lindr rssur, nd is h fluid dnsi. Coining quions nd nd solving for h flow r rsuls in Q i A s f, s 3 A w s whih shows h h flow r ino h lindr is funion of sool disln nd lindr rssur, ssuing onsn sul rssur fro h u. Equion 3 n linrizd ou oring oins s nd, Q i Q s, Q s s s i 4 s, s whr h snsiivi of fluid flow wih rs o lindr rssur is ngiv nd is h diffrnil rssur ross h lindr us h quiliriu rssurs r qul. Ngling h dnis of h vlv sool, h sool osiion is ssud o roorionl o h driving volg sn o h oil: 5 s oil oil 3

4 B. Rod, Bu nd Soil Ngling h inis of h ho s lins, h rod nd u will odld s singl rigid ss, s illusrd in Figur 3. Figur 3: Rod nd Bu A f Aling Nwon s sond lw on h rod/u ss, quion 6 dsris h oion of h u: A f 6 whr h ross-sionl r of h rod hs n ngld nd rssur ing on h lindr. is h n hdruli Th soil is odld s ssiv olin: f 7 No h h vlus of nd will vr drill, dnding on h soil s oosiion, dnsi, oisur onn,. For his siulion, h vlus of = 7 Ns/ nd =5 3 N/ hv n usd. 4

5 C. Bu Dnis Susiuing quions nd 5 ino 4 givs oil oil A 8 whr. Solving 8 for h diffrnil lindr rssur, oil oil A 9 Susiuing quions 9 nd 7 ino 6, oil oil A A nd hn rrrnging rsuls in h dni quion of h u oion in rs of h volg inu o h oil in h vlv: oil oil A A or or sil oil Th offiins in n found in insion. D. Th un Oror Th hun oror will odld s if roviding roorionl rror fd, whr h for rd on h sr is roorionl o h rror wn h dsird nd ul u loion: ds usr hnd F 3 5

6 E. i Disl fors Two fors r rrsnd nd disld hill o h usr s hnd h sr. Th firs for is h digging for, whih is roorionl o h diffrnil lindr rssur: F for 4 for Th sond for is h ring rror for, whih is roorionl o h ring rror wn h sr osiion nd u osiion, sld h wors sling rio: F os os 5 sl whr is h sr osiion nd sl is h rio of h u wors o h sr wors. F. Phno Dnis Figur 4 shows h fors ing on h sr. Th inu for Fhnd is sulid h oror, nd F nd F r luld h onrollr nd rrsnd h for uors in h sr. Th ding for ddd o irov sili. os Figur 4: Fors on h sr rsising h sr vloi hs n inu for fro oror F hnd digging for F for ring rror for F os Aling Nwon s nd Lw o h sr ilds F hnd F F 6 for os Susiuing quions 3, 4, nd 5 ino 6 ilds 6

7 sl os for ds usr 7 Susiuing quion 9 ino 7 nd solving for h sr lrion ilds h quion of oion of h sr in rs of h dsird u osiion ds nd h volg o h vlv oil : oil ds usr oil oil for usr os for sl os A 8 or or sil, ds oil nd h offiins in 9 n found in 8 insion. No h his is woinu ss, ds fro h oror nd fro h -o--drind onrollr, nd h sr s dnis r ould wih h u s dnis vi h hi disl fors. oil III. Ss Modling A. On-Loo S S Modl Dfining h s vrils 4 3,,, nd using quions nd 9, h oninuous i s s quions for h onloo ln r ds oil or 7

8 A Bu C B. Disrizion Th disr i s ris nd orrsonding o h oninuous i s ris r found solving h onvoluion ingrl ovr on i s T: whr T AT A T T Bd u T 3, A Bd 4 A T T nd h inu is ssud onsn ovr h inrvl. For linr i-invrin ss, h A s rnsiion ri n found ihr wih h Ll Trnsfor hod, A L si A or Tlor Sris nsion 5 A A A n d I A n d n!! n 6 rsuling in h disr s ri quion u 7 whr h i s T is ilii nd hs n rovd for lri. In his ror, h Ml funion d hs n usd o ou h vlus of nd. C. loi Osrvr I is ssud h h sr nd u will oh ossss osiion snsors llowing for dir fd onrol on nd. I is lso ssud h nihr of h vloiis nd will vill; hrfor, n osrvr will rquird o rovid fd on hs ss. Priioning quion ino surl nd isurl ss nd dfining h ouu s h surl ss, h on-loo ln os 8

9 [ I ] oil ds 8 whr h surl ss r T T. Th ss o osrvd is nd h isurl ss r 9 oil ds Rrrnging h firs suri quion in 8 rsuls in 3 oil d Th vloi s osrvr is dfind s: L oil whr is rli of h isurl ss nd h ls r on h righ orrs for osrvr inuris. Susiuing quion 3 ino 3, rling + wih + nd rrrnging givs ds 3 L L oil L ds L L Equion 3 nno dirl ugnd ino h s ri us of h r + on h righ hnd sid. To llvi his rol, dfin 3 L 33 hn L L L L L oil ds nd rrrnging 9 34

10 nd L L L L L L L oil ds 35 L 36 whr ws rld wih in 3. This will usd lr for fd on h isurl vloi ss. In h siulions h follow, h osrvr gins L wr hosn suh h h osrvr hiid dd rsons, i.. h ols of -L wr ld h origin in h z-ln. D. Ingrd Error Dfin nw s h sus ll of h rrors wn h sld sr osiion nd u osiion fro h sring oin o h rvious i s: i i i 37 sl hn h n vlu of will 38 sl Dfining n rror vor, sl E 39 quion 38 os E E 4 E. Augnd S Equion Coining quions 8, 35, nd 4 rsuls in h ugnd s quion, now inrsd in ordr hr fro h osrvr nd ingrion quions, o ol of svn ss:

11 ds u oil u r E ] [ 4 whr L L L L, r L, u L, nd fro 35. Thus h isurl ss r vill vi h osrvr in quion 4, vi quion 36, nd h ingrd su of h rrors r vill s. L u I. Conrollr Dsign Equion 4 n wrin or ol s ds oil o 4 Th finl s h rins is o rl h inu o h ln o h ohr s vrils. Dfin h onrol lw s oil i sl I sl v sl oil i i 43 whih is for of PID onrollr. Susiuing quion 39 ino 43 nd using h riiond ss ilds E E I v oil 44 nd susiuing quion 36 ino 44 nd rrrnging givs E EL E I v v oil 45

12 Finll, susiuing 45 ino 4 ilds h finl for of h s quion, inluding vloi osrvr, full s fd, nd PID onrol: o C ds 46. Siulion A. Prr nd in Slion Th ss drivd ov ws odd ino Ml using h ss rrs nd gins lisd in Tl. Tl : Ss Prrs nd Conrol ins ril Dsriion lu Unis Ss Prrs A Clindr r.8 Rod ss 5 g Bu ss 5 g lv snsiivi o sool osiion 5 /s lv snsiivi o rssur hngs /Ps oil Sool osiion/volg gin.4 / Clindr friion. Ns/ Soil ding offiin -7 Ns/ Soil sring offiin 5-3 N/ Mss of sr.5 g Conrol ins sl Wors sling rio / Msr ding offiin 5 Ns/ usr Oror for/rror gin N/ os i ring rror gin N/ for i digging for gin 6-9 N/P Proorionl s fd gin 3 / v loi s fd gin 4 s/ I Ingrl s fd gin 6 / s Ths vlus found in h Ml od givn h nd of h ror.

13 B. S Rsons Using h vlus lisd in Tl, h ss rsons o s inu ds = is illusrd in Figur 5. Figur 5: Ss S Rsons Figur 5 illusrs h osiions of h u nd sr, s wll s h driving volg sn o h vlv oil onrol ffor nd h sid lindr rssur in rsons o s inu. Phsill, his would rrsn h u ing ford on r downwrd ino h soil, sring ground lvl =. No h h ss rhs sd s in roil fiv sonds nd hiis no sd s rror. Also, no h h finl osiion of h sr is h s s h sld osiion of h u, whr sl =.83==. Th lindr rssur is oud fro quion 9, whr h sid lindr vloi hs n usd fro h osrvr, L L.,, Figur 6 illusrs h osiion rror nd ingrd rror during h siulion. Th rror is dfind s h diffrn wn h u osiion nd h sld sr osiion, rror= sl -. 3

14 Figur 6: Ss Error Figur 6 illusrs h h siuld ss hiis zro sd s rror du o h oinion of fd on h wo rror signls. C. Ss Rousnss o Prr riions In ri, i is d h h ss rrs os lil o vr will h soil roris. Figur 7 illusrs h s rsons of h ss fr drs for of n in h sring nd ding offiins of h soil. Figur 7: Ss S Rsons fr soil ror hng. Thus h siuld ss is rous o hngs in soil roris. 4

15 I. Conlusions A silifid dni odl of h Rooi Bho wih i Disl hs n drivd, PID onrollr hs n dsignd, nd siulions rsnd. A his i, lhough h rsuls r inonlusiv s o whhr ho s rforn n irovd, h r roising. Currnl, n of h ss oonns hv o sifid. Thrfor, h ss rrs lisd in Tl hd o hosn judiiousl fro logs, nuls, nd rough surns on h ho. In h nd, os of h vlus usd wr siions, nd gins wr hn hosn o rodu dsirl rsons. Th ss rrs will nd o udd nd h gins djusd on h vlvs, snsors, nd ohr quin os vill. For svrl rsons, i is susd h h grs onriuor o odling inuris will in h vlv. Firs, sin h flow quion hs n linrizd ou noinl oring oin, n signifin dviion fro his oin will onriu o rror. Sond, i is d h h vlv will in surion i.. iu flow nd h nd of h sool s rvl during uh of is orion. This for hs n ound for in quion 4 inrsing h flow snsiivi o =5 /s, vlu h is ovrsid sd uon h flow rs givn for il roorionl vlvs in his PM rng. Third, os lowr-nd roorionl vlvs hv ddnd suh h h vlv hiis zro flow ovr rng round zro, nd onsqunl nno rvrs flow insnnousl. Finll, h rsons i of n roorionl vlvs is on h ordr of 5 s, so h ho s sd will liid his for in ddiion o h iu flow r. Soil roris r d o vr dnding on oosiion, dnsi, oisur onn,. In ddiion, h ssiv olin odl givn in quion 7 will vr dnding on h gor of h u. Th n s is o nd h odl o inlud 3-D lin s inis, siul h ss dnis ouing h gori Join, nd solving h forwrd dnis quions vr i s. Muh of his wor hs n rford, nd onl nds o inorord wih h onrol dsign onind hrin. Th rsul of his ndd odl will rinl n iorn ilson in h dvlon of h Rooi Bho wih i Disl. 5

16 II. Ml Cod % BACOE -D POSITION CONTROL w/aptic FORCE FEEDBAC los ll ; lr ll ; l ; % Josi rrs =.5; =5; for=6-9; os=; sl=; usr=; % PID Conrollr Prrs T=.; P=3; =4; I=6; % lv rrs oil=.4; =5; =-4-9; % Clindr rrs A=.8; =.; =5; % ~4" di lindr % Bu rrs =5; % Soil rrs =53 =7; % Siulion i vor =:T:*T; % Dfin onsns/offiins =A^/-+/+; =-/+; =-A**oil/+*; 3=-/; 4=-os*sl/; 5=-for*A/*; 6=os-usr/; =for**oil/*; 3=usr/; % Dfin oninuous i ss A=[ ; ; ; ;]; B=[ ; ; ; 3]; C=[ ]; D=; ss=ssa,b,c,d; % Disriz ss ssd=dss,t; 6

17 [ C D]=ssdssd; % Priion ino surl nd isurl s quions =:,:; =:,3:4; =3:4,:; =3:4,3:4; =:,; =3:4,; =:,; =3:4,; % Pl ols for dd osrvr L=l,,[ ]; % Dfin osrvr suris r=-l*; =r*l+-l*; u=-l*; u=-l*; % Dfin Ingrl of Error s E=[- sl]; % Dfin ss ris w/o fd onrol o=[ zros,3; zros,3; zros r [ ]'; E zros,4 ;]; =[;;u;]; =[;;u;]; % Dfin PID Conrollr =[P*E+*E*L *E I]; % Dfin losd-loo disr-i ss C=[ zros,6; zros,5; ; L, L, ; zros,6 ; E zros,5;]; l=o+*; ssd=ssl,,c,,t; olssd % =====Siul s rsons===== ds=ons,lngh; [ s]=lsissd,ds,; % Assl ouu =:,; =:,; oil=:,3; hd=:,4; sig=:,5; rr=:,6; =A/*hd-*oil/*oil; % Plo rsuls figur sulo,, sirs,rr 7

18 il'posiion Error' ll'rror []' grid on sulo,, sirs,sig il'ingrd Error' ll'\sig [*s]' grid on figur sulo4,, sirs,,'r' il'bu Posiion' ll'posiion []' grid on sulo4,, sirs,,'' il'msr Posiion' ll'posiion []' grid on sulo4,,3 sirs,oil,'g' il'olg o oil' ll' o_i_l []' grid on sulo4,,4 sirs,,'' il'clindr rssur' ll'_ [P]' grid on ll't [s]' 8