Robust Resonance Suppression Control based on Self Resonance Cancellation Disturbance Observer and Application to Humanoid Robot

Transcription

1 Robut Reoace Suppreio Cotrol baed o Self Reoace Cacellatio Diturbace Oberver ad Applicatio to Humaoid Robot Motoobu Aoki ad Hirohi Fujimoto ad Yoichi Hori The Uiverity of Tokyo Tokyo, Japa Taro Takahahi Advaced Techology Egieerig Departmet, Parter Robot Diviio, Toyota Motor Corporatio Tokyo, Japa taro takahahi@mail.toyota.co.jp Abtract I thi paper, a robut cotrol method for twoiertia ytem i propoed baed o the Self Reoace Cacellatio Cotrol (SRC) ad the Self Reoace Cacellatio Diturbace Oberver (SRCDOB). The SRCDOB propoed i thi paper improve the robute of cotrol ytem by rejectig diturbace ad compeatig for deviatio of plat dyamic. The feedback cotroller for two-iertia ytem i deiged baed o a omialized plat model by SRCDOB ad it effectivee i verified through imulatio tudy. I additio, the propoed robut reoace uppreio cotrol method i applied to a humaoid robot cotrol ytem ad experimetal reult demotrated the effectivee of the propoed robut cotrol method. I. INTRODUCTION Low birthrate ad logevity have bee progreig, o humaoid robot have bee the focu of attetio a the huma labor force which ca be replaced with people. Recetly, improvemet i kiematical performace i remarkable through the developmet of techology. Therefore, it i eceary to model a plat by a two-iertia ytem, ad itroduce a reoace uppreio cotrol. I the motio cotrol field, may reoace uppreio cotrol uch a Notch Filter, Reoace Ratio Cotrol, SRC are propoed, but thee method are ot robut for modelig error []-[5]. It i difficult to apply thee method for a humaoid robot which chage it dyamic due to the chage of iertia momet of iertia, cetrifugal force, corioli force, gravity, frictioal force, i accordace with the poture. O the other had, Diturbace Oberver (DOB) i propoed a a cotrol method which regard all modelig error a diturbace, omialize a plat, ad improve the robute over modelig error[6][7]. A DOB ca omialize a oeiertia ytem, but if thi method i applied to a two-iertia ytem, a big vibratio i iduced accordig to the elaticity ad the load iertia of the tramiio. From the above reao, robut reoace uppreio cotrol i required for modellig error of a plat modeled a a twoiertia ytem, to improve cotrol performace of a humaoid robot. Thu i thi paper, a robut reoace uppreio cotrol for modellig error of a two-iertia ytem i realized for a humaoid robot leg by uig both SRC with coideratio of the vicoity term of load, ad SRCDOB together. The compoitio of thi paper i how below. Firtly, the compoitio of the 3-joit leg robot created for thi reearch i explaied. Secodly, the deig method of SRC i coideratio of the vicoity term of load i tated. Moreover, SRCDOB devied baed o the theory of SRC i tated, ad thi method omialize the apparet plat of SRC. The, it i how that a robut reoace uppreio cotrol for modellig error of a two-iertia ytem i realized by uig SRC i coideratio of the vicoity term of load ad SRCDOB together. Fially, both method are applied to a 3- joit leg robot, ad it i how that the diturbace uppreio characteritic ad the referece trackig performace of the cotrol method which combie excel thoe of the cotrol method which combie. II. 3-JOINT LEG ROBOT AND MODELING IN TWO-INERTIA SYSTEM A. Baic Compoitio of 3-Joit Leg Robot We made 3-Joit leg robot how i Fig. i order to do baic examiatio of cotrol deig i a robot leg. The 3- joit leg robot ha hip, kee ad akle joit. Each joit i coected to a harmoic gear through a timig belt from a motor how i Fig.2. Modelig i two-iertia ytem for each joit i eceary uder the ifluece of the elaticity of each timig belt. The block diagram for the two iertia ytem i how i Fig.3. Motor momet of iertia, motor dampig coefficiet, load momet of iertia, load dampig coefficiet ad prig cotat are deoted repectively a J M, B M, J L, B L, K, ad their omial value are deoted a J M, B M, J L, B L, K. Gear ratio i deoted a. Here after, all variable with ubcript deote omial value, ad motor ide agular poitio i deoted a θ M, ad load ide agular poitio i deoted a θ L. I thi paper, the kee joit i examied amog the three.

2 6 Meaured Value Nomial Value Magitude [db] Fig.. 3-Joit Leg Robot Fig. 2. Compoitio of Tramiio of Joit dl JLBL K - θl Phae [deg] Frequecy [Hz] TM - JMBM θm Fig. 4. Bode Diagram for Kee Joit dm Fig. 3. Block Dyagram of 2-iertia Sytem B. Frequecy Repoce Characteritic of Kee Joit ad it Modelig i Two-iertia Sytem The bode diagram of the frequecy repoce characteritic from motor torque referece (T M ) to motor agle (θ M )forthe kee joit ad thoe of two-iertia ytem fittig model are how i Fig.4. Thi frequecy characteritic i meaured makig the thigh ad the lower thigh perpedicular to the groud, ad the forefoot horizotal to the groud. The reoace at 6 Hz come from the timig belt. Nomial value decided by thi frequecy repoce characteritic are how i Table.I. The reao why thi model fittig i ot doe exactly for the gai i the low frequecy level, i that gravity ifluece the robot i low frequecy level. Thi problem i olved by uig SRCDOB to tate i ectio III-B. III. SELF RESONANCE CANCELLATION CONTROL AND SELF RESONANCE CANCELALTION DISTURBANCE OBSERVER SRC i a reoace cacellatio method propoed by Sakata et al[5]. The SRC cacel the reoace of the apparet plat, by uig ecorder attached at both the motor ad the load ed, regardig the ytem a I2O oe, ad feedig back the TABLE I MODEL PARAMETER J M [Nm/ 2 ].4 J L [Nm/ 2 ]. B M [Nm/]. B L [Nm/] K [N/m].5 2 ecorder igal with ome gai multiplied. The apparet plat of SRC i metioed later. SRC deig ca be doe without kowledge of prig cotat (K), o it i robut with repect to perturbatio i prig cotat (K). Alo, thi method ha bee applied to a three-iertia ytem by Shiraihi et al[8]. However, SRC propoed by Sakata et al i tated eglectig the vicoity term, ad the deig procedure i alo complicated. Moreover, becaue SRC doe ot take modelig error ito coideratio, it i difficult to be applied to a humaoid robot which chage momet of iertia, cetrifugal force, corioli force, gravity, frictioal force accordig to the potual chage. Furthermore, SRC propoed by Sakata et al alo ha a problem that a teady-tate error arie due to load ide diturbace torque. I thi ectio, SRC deig method olvig thee problem ad SRCDOB method baed o the theory of SRC are tated. A. Self Reoace Cacellatio Cotrol For a two-iertia ytem, trater fuctio from motor torque referece (T M ) to motor aglular poitio (θ M ), from motor torque referece (T M ) to load agular poitio (θ L ), from load ide diturbace torque (d L ) to motor aglular poitio (θ M ), from load ide diturbace torque (d L ) to load agular poitio (θ L ) become P θ M J L 2 B L K 2 MM = = () T M D M P θ L K LM = = (2) T M D M P ML = θ M d L = K D L (3) P LL = θ L d L = J M 2 B M K D P (4)

3 where, D M = J M J L 4 (J M B L J L B M ) 3 ( 2 J M K J L K B M B L ) 2 ( 2 B M K B L K) (5) D L = J M J L 4 (J M B L J L B M ) 3 J M K J LK MB L 2 B M K B LK 2 : (6) I order to take ito coideratio the vicoity term which wa ot take ito coideratio i the covetioal SRC, the block diagram of SRC i redraw i Fig.5. C FB i the feedback cotroller which ca be deiged freely. The differece from the covetioal SRC i the poit of takig β ad δ ito coideratio. 2 Applyig the above parameter, the reoace term of omial SRC plat (P SRC ) i cacelled out. Fially, C FB hould be deiged for omial SRC plat (P SRC ). The, uig motor agular poitio error (e M )ad load agular poitio error (e L ), SRC output agular poitio error (e SRC )idefieda e SRC = α β e M γ δ e L (2) which i zeroed by SRC. However, load agular poitio error (e L ) which hould be i ot zeroed by it. Thi problem i olved i ectio III-C. B. Self Reoace Cacellatio Diturbace Oberver DOB i a cotrol method which omialize a oe-iertia plat i.e. a plat without reoace term[6][7]. From equatio (), becaue omial SRC plat (P SRC ) doe ot have reoace term, it ca be expected eaily that DOB for twoiertia ytem i realized if DOB i deiged for SRC plat P SRC. Therefore, i thi ectio, the deig method of DOB baed o the theory of SRC, i.e., SRCDOB deig method i tated. Equatio (9) i tated for a omial value, but modelig error arie i fact. Therefore SRC plat hould be P SRC = P MM α β P LM γ δ : (3) Fig. 5. Block Diagram of SRC Whe both ide are multiplied by T M, I thi cae, the omial trafer fuctio from load agular poitio referece (θ re f L ) to actual value (θ L) i deoted G C for cloed loop, ad G O for ope loop, a follow P LM ρ α β γ δ C FB G C = (7) P SRC C FB G O = P SRC C FB (8) ff where, P SRC = P MM α β P LM γ δ : (9) Where, the apparet plat of SRC i defied a SRC plat (P SRC ). Next, the omial value of SRC plat (P SRC ) i expreed a P SRC = 2 : () Normally there are a ifiite umber of olutio. However, without uig K, each parameter ca be determied a follow: α = J M β = B M γ = J L δ = B L : () T M P SRC = T M fp MM (α β )P LM (γ δ )g, θ SRC = α θ M βθ M γ θ L δθ L : (4) Where, SRC output agular poitio (θ SRC )idefieda θ SRC = T M P SRC : (5) θ SRC i a agle to ue i the theory of SRC, o it i completely differet from θ M ad θ L. The, whe DOB i cotructed uig SRC output agular velocity ( θ SRC ), the block diagram i draw i Fig.6. Thi method i amed SRCDOB. I thi cae, SRCDOB etimated diturbace ( dˆ SRC ) i determied a dˆ SRC = Q T M θ SRC : (6) Takig modellig error ito coideratio, SRCDOB etimated diturbace ( dˆ SRC ) ca be caluculalted a follow. dˆ SRC = QT M fδp MM (α β )ΔP ML (γ δ )g Qd M [ J M fδp MM (α β )ΔP ML (γ δ )g] Q d L [ J M fδp LM (α β )ΔP LL (γ δ )g] (7) where, ΔP MM = P MM P MM ΔP LM = P LM P LM (8) ΔP ML = P ML P ML ΔP LL = P LL P LL :

4 Fig. 6. Block Diagram of SRCDOB Fig. 7. Block Diagram of SRC with SRCDOB Moreover, if there i o modelig error, SRCDOB etimated diturbace ( dˆ SRC )i dˆ SRC = Q d d L M : (9) Equatio (9) how that there i o reoace term for SRCDOB etimated diturbace. Therefore, cut-off frequecy of the Q filter ca be larger tha the reoace frequecy. The, the trafer fuctio from apparet motor torque referece (TM ) to SRC output agular velocity ( θ SRC ) i caluculated i (2) for Q ß, θ SRC TM = = P SRC (2) reultig i the ame form a the omial SRC plat (P SRC ) i (). Thi how that a SRC plat (P SRC ) i omialized by SRCDOB. Therefore a robut cotrol ytem for all parameter e.g. J M, B M, J L, B L for two-ietia ytem i compoed by combiig SRC which i robut for prig cotat (K) ad SRCDOB which omialize the SRC plat (P SRC ). C. Cotrol Sytem Combiig Self Reoace Cacellatio Cotrol ad Self Reoace Cacellatio Diturbace Oberver The block diagram of the cotrol ytem combiig SRC ad SRCDOB i how i Fig.7. However, a tated i III-A, SRC output agular poitio error e SRC i zeroed by SRC, but load agular poitio error (e L ) which hould be zero i ot zeroed by it. Thi i the ame i the cae of combiig SRC ad SRCDOB. I order to olve the problem, if are deiged a B M = adb L 6= eveithecaeofb M 6=, the cotrol ytem with e SRC! ade L! ca be deiged, becaue the umber of itegrator of the load aglular poitio error i oe larger by oe tha that of the motor agular poitio error. IV. SIMULATIONS I thi chapter, the cotrol yte combiig, ad the cotrol ytem combiig are compared i imulatio. Modellig error i give for load iertia momet of J L = 2J L. The pole of each feedback cotroller are placed to meet the expeirmetal tability coditio i ectio IV-C a the form of multiple root. The pole (ω C ) of the PID cotroller are placed at ω C = 3 2π [rad/ec] a a multiple root for the rigid body of P MM, while that of the SRC i at ω C = 5 π [rad/ec] a a multiple root for the omial SRC plat P SRC. Moreover, The Q filter trafer fuctio i choe to be Q ω Q = : (2) ω Q The Q filter cutoff frequecy (ω Q ) of DOB i placed at ω Q = 25 2π [rad/ec] ad that of SRCDOB i placed at ω Q = 6 2π [rad/ec] to meet the expeirmetal tability coditio i ectio IV-C too. A. Nomializatio of SRC plat (P SRC ) by SRCDOB I thi ectio, it i imulated that SRC plat (P SRC ) i omialized by SRCDOB whe modellig error i iduced for load iertia momet J L = 2J L. The three trafer fuctio: = 2 ; SRC plat (P SRC ) whe the modellig error exit; ad omialized SRC plat (P SRC ) by SRCDOB whe the modellig error exit, are how i Fig.8. Fig.8 poit out that SRC plat (P SRC ) i omialized by SRCDOB ad the reoace i almot cacelled i the cae which the modellig error i iduced for load iertia momet J L = 2J L. Becaue Q i ot completely, the reoace doe ot completely diappear. If Q were, the reoace would cacelled completely. B. Seitivity Fuctio of Cotrol Sytem Combiig SRC ad SRCDOB I thi ectio, motor ide eitivity fuctio ad load ide eitivity fuctio of the cotrol ytem combiig PID ad DOB ad the cotrol ytem combiig are compared i the cae which modellig error i iduced for load iertia momet J L = 2J L. Motor ide eitivity fuctio i determid a the trafer fuctio from motor ide diturbace torque (d M ) to motor ide iput torque u M, ad load ide trafer fuctio i determied a the trafer fuctio from load ide diturbace torque (d L )toloadide iput torque (u L ). Motor ide trafer fuctio i how i Fig.9 ad load ide trafer fuctio i how i Fig..

5 Magitude [db] Magitude [db] [Hz] 2 Frequecy 3 4 Fig.. Load Side Seitivity Fuctio Phae [deg] P SRC P SRC (J L =2J L ) P SRC with SRCDOB (J L =2J L ) 2 3 Frequecy [Hz] Fig. 8. Bode Plot of P SRC Load Agle [mrad] Time [ec] Fig.. Motor Side Diturbace Additio Experimet Magitude [db] [Hz] 2 Frequecy 3 4 Fig. 9. Motor Side Seitivity Fuctio Fig.9 poit out that the gai for low frequecie of the cotrol ytem combiig are maller tha that of the cotrol ytem combiig. Therfore, the cotrol ytem combiig i more robut for motor ide dituebace torque (d M ) tha the cotrol ytem combiig. V. EXPERIMENTS I thi ectio, the cotrol yte combiig ad the cotrol ytem which combie are compared i experimet. The pole of cotroller ad the Q filter cut-off frequecie are ame i Sectio IV. A. Motor Side Diturbace Torque Additio Experimet I thi ectio, the cotrol ytem combiig ad that of are compared i motor ide dituebace torque uppreio efficiecy. Motor ide diturbace torque i added from oftware. For both cotroller, the load agular poitio referece i et to, ad a motor ide tep diturbace torque Nm at time.5 i added. The experimet reult i how i Fig.. Fig poit out that the cotrol ytem combiig i uppreed more tha the cotrol ytem combiig a ame a Fig.9 how. B. Load Side Diturbace Torque Additio Experimet I thi ectio, the cotrol ytem combiig PID ad DOB ad the cotrol ytem combiig are compared i load ide dituebace torque uppreio efficiecy. Step iput torque i added to the akle motor i the tate which akle i bet to the mechaical limit, ad the reactio torque i regarded a the load ide diturbace torque of kee joit. For both cotroller, the load agular poitio referce i et to, ad a motor ide tep diturbace torque for the akle joit Nm at time.5 i added. Thi mea that load ide torque for akle joit i 2Nm becaue the ratio of tramiio for the akle joit i 2. The experimet reult are how i Fig.2. Becaue the cotrol ytem combiig doe ot ue load agular poitio θ L, a teady-tate error arie by load ide

6 Load Agle [mrad].2.. Load Agle Error[mrad] Time [ec] Time[ec] Fig. 2. Load Side Diturbace Additio Experimet Fig. 4. Orbit Followig Experimet (Error) Load Agle[rad] Referece Time[ec] Fig. 3. Orbit Followig Experimet diturbace torque i the cotrol ytem. However, a teadytate error arie by load ide diturbace torque i the cotrol ytem combiig. The vibratio of 2Hz arie for both cotrol ytem, but the factor i the reoace of hip joit, ad thi i ot for kee joit cotroller. C. Referece Trackig Experimet I thi ectio, referece trackig experimet for the cotrol ytem combiig ad referece trackig experimet for the cotrol ytem combiig SRC ad SRCDOB are compared. The real referece of a humaoid robot tep i ued i thee experimet. I thee imulatio, firtly a leg i take to iitial poture ad two time of tep are performed. The, the hip joit ad the akle joit move imultaeouly, o the reactio torque work a diturbace torque for kee joit. Moreover, the modelig error i alo take ito coideratio becaue a poture chage occur. Time repoe of referce trackig experimet i how i Fig.3 ad the error with referece i how i Fig.4. Fig.3 ad Fig.4 poit out the cotrol ytem combiig SRC ad SRCDOB ha better trackig characteritic tha that of the cotrol ytem combiig. VI. CONCLUSION I thi paper, the deig method of SRC i coideratio of the vicoity term of load ad SRCDOB deig method are tated. The, Combiig thee, it i tated that robut reoace uppreio cotrol for a two-iertia ytem i realized, ad thi cotrol method i applied for a leg of humaoid robot i imulatio ad experimet. The cotrol ytem combiig i better tha the cotrol ytem combiig. I particular, the pole of PID are placed at ω C = 3 2π [rad/ec], but thoe of SRC are at ω C = 5 2π [rad/ec]. Moreover, The Q filter cut-off frequecy of DOB i ω Q = 25 2π [rad/ec], but that of SRCDOB i ω Q = 6 2π [rad/ec]. Therfore, the diturbace uppreio characteritic of motor ide ad load ide for the cotrol ytem combiig SRC ad SRCDOB better tha the cotrol ytem combiig PID ad DOB. Alo i referce trackig experimet, the cotrol ytem combiig i uperior to the cotrol ytem combiig. The cotrol ytem combiig propoed i thi paper i clear, ad thi method ca be applied for may appllicatio ot oly humaoid robot. It would be our pleaure if thi paper could be helpful to the tudy of motio cotrol. REFERENCES [] Y. Hori, H. Sawada, ad Y. Chu: Slow reoace ratio cotrol for vibratio uppreio ad diturbace rejectio i torioal ytem, IEEE Tra. Id. Electro., vol. 46, o., pp , Feb [2] K. Ohihi, N. Shimada, ad T. Miyazaki: High-performace robot motio cotrol baed o zero-phae otch filter for idutrial robot, The th IEEE Iteratioal Workhop o Advaced Motio Cotrol, pp , March 2-24, 2, Nagaoka, Japa [3] K. Yuki, T. Murakami, ad K. Ohihi: Vibratio Cotrol of a 2 Ma Reoat Sytem by the Reoace Ratio Cotrol, Tra. It. Elect. Eg. Jp. vol.3-d, o., pp , 993. [4] Y. Seki, K. Saiki, H. Fujimoto: Self Reoace Cacellatio uig Multiple Seor for Ballcrew Drive Stage, The 2th IEEE Iteratioal Workhop o Advaced Motio Cotrol, pp.-6 March 25-27, 22 [5] K. Sakata, H. Fujimoto, ad K. Saiki Optimal mechaical parameter deig uig Self Reoace Cacellatio cotrol for gatry-type high preciio tage,to be publihed i Proc. IEEJ Idutry Applicatio Society Cof. (JIASC 2), (i Japaee). [6] K.Ohihi: New Developmet of Servo Techology i Mechatroic, IEEJ Traactio o Idutry Applicatio, vol.7-d, o., pp.83-86, 987. (i Japaee) [7] T. Umeo ad Y. Hori, Robut peed cotrol of DC ervomotor uig moder two degree-of-freedom cotroller deig, IEEE Tra. Id. Electro., vol. 38, pp , Oct. 99. [8] T. Shiraihi, H. Fujimoto: Vibratio Suppreio Poitio Cotrol of Three-iertia Sytem Uig Self Reoace Cacellatio Cotrol, i Proc. IEE of Japa Techical Meetig Record, IIC--7, 2 (i Japaee).