Proc. Austrlsin Conference on Rootics nd Automtion Aucklnd, 7-9 Novemer Sensorless orce Estimtion for Roots with riction John W.L Simpson, Chris D Cook, Zheng Li School of Electricl, Computer nd Telecommunictions Engineering University of Wollongong N.S.W john_simpson@uow.edu.u Astrct This pper descries method for estimting forces pplied t the end effector of root without the need for force sensors. Servo motor currents nd positions re used together with n ccurte system model to estimte pplied forces. The system model includes inerti, friction nd position dependent force components. The signl processing techniques required to extrct the force informtion from the servo motor dt re detiled nd experimentl results from SCARA root presented. Introduction orce control hs een used in mny rootic pplictions including ssemly tsks, serching nd grsping tsks nd mchining tsks such s grinding, nd deurring. Typiclly these pplictions hve een implemented using force sensor to give force feedck signl. In this pper method to clculte forces pplied y n end effector without the use of force sensor is given. The method uses n ccurte model of the rootic system plus servo motor position nd current dt to estimte the pplied force. orce sensors hve monetry cost nd re typiclly less mechniclly roust thn the roots they re mounted on. In ddition force sensors normlly hve s prt of their design n elstic sensing element whose deformtion is mesure of the pplied force. This elstic element reduces the stiffness of the root s mechnicl system. As force is pplied this leds to greter deflection of the end effector nd greter positionl error. In rootic systems servo motor position nd torque informtion (deduced from motor currents) hs een used to clculte nd control forces t root s end effector This re of work is often referred to s implicit force control. The prolem is tht the motor s torque hs to overcome friction nd other torques in ddition to n often very smll component relted to the force or torque t the end effector. In ddition, motor currents re notoriously noisy with unwnted signls from vriety of motor nd inverter relted effects. [Gorinevsky et l, 997] in their discussion of implicit force control schemes suggest tht these cn only e used with direct drive mnipultor rms or if the friction in the ger trins is smll. [Wd et l 994] nd [Rocco et l 997] implemented force control schemes using direct drive system. In this work models of the rootic mnipultor hve included inerti nd viscous friction components. [Ohishi 993] lso used direct drive mechnism including inerti viscous nd coulom friction in force oserver. [Gorinevsky et l, 997] lso commented tht in most industril mnipultor systems joint friction y fr exceeds the torques generted in contct tsks. Other uthors [Elosegui et l 99], [Tghird nd Belnger 997], [Tghird et l,997], [Luh et l 983] hve used joint torque sensors to eliminte the effects of friction. The joint torque sensors re instlled on the root linkge side of the ger trin nd re used s feedck signl in torque control loop. [Elosegui et l 99] detils the use of joint torque sensors to reduce the effects of the non-idel trnsmission due to the presence of stiction, coulom friction nd cogging. Joint torque sensors hve een found effective in force control loops s hve the use of force sensors t the end effector. However ll these schemes hve the disdvntge of hving to use dditionl sensors. The force estimtion techniques descried in this pper re cple of deling with non-idel trnsmission, friction nd motor cogging nd other effects without the need for dditionl force sensors since they require mesurement only of motor current. The servo motor moving ech xis of root is typiclly supplied y electronic servo drives operting in current control mode. The servo drive pplies known current to the servo motor nd using the motor's torque constnt the motor torque cn e clculted. In the work presented here the motor torque is ssumed to e mde up of the torque ccelerting the mechnicl components, the torque to overcome friction, the torque required to overcome numer of position dependent torques cused y the drive trin nd servo motor construction nd the torque derived Copyright ARAA 94
x = spring cosθ y = spring sinθ from the end effector s opertion. The mechnicl system used in the SCARA root considered here hs gerox etween the servo motor nd the root s links. The force pplied t the end effector will e significntly reduced y the gering rtio (t lest :8 for this root) nd so will typiclly e very smll prt of the totl servo motor torque. To e le to seprte the torque component due to the pplied force from the other motor torques n ccurte system model is needed. This includes the inerti, viscous nd coulom friction, nd position dependent torque components. Other uthors [We et l, 998], [Popovic et l, 998] nd [Armstrong 99] hve document position dependent components in complex mechnisms. It is shown here tht mny torque components in the motor currents re position dependent, nd tht they cn e removed y pproprite signl processing, thus llowing sustntil improvement in the ccurcy of estimting end effector forces from motor currents. Experimentl Setup The force estimtion techniques were implemented on Hirt ARI3 SCARA Root. The root hs 4 xes. The min rottion xes, A nd B, hve hrmonic ger rrngements. The gering rtios re nd 8 for the A nd B xes respectively. The liner Z-xis hs elt nd screw rrngement nd the wrist rottion W-xis is elt nd ger rrngement. DC motors power ll the xes. The A nd B xis motors re driven y Bldor TSD series DC servo-drives nd the Z nd W xes re driven y Yskw DC servo-drives. All the servo drives re current controlled. The root controller hs een reuilt to e completely controlled y dedicted Digitl Signl Processor (DSP). The Root nd its control cuicle re shown in figure An externl force is pplied to the end effector of the root y stretching spring. One end of the spring is connected to fixed point nd the other end is connected to the root s end effector. In the experiments, coordinted motion of the A nd B xes of the Hirt root is used to trck circulr pth out fixed point. or this rrngement the mgnitude of force will e constnt nd the direction of the force will lwys e in direction norml to the circulr pth eing trcked. The mgnitude of the spring force cn e chnged y trcking circulr pths of different rdii. A schemtic representtion of the Hirt root s pth nd the spring rrngement is shown in figure. Y r θ igure riction Controller Block Digrm spring The link lengths of the A nd B xes re denoted s r nd r nd re lso shown in figure. Let the mgnitude of the force pplied y the spring e spring. Let θ e the ngle the direction of the spring force mkes with the x xis. The spring force cn e resolved into its x nd y components x nd y respectively such tht: r θ Root Pth Spring y θ x X cos x = spring θ y spring θ = sin Eq Let (x,y) e the Crtesin coordintes of the end effector s position. The (x,y) coordintes re relted to the A nd B joint ngles θ nd θ respectively y the stndrd kinemtic eqution for SCARA root [Schilling, 99] x = r cosθ + r cos( θ + θ ) y = r sinθ + r sin( θ + θ ) Eq igure SCARA Root nd Control cuicle The DSP reds ll the shft encoder position dt nd end trvel limit switch informtion, implements the control strtegy nd outputs the drive signl. The DSP system gives flexile nd esy to progrm system, nd very fst smple rtes, nd enles the esy implementtion of vriety of control nd signl processing strtegies. The force spring. will produce torques t the A nd B joints denoted T nd T respectively. Letting torque tht cuses rottion in the clockwise direction e positive the reltionship etween the spring force components nd the joint torques is given y: T y x x = Eq 3 T r sin( θ + θ ) r + cos(θ θ y The results given in this pper re for the root trcking circulr pth with 3mm rdius nd n ngulr velocity 9
of.4rd/sthis corresponded to spring force of 3. N which is only out 4% of the root s mximum force cpility. Extrcting such smll force from the motor currents hence represents chllenging test for the proposed method. Using Eq nd 3 the expected torques t the A nd B xes joints cn e clculted nd re shown in figure 3 3 3 3 4 Time (s) 3 A Axis Spring Torque A A Axis Spring Torque vs Position 3..8.6.4. 3 4 Time (s) igure 3 A nd B Axes Torques The top two grphs in this figure show the spring torque vlues plotted ginst time nd the ottom two grphs show the torque vlues plotted ginst position. In section 4 the grphs plotting torque ginst position re used s seline to compre the output of ech stge of the force estimtion signl s processing. 3 Position Dependent Torque Vritions Even though inspection of the motor currents in the time domin seem to show mssive mount of noise, it turns out tht viewed in the position domin the sitution ecomes more optimistic. Position dependent torque components cn e considered s prt of n ccurte system model used to clculte end effector forces from servomotor current informtion. These components hve een found to e due to numer of cuses including motor poling, sttor slot cogging, nd vrious perturtions introduced y the mechnicl drive trin design, such s ger nd elt teeth nd shft eccentricities. In ddition there re numer of frictionl torque components. In the rest of the discussion here ll these components re referred to s Position Dependent Torque Vritions (PDTV) To discuss PDTV numer of terms need to e introduced. or periodic function of ngulr position θ mesured in rdins let the period e Ρ. This will e referred to s the position period. The position frequency Φ / is the inverse of the position period. The units for the position frequency Φ / re rds -. A more convenient set of units is to relte position frequency to the revolutions of the servo motor. The units used here re Cycles Per Revolution (CPR) i.e. the numer of times prticulr B Axis Spring Torque B Axis Spring Torque vs Position... position frequency occurs per revolution. There is ger ox etween the servo-motor nd the root linkge on ech of the And B xes of the root nd the ger rtio is denoted s n. The reltionship etween the position frequency in rds - Φ /, position frequency in CPR Φ, the ger rtio n nd position period Ρ is given in Eq 4 Φ = Ρ Φ = πφ n Eq 4 A PDTV with position frequency of CPR occurs times per revolution of the servo motor or n per revolution of the root s xis. The PDTV component will hve position period of π/n rds. To identify the PDTV, experiments re conducted where ech root s xis is run t s slow constnt velocity s is prcticl with no end effector lod. The digitl control system hs set smpling rte nd records the position nd torque dt t set time intervls. The position nd torque vlues recorded t the sme time re grouped together. Since the velocity will never e perfectly constnt the torque vlues will not e recorded t eqully spced positions. To del with this prolem the position xis is divided up into numer of position intervls or position ins. If more thn one torque vlue flls within ech in these vlues re verged together to give single torque vlue for the position in. If t lest one torque vlue flls within ech position intervl then the signl cn e regrded s discrete position signl. All the signl processing techniques tht cn e pplied to discrete time signl cn then lso e pplied to this discrete position signl. Hence position smpling rtes, Nyquist position frequencies nd lising hve to e considered for the discrete position signl s they would hve to e considered for discrete time signl. Consider the differentil eqution descriing the system with inerti J, viscous friction f v, Coulom friction f c, PDTV T pos (θ): d θ dθ T motor = J + f ( θ ) v + f c + T Eq pos dt dt The inerti, viscous nd Coulom friction prmeters were mesured directly nd stored in tle, ut the ccelertion nd velocity must e clculted from the position dt. The PDTV signl is then the motor torque minus the torque ccelerting the mechnism nd the Coulom nd viscous friction torques. The PDTV signl clculted t ech time intervl is turned into discrete position signl nd st ourier Trnsform (T) is pplied to this signl. A typicl resulting position frequency spectrum is shown in figure 4. The PDTV mgnitudes do chnge with velocity, nd lso over time s mechnicl components wer, or chnge fter mintennce, due to relignment of shfts, ering prelods nd so on. However the frequencies t which PDTV occur re highly repetle nd do not chnge with 96
m ) N e ( T orqu..4.4.3.3..... The torque estimtion signl includes the spring torque component T spring, the PDTV components T pos (θ) nd noise signl. T = T + T θ Eq 8 est spring pos + ( ) noise The signl processing ims to remove the noise nd PDTV signls from the torque estimtion signl to leve the spring torque. The torque estimtion signls for the A nd B xes of the Hirt root re shown in the top plots of figures nd 6 respectively. 3 3 4 Position requency (CPR) igure 4 B Axis PDTV Position requency Spectrum velocity or over time. Hence for given mechnism the position frequency spectrum only needs to e chrcterised once nd only t one velocity. 4 orce Estimtion Experiments The estimtion of externl forces pplied y the end effector requires n ccurte model of the mechnicl system, including its mss, friction nd PDTV. When no externl forces re pplied the torque delivered y the motor to the system is ssumed to ccelerte the mechnism nd to overcome its friction nd PDTV. When n externl force is pplied the difference etween the model nd the oserved motor torque is then the externl pplied force. The friction model includes Coulom nd viscous friction. The PDTV re implicitly included s prt of the signl processing. In the experiments conducted here the differentil eqution descriing the system is given y : d θ dθ T motor = J + f v + f c + T dt dt ( θ ) T spring is the torque component due to the pplied spring force spring. The T spring component is either the torque component T or T s given y Eq 3 depending on whether the A or B xis is eing nlysed. The B xis position ffects the A xis inerti ut prt from this the A nd B xis re ssumed to e independent of ech other. Centripetl forces, Coriolis forces nd coupling inertis hve een neglected. As the root ttempts to descrie circle t constnt velocity for given spring tension the motor torque T motor nd position θ dt re recorded. rom the position dt the velocity nd ccelertion vlues re clculted. Hence inertil torques cn lso e clculted. Estimtes of the viscous nd coulom friction prmeters re derived from previous experiments y direct mesurement. Thus from the known dt torque estimtion signl T est cn e generted nd is given y: T est = T motor d θ J f dt v dθ f dt c pos Eq 6 Eq 7 A Axis orce Estimtion Signl..9.8.7.6..4.3. A Axis iltered orce Estimtion Signl..9.8.7.6..4.3. igure A Axis Time Domin orce Estimtion Signls B Axis orce Estimtion Signl.8..4.6.8. B Axis iltered orce Estimtion Signl.8..4.6.8. igure 6 B Axis Time Domin orce Estimtion Signl The lighter line plots show the force estimtion signl clculted from the recorded position nd torque dt. The drk line is the torque vlue clculted from the pplied spring force nd is the sme s tht shown in figure 3. The first stge of the signl processing is to filter the force estimtion signl with low pss filter in the time domin. The filtered force estimtion signls re shown in the ottom plots of figures nd 6. The low pss filter removes the higher frequency noise components which re mostly cused y decoder quntistion noise. This high frequency noise is mde worse y the doule 97
differentition of the position dt to clculte the ccelertion vlue. The filtered torque estimtion signl is then turned into discrete position signl. The position domin torque estimtion signls for the A nd B xes re shown in the top plots of figures 7 nd 8 igure 7 A Axis Position Domin orce Estimtion Signls B Axis Position Domin Signl.8..4.6.8. A Axis Position Domin Signl..9.8.7.6..4.3. A Axis iltered Position Domin Signl..9.8.7.6..4.3. B Axis iltered Position Domin Signl.8..4.6.8. igure 8 B Axis Position Domin orce Estimtion Signls The next stge of the signl processing is to remove the PDTV components. The discrete position signl is processed with filter with notches t the sme position frequencies s the min PDTV components. or exmple these PDTV components re shown in figure 4 for the B xis. The torque estimtion signl fter the PDTV components hve een removed re shown in the ottom plots of figures 7 nd 8. These show tht the signl processing hs significntly reduced the noise on the torque estimtion signl for the A nd B xes. The reduction in the noise ws greter on the B xis thn the A xis. Prt of this ws due to the component of the force spring force seen t the B xes motor ws lrger motor torque ecuse the B xes hs lower ger rtio thn the A xes. In ddition the PDTV component were filtered more effectively on the B xes thn the A xis. This ws ecuse the B xes hd one lrge PDTV component t CPR (s shown in figure 4) which is esy to filter. The A xis PDTV spectrum hd more frequency components ut of less mgnitude nd the position domin filtering wsn t s effective. Conclusions Externl forces pplied to the end effector of root will produce torques tht the servo motors tht drive ech of the xes of the root will hve to overcome. The reserch presented here shows tht these torques cn e determined from servo motor current nd position dt in SCARA root whose mechnicl system hs high ger rtio nd in which friction is significnt. The method uses detiled model of the mechnicl system nd processing of the servo motor dt in oth the time nd position domins. The torque estimtion technique ws demonstrted for coordinted motion of two xes on SCARA root. Ech xis moved with continully vrying velocities to pply constnt ut very smll end effector force of only 3.N. The method gve good estimtes of force despite the demnding nture of this test. Acknowledgements This work ws prt of project funded y the Austrlin ederl Government Coopertive Reserch Centre for Intelligent Mnufcturing Systems nd Technologies. References [Gorinevsky et l, 997] Gorinevsky.D.M, ormtsky.a.m, Schneider.A.Y, orce Control of Rootics Systems, New York, CRC Press, 997 [Wd et l 994] Wd.H; Kosuge.K, ukud.t, Wtne.K, Design of force controller sed on frequency chrcteristics, IEEE Interntionl Conference on Rootics nd Automtion, Vol pp 6-6, My 8, 994 [Rocco et l 997] Rocco P, erretti.g, Mgnni.G, Implicit force control for industril roots in contct with stiff surfces, Automtic, Vol 33 No pp4-47, 997 [Ohishi 993] Ohishi.K, Sensorless force control using H ccelertion controller, Asi-Pcific Workshop on Advnces in Motion Control, pp3-6, th June 993 [Elosegui et l 99] Elosegui.P, Dniel.R.W, Shrkey.P.M, Joint servoing for roust mnipultors force control, IEEE Interntionl Conference on Rootics nd Automtion, Vol pp 46-, 3 th My, 99 [Tghird nd Belnger, 997] Tghird.H.D, Belnger.P.R, Intelligent torque sensing nd roust control of hrmonic drive under free motion, IEEE Interntionl Conference on Rootics nd Automtion, p749-74, April 997 98
[Tghird et l,997] Tghird,H.D, Helmy.A, Belnger.P.R, Intelligent uilt in torque sensor for hrmonic drive systems, IEEE Conference Instrument nd Mesurement Technology., p969-974, My 997 [Luh et l 983] Luh.J.Y.S, isher.w.d, Pul.R.P.C, Joint torque control y direct feedck for industril roots, IEEE Trnsctions on Automtic Control, vol AC-8, No, erury, 983 [Popovic et l,998] Popovic, M.R, Goldenerg A.A, 998 Modelling of riction Using Spectrl Anlysis. IEEE Trnsctions on Rootics nd Automtion Vol 4/:4-. [We et l, 998] We, S, Cook, C.D 998 Identifiction of Drive Trin Torque Disturnces for n X-Y Tle Test Bed, Proceedings of the ifth Interntionl Conference on Control, Automtion, Rootics nd Vision, Singpore, Vol: 439-443 [Armstrong 99] Armstrong B, 99 Control of mchines with riction MA Kluwer, Boston [Schilling, 99] Schilling R.J, undmentls of Rootics: Anlysis nd Control, New Jersey, Prentice- Hll, 99. 99