Static compensation ZMP algorithm preventing tips-over of a tele-operation excavator

Transcription

1 Statc copensaton ZMP algorth preventng tps-over of a tele-operaton ecavator Bo-Hyun Yu *, K-Young Park, Kyung-Don Lee, and Chang-Soo Han 2 Departent of Robot Technology, Insttute for Advanced Engneerng, Young-n, Korea 2 Departent of Mechancal Engneerng, Hanyang Unversty, An-san, Korea * Correspondng author (zzgee@ae.re.kr) Purpose Research on tele-operated ecavators that protect the operator fro the rsk of tp-over ecavators n hazardous workng areas has ncreased n popularty. The tele-operated ecavator s able to protect workers fro rsks n hazardous workng areas, but the operator cannot drectly access the tp-over nforaton fro the tele-operated ecavator. We propose the statc copensaton ZMP (Zero Moent Pont) algorth for preventng ecavator tp-overs. Method Frstly, kneatc and knetc analyss of the ecavator was perfored. Secondly, statc copensaton ZMP algorth, whch uses ZMP algorth to deterne gat stablty of a bped walkng robot, was developed to prevent tps-over of teleoperaton ecavator. Statc copensaton ZMP algorth nzes ZMP-error due to rapdly changng ecavator acceleraton usng the center of gravty through a statc copensaton algorth. Fnally, the result of the proposed algorth s sulated by RecurDyn odel wth Matlab Sulnk co-sulaton ethod. Results & Dscusson In the sulaton result ZMP has been copared to statc copensaton ZMP-algorth usng an ecavator dynac odel. Fro these results we see that the ZMP value s bgger than the statc copensaton ZMP. Ths eans that the general ZMP s dsturbed due to rapdly changng ecavator acceleraton. Ths proble s nzed by the statc copensaton ZMPalgorth. Keywords: ecavator, tps-over, ZMP (Zero Moent Pont), CoG(Center of Gravty) INTRODUCTION In ost of constructon and cvl engneerng works, hydraulc ecavators are used. Hydraulc ecavators can apply to dverse knds of works, and copared wth other equpent tes, they offer hgh econoc value and unversalty and thus contnue to be wdely used. For nstance, ecavators represent over 30% of heavy equpent tes that are recently beng used n constructon and cvl engneerng work stes. Ecavators can apply to dverse works, and are heavly used n dangerous areas copared wth other heavy equpent tes. In dangerous areas, whch have unconstructed envronents, workers are always eposed to dangerous accdents. In actualty, ost of accdents n constructon works and cvl engneerng are nvolved wth ecavators. To address these probles, research on teleoperaton ecavators s recently beng conducted actvely to secure the safety of workers n dangerous areas. The tele-operaton ecavators are operated by workers at a short dstance wth vsual confraton. Ths can secure the safety of workers, but lts the work s nforaton on the ecavator. Notably, the worker cannot feel the declne of the ecavator, and thus can hardly epect the tps-over of the ecavator. To address these probles, research s beng actvely conducted to sense the tps-over of the ecavator. The research ethods for sensng the tps-over of the ecavator are classfed nto the statc ethod desgned to trace the CoG(Center of Gravty) of the ecavator and epect the tps-over, and nto the ZMP (Zero Moent Pont) ethod whch consders oent. Frst, the statc ethod, whch uses the CoG of ecavator, s to trace the ecavator's CoM(Center of Mass) changng to ts behavors and to confr whether the ecavator s devated fro the support polygon. However, f the ecavator eperences a sudden behavor change, kneatc causes cannot be consdered and thus errors are created between the CoM and the calculated CoG. To address these probles, research s beng conducted to apply ZMP - whch s used to assess the behavoral stablty n bped walkng robots -3 - to ecavators 4. The ZMP algorth can consder the knetc features assocated wth a sudden ecavator behavor n tracng CoM, thus securng ore accurate traceablty than the ethod of tracng CoG. However, ecavators, whch use hgh-power hydraulc cylnders, generate bg shocks n the drvng unt when t eperences behavoral changes, copared wth bped walkng robots. Ths shock can be seen as force, whch eans a sudden acceleraton change. The conventonal ZMP algorth, whch uses each lnk's CoM(Center of Mass) acceleraton n the Cartesan coordnate space n tracng the entre ecavator CoM, very senstvely responds to acceleraton changes. Thus, the estng ZMP algorth - copared wth when t s used n bped walkng robots -

2 would create great accuracy error n hydraulc ecavators whch eperence sudden acceleraton changes. To address these probles, ths study proposes the statc copensaton ZMP algorth by whch the statc ethod desgned for tracng CoG less affected by sudden acceleraton changes s copensated wth ZMP algorth so to rean robust to acceleraton changes assocated wth shocks. To verfy the proposed algorth, the knetc analyss tool RecurDyn s used to create an ecavator odel, and the created odel s lnked wth Mathworks' Sulnk so as to develop the co-sulaton envronent. Thus, the algorth that reans robust to acceleraton changes due to shocks was sulated. KINEMATIC ANALYSIS Ecavator Kneatcs Analyss Generally, ecavator stablty s deterned by ts total CoM. The ecavator can have support polygon and stablze tself f ts total CoM reans wthn ts support polygon such as floor contact area. However, the ecavator wll lose the floor reacton and ts CoM devates fro ts support ploygon. Ths state s defned as the ecavator's unstable state. Ecavator stablty analyss concerns the dscovery of ts total CoM poston. In ths paper, the ecavator workng syste s assued as a rgd body, and the ecavator cabn s swng oton s not consdered. In order to epress the ecavator behavor on the Cartesan coordnate 5, the ecavator coordnate syste s set as n the Fgure. In the above D-H Paraeters, the oton of the workng syste ts locaton and posture, whch conssts of the boo, ar and bucket, s epressed n ters of each dsplaceent functon of workng systes as n (). T Rot (, α ) Trans(, a ) Rot ( z, θ) Trans( z, d) () c23 s23 0 Lc + Lc Lc s23 c23 0 Ls + Ls Ls T The bucket end-effect's locatons, naely, p, p, φ are created as n (2). P lc + lc + lc Py ls + ls + ls φ θ + θ + θ y (2) If the fnal ecavator jont angles, naely, θ, θ2, θ3 are gven, each as' arbtrary locaton can be evaluated, usng the above epresson. Cylnder Kneatcs Analyss Usng the kneatc analyss, the relatonal epresson was deterned between the ecavator's jont coordnate and Cartesan coordnate. However, the hydraulc ecavator behavor s drven by the hydraulcs cylnders attached to the boo, ar and bucket. Thus, n order to know the jont angle, the relatonal epresson between the jont and cylnder s needed. Fgure 2 show the coordnate syste and paraeters defned to evaluate the relatonal epresson between the boo and the hydraulc cylnder. Fg.. Ecavator Model Coordnate Syste Ths can be epressed n ters of D-H paraeters as n Table. Table. Ecavator Model D-H Paraeters d a α θ θ 2 L 0 0 θ 2 3 L θ 3 4 L Fg.2. Boo Cylnder Coordnate and Paraeters Accordng to the second cosne law, the cylnder and boo kneatc paraeters can be defned as n (3). l l + l 2l l cosα (3) bcyl b b2 b b2 b

3 Thus, the angle α b created by the cylnder can be defned as n (4). l + l l cosαb 2l l α b b2 bcyl b b2 l + l l b b2 bcyl b cos 2lb l b2 (4) Fnally, usng the relatonshp between the boo and the hydraulc cylnder, the followng relatonal epresson (5) between the hydraulc cylnder dsplaceent and jont angle can be evaluated. π θ δ + α + δ b2 b b θ π + δ α δ b2 b b (5) Fg.4. Bucket Cylnder Coordnate and Paraeters Fgure 3 shows the coordnate syste and paraeters defned to evaluate the relatonal epresson between the ar and the hydraulc cylnder. Frst, the hydraulc cylnder and the four-bar lnkage start area can be evaluated wth the sae ethod as the boo. The result s shown as n (7). l l + l 2l l cosα kcyl k k 2 k k 2 k cosα α k l l l 2l l k k 2 kcyl k k2 l l l k k 2 kcyl k cos 2lkl k2 (7) Fg.3. Ar Cylnder Coordnate and Paraeters The ar has a slar structure to the boo, so n the sae ethod as wth the boo, ts rotatonal relatonshp between the hydraulc cylnder and the jont angle can be evaluated. The result s as per the followng. l l + l 2l l cosα acyl a a2 a a2 a la la2 lacyl αa 2la la2 l a la2 l acyl a cos 2la l a2 cos α π δa + αa + δa2 θ2 θ δ + α + δ π 2 a a a2 (6) Fgure 4 shows the coordnate syste and paraeters defned to evaluate the relatonal epresson between the bucket and the hydraulc cylnder. The bucket has a four lnk structure, so t needs a dfferent ethod fro the prevous one. Usng the kneatc constrant condtons, the rotatonal value α k 2, whch changes accordng to the hydraulc cylnder, can be defned as n (8). 2π α + α + β α 2π α β k2 k k2 k2 k k2 (8) The four-bar lnkage can be defned as follows usng the second cosne law, and thus all rotatons wthn the four-bar lnkage can be defned as n (9). l l + l 2l l cosα 2 2 klnk k 2 k 5 k 2 k 5 k 2 lklnk + lk 5 l k 2 k 3 cos 2lklnklk 5 lklnk + lk 4 l k 3 k 4 cos 2lklnklk 4 a a (9) Usng the kneatc constrant relatonshps, the bucket rotaton that we want to know fnally was deterned as n (0).

4 l l + l 2l l cosα kcyl k k 2 k k 2 k cosα k l l l 2l l k k 2 kcyl k k2 l k lk 2 l kcyl αk cos 2lkl k2 2π β + α + α + β θ + π k k3 k4 k2 3 θ β + α + α + β π 3 k k3 k4 k2 (0) Fnally, the jont velocty and acceleraton relatonshp between cylnder and jont angle s calculated by dfferental ethod. dθ d θ θ, θ () dt dt Knetc Analyss In order to apply ZMP for preventng ecavator tpsover, frstly, we need to know CoM(Center of Mass) pont acceleraton each lnk because force and oent s a functon of acceleraton. Thus, the each jonts epresson, deterned above, should be dfferentated nto velocty and acceleraton epresson for CoM pont each lnk. In order to ap the relatonshp between jont coordnate and Cartesan coordnate, Jacoban should be used. Frst, usng the kneatc analyss, the poston func- X F q should be evaluated, and the corre- ton ( ) spondng result s partally dfferentated as uch as the jont varable to obtan Jacoban J( q ). If the result s partally dfferentated agan, the accelera- can be obtaned. Ths can be ton Jacoban J( q) epressed n ters of epresson as n (2). ( ) ( ) X F q df q dq X Jq dq dt X Jq + Jq (2) on the Cartesan coordnate usng the followng epresson. W X W Y W Z X, Y, Z W W W (3) Each lnk's weght s ultpled, and the result s dvded by the total weght. Then, we know the CoG entre ecavator. However, ths ethod does not consder oent effect. ZMP (Zero Moent Pont) The general ZMP(Zero Moent Pont) algorth was used to assess the walkng stablty of bped walkng robots. ZMP s a pont on the floor where the resultant oent of the gravty, the nertal force of the syste and the eternal force s zero 6,7. The ecavator coordnate syste about the nertal coordnate syste s defned as n Fgure 5. Fg.5. The Ecavator Coordnate Syste and The Inertal Coordnate Syste. The appng between nertal coordnate and ecavator coordnate s as n (4) ( X, Y,) T R ( X, Y,) (4) ZMP ZMP ZMP ZMP Fgure 6 shows the relatonshp wth the arbtrary ass syste n the syste coordnate syste. T Usng the above Jacoban analyss, the relatonshp between jont coordnate and Cartesan coordnate was deterned. STATIC COMPENSATION ZMP CoG (Center of Gravty) Generally, the ecavator CoG can be calculated f all ovng lnks are separated, and f the poston and weght of separated lnks are known. If the CoM(Center of Mass) poston and ts total weght of each lnk s known, CoG can be calculated Fg.6. Defnton of vectors for syste The followng equaton of oton at an arbtrary pont on the ground P s defned, whch s acqured by applyng d Alebert prncple.

5 ( ) ( ) r P r + g p + ρ p c ( ) + T M T s p f M j k k p j k (5) The poston vector of the ass center to the pont P s descrbed as n (6). If the pont P s ZMP, the p, y,0 T and the total poston vector s ZMP [ ZMP ZMP ] T oent s M [ 0,0, M ]. P ρ z ( r p) (6) c Generally, Tele-operaton ecavator appled does not have a sensor syste to easure eternal force because eternal force s very hgh power than bped walkng robot and there s no force sensor to easure hgh power. If eternal force does not est, can be deterned as n (7), (8) n ters of, y ZMP coponent. ( + ) ( z + gz) z g z z R ZMP y ( + ) ( z + gz) z g y yz z R ZMP (7) (8) The above equaton, appled to the tele-operaton ecavator, s appled to get the poston of CoG preventng ecavator tps-over. Where, each coponent s ass,, y, z poston coordnates value of the each coponent, g z acceleraton of gravty. Fg.7. Concept of the Statc Copensaton ZMP above calculaton, ( ) dff Fgure 7 shows ZMP and CoG postons that can be obtaned when the snusod s appled to each cylnder of the ecavator accordng to te. If the dfference between ZMP and CoG s evaluated fro the F t can be evaluated. Generally, due to nertal nfluence, ZMP value has always greater value toward the ncreasng oveent drecton than CoG value. And, the acceleraton functon ZMP and the poston su CoG creates great dfference due to nerta, so, f the relevant syste's nstantaneous acceleraton s deterned, the au error scope can be defned. By ths concept, f the dfference Fdff ( t ) between CoG ( ) F t and F () t devates fro the au ZMP error scope, ths can be lted by cylnder au acceleraton specfcaton, Frequency analyss, whch s low-pass-flter, apply to the Fdff ( t) functon to nze hgh frequency nose ore than control FCoG sgnal. After sgnal processng Fdff ( t ) add to ( t ). Fnally, the overall ZMP error can be reduced by statc copensaton ZMP. Fgure. 8 shows the SCZMP(Statc Copensaton ZMP) algorth flow chart. Statc Copensaton ZMP Ecavators, whch use hgh-power hydraulc cylnders, generate great shocks n the drvng unt accordng to changng behavors. The shock created n the drvng unt can be seen as force, whch eans a sudden acceleraton change. Thus, the ZMP algorth, whch uses the acceleraton, senstvely responds to acceleraton changes. In order to address these probles, we propose a ethod to analyze the correlaton between the ecavator CoG and ZMP to reduce ZMP errors. Fg.8. Statc Copensaton ZMP Algorth Flow Chart SIMULATION Co-Sulaton Syste The ecavator odel for sulaton was created by RecutDyn Dynac Sulator, and these paraeters used for sulaton odel shows Table 2.

6 Fg.9. RecurDyn & Matlab Sulnk Co-Sulaton Block Dagra for Ecavator Dynac Sulaton Table 2. Ecavator Model Paraeters for Sulaton Paraeter Value Paraeter Value L sp 200 L c 22.5 L 2 y 0 L 3 d 2 l y 35 l 2 drvng 9. kg l cabn 6.94 kg jont 5.74 boo 7.07 kg α ar.69 kg α 2 bucket 0.9 kg α deg 0.79 deg deg Table 3. Ecavator Cylnder Input Paraeters for Stable Sulaton Input Paraeter Paraeters Te Boo Cylnder Velocty Ar Cylnder Velocty Bucket Cylnder Velocty 0 ~ 3 second -50 ~ 50 /s -20 ~ 20 /s -25 ~ 25 /s Fgure 0. Shows sequence of sulaton process. Red crcle s path of CoM(Center of Mass) respectvely. (a) s dsplaceent graph of boo, ar, and bucket cylnder. (b) s velocty graph of boo, ar, and bucket cylnder. (c) s acceleraton graph of boo, ar, and bucket cylnder. The shock created n the drvng unt, whch eans a sudden acceleraton change, was odeled by Gaussan Dstrbuton Nose n Matlab Sulnk functon block. Ths shock can be seen as force, whch eans a sudden acceleraton change. The ZMP algorth, whch uses the acceleraton, senstvely responds to acceleraton changes. Statc copensaton ZMP algorth was developed by Matlab Sulnk n Fgure 9 whch s very convenent ethod to sulate dynac algorth. Fnally, we sulate CoG, ZMP, and Statc Copensaton ZMP algorth usng Co-Sulaton(RecurDyn & Matlab Sulnk) ethod. Sulaton Confguraton Sulaton nput s snusodal sgnal as each cylnder velocty durng 3 seconds. Table. 3 shows boo, ar, and bucket cylnder velocty nput paraeters. Fg.0. Ecavator Sulaton Sequence and Each Cylnder Dsplaceent, Velocty, and Acceleraton. The odel s carred out cylnder dsplaceent fro the nput. Ths s converted by cylnder kneatcs to each jont angle. Also, jont angular velocty and acceleraton are derved fro dfferental equaton of jont angle. And then, we get the velocty and acceleraton of each lnk CoM pont on Cartesan coordnate syste. Fnally, we get the CoG, ZMP, and Statc Copensaton ZMP result.

7 Sulaton Result Fgure s sulated by addng nose, whch has Gaussan Dstrbuton, to each jont acceleraton. (a) (b) Fg.3. (a) Stable Sulaton Moton, (b) Unstable Sulaton Moton Fg.. Stable Result wth cylnder acceleraton nose Fro the above result, we can see that ZMP value have uch nose ore than SCZMP value. To sulate ecavator stablty when ts CoM s n the support polygon. We change sulaton nput paraeters. Table. 4 shows sulaton nput paraeters. Table 4. Ecavator Cylnder Input Paraeters for Unstable Sulaton Input Paraeter Te Boo Cylnder Velocty Ar Cylnder Velocty Bucket Cylnder Velocty Paraeter 0 ~ 3 second -60 ~ 60 /s -25 ~ 25 /s -30 ~ 30 /s Fgure 2 shows CoM value s out of support polygon area. Fgure 3 shows ecavator stable, and unstable result, respectvely. Fg.2. Unstable Result wth cylnder acceleraton nose Fgure 3 (a) s stable ecavator oton, (b) s unstable ecavator oton. CONCLUSION In ths paper we ntroduced Statc Copensaton ZMP algorth. Ths algorth can be confored that a ore accurate output, copare wth general ZMP algorth. Also we show ecavator tps-over when t s total CoM out of ts support polygon such as floor contact area. ACKNOWLEDGMENT Ths work was supported by the Industral Strategc technology developent progra (004080, Developent of reotely ploted and optonally anned ecavatng syste usng detachable robot anpulator wthout any converson of conventonal ecavator) funded by the Mnstry of Knowledge Econoy (MKE, Korea). References. Kajta, S., Yoko, K., Sago, M., and Tane, K., Balancng a Huanod Robot Usng Backdrve Concerned Torque Control and Drect Angular Moentu Feedback, Proceedngs of ICRA 200, pp , Kajta, S., Yoko, K., Sago, M., and Tane, K., Balancng a Huanod Robot Usng Backdrve Concerned Torque Control and Drect Angular Moentu Feedback, Proceedngs of ICRA 200, pp , Sughara, T., Nakaura, Y., and Inoue, H., Realte Huanod Moton Generaton through ZMP Manpulaton based on Inverted Pendulu Control, Proceedngs of ICRA 2002, pp , L, T.H., K, Y.S., Yang, S.Y., Developent of tppng-over rate coputaton syste for hydraulc ecavator havng crane functon, KORUS 2004, Vol. 3, pp , Kovo, A.J., Kneatcs of ecavators (backhoes) for transferrng surface ateral, J. Aerosp.Eng., Vol. 7(), pp. 7-3, Sugano, S., Huang, Q., and Kato, I., Stablty crtera n controllng oble robotc systes, Proceedngs of the 993 IEEE/RSJ Internatonal Conference on Intellgent Robots and Systes, Vol. 2, pp , Takansh, A., Tochzawa, M., Karak, H., & Kato, I. Dynac bped walkng stablsed wth optal trunk and wast oton, Paper presented at the

8 IEEE/RSJ Internatonal Workshop on Intellgent Robots and Systes, "The Autonoous Moble Robots and Its Applcatons" (IROS '89), 989.