Dynamic Walking of Biped Robots with Obstacles Using Predictive Controller

Transcription

1 ICCKE011, Intrnational Confrnc on Computr and Knowldg Enginring Oct 13-14, 011, Frdowsi Univrsity of Mashhad, Mashhad, Iran Dynamic Walking of Bipd Robots with Obstacls Using Prdictiv Controllr Nasrin Kalamian Dpartmnt of Elctrical Enginring Iran Univrsity of Scinc and Tchnology Thran, Iran Mohammad Farrokhi Dpartmnt of Elctrical Enginring Iran Univrsity of Scinc and Tchnology Thran, Iran Abstract This papr proposs a control mthod for walking of bipd robots whil stpping ovr larg obstacls, which is a big challng for ths robots. To this nd, Nonlinar Modl Prdictiv Control (NMPC is mployd. Th main advantag of this approach is that thr is no nd for trajctory planning. In othr words, th robot finds th optimum gait lngth basd on stability of th closd-loop systm and going ovr th obstacls. Simulation rsults show good prformanc of th proposd mthod, whr th bipd can stp ovr a 40 15cm obstacl in th sagittal plan without colliding with it. Kywords-bipd robots; dynamic walking; nonlinar modl prdictiv control; obstacls. I. INTRODUCTION During th last two dcads, walking robots hav bn considrd by many rsarchrs in this ara. This is mainly du to th advantags at walking that othr robots (lik whld robots can not prform. Lggd robots can walk in unknown, irrgular, rough and sloppy trrains. Thy can cross ovr obstacls or pass through ditchs; go up and down stairs whras whld robots ar not abl to do ths tasks. Gnrally, th obstacls that ar considrd in almost all paprs ar vry short. Thr ar wll-known robots that can stp ovr small obstacls, such as Johnni that can cross ovr a 5 cm obstacl [1] and ASSIMO that can stp ovr flat obstacls []. Prvious studis on walking and stpping ovr obstacls hav dsignd an off-lin trajctory for th tip of th swinging lg and th hip of robot; thn, th bipd is controlld to trac ths prdfind paths. In [3], authors hav prformd this mthod for RP- robot. In thir mthod, first, an algorithm finds fasibility conditions for stpping ovr an obstacl. If th answr is ys, thn th robot uss th prdfind off-lin trajctory and crosss ovr th obstacl. Yagi and Lumlsky considrd svral trajctoris for crossing ovr diffrnt obstacls [4]. Th robot snss obstacls and according to an algorithm slcts th bst prdfind trajctory. Thn, th robot is controlld using th Zro Momnt Point (ZMP stability critrion. For wid obstacls, Jafri t al. hav proposd a mthod, in which th stratgy dcids whthr th robot can stp ovr th obstacl or should stp on it [5]. If th robot cannot stp ovr th obstacl, it stops bfor that. Th humanoid robot BR- can succssfully stand on obstacls. In [6] for stpping ovr obstacls, authors proposd a mthod to maintain th projction of th Cntr of Mass ( of th robot in th supporting polygon ara that can guarant static stability. owvr, th RP- robot walks and crosss ovr obstacls vry slow. Prviw control in anothr mthod that has bn usd to gnrat trajctory in ordr to stp ovr a 15 5cm obstacl by RP- bipd [7]. Thy hav usd th ZMP critria to guarant dynamic stability of th robot. wang t al. hav proposd a dcntralizd control mthod basd on fuzzy logic for stpping ovr obstacls for small siz humanoid robots [8]. In [9], Kushida t al. hav proposd a hybrid dynamical systm (DS as a systm that has both th continuous and discrt vnts. DS can b formulatd as a linar inquality, whr logical variabls ar spcifid by mixd logical dynamical systm. In [10], a mthod has bn proposd for RP- (165cm, 30DOF robot that can cross ovr 5 5cm obstacl [10]. Th ft trajctory is dsignd off-lin but it can bcom adaptiv. Prdfind or off-lin trajctory planning has som inhrnt problms. First, th trajctory must b customizd for th robot in hand. nc, vry robot nds its own trajctory. Scond, for diffrnt trrains diffrnt trajctoris must b dsignd. Third, a prdfind trajctory dos not rsmbl human walking. Du to ths disadvantags, th Modl Prdictiv Control (MPC mthod is usd in this papr for bipd walking and stpping ovr obstacls without any nd for off-lin trajctory gnration. In [11] authors usd th MPC for gnrating on-lin trajctory and control at th sam tim. In thir mthod, by considring physical constraints of th robot, an optimal path and control mthod is cratd. MPC controllr has also bn usd for dynamically walking of humanoid robot RP- [1]. In this mthod, th cost function minimizs th rror btwn th ral and dsird ZMP that is gnratd off-lin. Azvdo t al. hav considrd som physical and usful constraints for static walking at flat surfacs [13]. owvr, th problm in this mthod is that th robot walks vry slowly. In [14], a ral-tim control mthod basd on Nonlinar MPC (NMPC is proposd for controlling a 7DOF bipd robot. Th optimization problm is solvd using th SQP algorithm. Th NMPC uss th dynamic modl of robot for prdiction. nc, du to xisting of uncrtaintis in th modl, th modl prdiction is not prcis. In [15] authors us nonlinar disturbanc obsrvr (NDO to ovrcom this problm. Morovr, th gait lngth is not fixd. Th robot's walking is similar to human bhavior. owvr, th robot walks slowly. This papr is organizd as follows. In Sction, th dynamic modl of th robot will b illustratd. Th structur of th NMPC controllr, th cost function and constraints will b givn in Sction 3. Sction 4 shows simulation rsults followd by conclusion in Sction

2 ICCKE011, Intrnational Confrnc on Computr and Knowldg Enginring Oct 13-14, 011, Frdowsi Univrsity of Mashhad, Mashhad, Iran II. DYNAMIC OF BIPED ROBOT Th bipd robot that is usd in th papr is shown in Fig. 1. It can walk in th sagittal plan. Th ft hav no mass and ar considrd fr friction. On stp includs thr phass: 1 Doubl Support Phas (DSP, Singl Support Phas (SSP, and 3 SSP impact. Th DSP happns whn both lgs ar on th ground. On th othr hand, th SSP happns whn just on lg (calld th supporting lg is on th ground. Th SSP impact is right aftr th SSP whn th tip of th swinging lg contacts th ground. Th DSP and SSP hav diffrnt dynamics and must b considrd sparatly [16]. A. Singl Support Phas Th dynamic of SSP can b writtn as [16] D ( θ & θ + h( θ, & θ & θ + G( θ = T (1 (1 whr D 5 5, h 5 1, G 5 1, and T 5 1 ar th inrtia matrix, th vctor of cntriptal, th Coriolis torqus, th gravity vctor, and th vctor of joint torqus, rspctivly. B. Doubl Support Phas Th dynamic of SSP can b prsntd as [16] T D ( θ & θ + h( θ, & θ & θ + G( θ = T + J ( θ λ ( whr J 5 and λ 1 ar th Jacobian matrix and th Lagrangian vctor, rspctivly. C. SSP Impact At th nd of th SSP, th robot lands its swinging lg. Nvrthlss, thr is a suddn impact btwn th ground and th tip of th foot. This impact affcts angular vlocity. If th impact is larg, thn th angular position and vlocity may incur larg changs to th robot, causing instability. nc, th control mthod should produc as littl impact as possibl. Th angular vlocity immdiatly right aftr th contact is & θ = & T T θ + D J ( JD J ( J & θ whr impact (3 θ & is th angular spd right bfor th impact. III. NMPC CONTROLLER Modl Prdictiv Control is a gnral control schm that is dsignd to solv onlin a squnc of optimal control problms with som constraints [13], [14]. On of th advantags of NMPC is that it dos not nd any off-lin trajctory gnration. nc, it is possibl to mak th robot to walk lik human. In th proposd mthod in this papr, th gait lngth is not fixd and th robot can stand at a suitabl point bfor th obstacl, vn whn th normal walking cycl is not compltd. In this papr NMPC is usd for a bipd robot to walk on flat surfacs and thn, stp ovr larg obstacls. Th cost functions for walking on flat surfacs and obstacl avoidanc is similar but th constraints ar diffrnt. A. Cost Function Th cost functions for th DSP as wll as th SSP ar th sam as: Figur 1. Fiv link bipd robot. B. Cost Function Th cost functions for th DSP as wll as th SSP ar th sam as: whr that Nc 1 T J = w T ( t + i t T ( t + i t + α = 1 dsird and 1 i = 0 N p dsird (& ( + α & j = 1 w x t j t x dsird 1 xp x x t j t (4 (5 ( + + σ ar th dsird horizontal final x vlocity and position of th final stop point right bfor obstacls, rspctivly, and σ is a paramtr that rgulats th acclration of th robot locomotion. At th bginning final of motion, th rror x x is imum and α is 1. Whn th robot is closd to th final position, th rror and α approach zro and th robot stops. Paramtrs N p and N p ar th prdiction and control horizons, rspctivly, is th sampling tim, and w 1 and w ar th wights for th rquird torqus and tracing th dsird horizontal vlocity, rspctivly. C. Constraints For vry phas of walking, diffrnt constraints ar ndd that ar introducd in th followings. 1 DSP Constraints for Flat Surfacs For this phas, th physical, forward motion, stability, and nrgy optimization constraints ar dfind as follows: 1. Th joints constraints: t 161

3 ICCKE011, Intrnational Confrnc on Computr and Knowldg Enginring Oct 13-14, 011, Frdowsi Univrsity of Mashhad, Mashhad, Iran q q q i, min i i, whr, for th robot in Fig. (6 π q1 = θ1; q = π + θ1 θ ; q3 = π + θ θ3; (7 q4 = θ 4 ; q5 = π + θ5 ( θ 3 + θ 4 To prvnt singularity of Jacobian matrix, th controllr should guarant q π, q5 π.. Th actuators torqu should b limitd: Tmin T T (8 3. Th bipd robot should b at rctd postur during its locomotion: h h (9 min hip h whr h hip is th normal hight of th robot hip. 4. Th torso is almost 50% of robot's wight and has important rol in dynamic stability and it should b upright during walking: θ min θ trunk θ (10 5. Bipd robot must only walk forward. nc, th robot spd (in th x dirction must b positiv: Th tip hight of th swinging lg should b abov th ground: y = Th support ara for dynamic stability is x x x FootLngth (11 ZMP b + SSP Constraints for Flat Surfacs For smooth and normal walking at flat surfacs, th following constraints ar dfind for th SSP phas: 1. Th first fiv constraints ar similar to th DSP constraints. 6. Th tip hight of swinging lg should b rstrictd: 0 y (1 7. Th horizontal spd of th tip should b adaptd to th robot vlocity: β β min sin( sin( πy πy (13 8. During tak off, th vrtical spd of th tip must b positiv and during landing should b ngativ and adaptd to th robot vlocity δ + sgn( x x x sin( π x b y& b 0 sin( πy (14 9. Dynamic stability should b guarantd with limitation of ZMP in th support polygon x x x b ZMP b + FootLngth. 3 Stpping ovr Obstacls Thr ar thr phass for stpping ovr an obstacl: first, on lg crosss ovr th obstacl (SSP1; scond, th torso mov forward (DSP, and third, th back lg passs ovr th obstacl (SSP. Ths phass ar shown in Fig.. Th constraints for SPP1 and SSP phass ar: 1. Th first fiv constraints ar similar to th DSP constraints. 6. Th horizontal spd of th tip should b adaptd to th robot vlocity πx βmin sin( + x O3 (16 πx β sin( + x O3 7. Th vrtical vlocity of th tip should b as x < y& > 0 x = 0 y& (17 3 x > y& < Th tip hight of th swinging lg should b rstrictd x, x 0 y 3 O, (18 x,min 3 O y O, 9. Th horizontal position of th kn should not contact with th obstacl: xkn x O ( For dynamic stability, th ZMP must guarant xb xzmp xb + FootLngth. (0 Th constraints for DSP phas ar: 1. Th first svn constraints ar similar to th walking DSP constraints. 8. Th horizontal position of th kn should not contact with th obstacl lik (19. Fig. 3 shows block diagram of control procdur for walking ovr flat surfacs and stpping ovr obstacls. Th bipd robot walks ovr th flat trrain normally. If an obstacl is dtctd in th vicinity of th robot, th controllr rgulats th gait lngth (i.. ithr making a shortr or longr gait in ordr to stop at an appropriat plac bfor th obstacl. If bipd robot can stp ovr th obstacl, th controllr prforms th task; othrwis, th robot stands still bcaus th robot in this papr can just mov in sagittal plan. IV. SIMULATION RESULTS (15 Th bipd robot paramtrs that hav bn xtractd from [16] ar givn in Tabl I. Tabl II shows th minimum and imum of variabls. Othr paramtrs ar considrd as: 16

4 ICCKE011, Intrnational Confrnc on Computr and Knowldg Enginring Oct 13-14, 011, Frdowsi Univrsity of Mashhad, Mashhad, Iran dsird FootLngth = 15 cm, = 1m/s, N = 5, N = 4, w 1 = 100, w = 0. 01, σ = 0. 03, Obstacl _ ight = 40 cm, Obstacl _ Lngth = 15 cm and t = 0. 0 s. Fig. 4 shows svral walking cycls and obstacl dtcting. Th last cycl is shortr than th othr cycls bcaus an obstacl is dtctd (Fig. 5. Thr phass of stpping ovr th obstacl ar shown in Fig. 6. Th grn shad around th obstacl is considrd for th safty claranc. Th tip of th swinging lg and th kn should not contact with th safty ara around th obstacl (Fig. 7. Th bipd robot continus walking aftr passing th obstacl (Figs. 8 and 9. It should b notd that aftr crossing ovr th obstacl, th robot's lgs ar nxt to ach othr and it should stp a short gait bfor it can rsum its normal walking. Fig. 10 shows that th joint torqus limits (±50Nm ar not violatd by th controllr. p c TABLE II. Variabl MINIMUM AND MAXIMUM OF VARIABLES Min. and Max. of Variabls Minimum Maximum h hip 0.98m 1.08m θ trunk -3 3 T i -50N.m 50N.m O Obs_ight+0.01m Obs_ight+0.03m β 4 10 a SSP1 b DSP c SSP Figur. Stpping ovr obstacl Figur 4. Svral cycl and stop bfor obstacl Start Walking at flat surfac Continu walking No Is an obstacl in th path? Ys Rgulat your stp lngth and stop right bfor obstacl. Cross ovr obstacl Ys Ar you abl to stp ovr this obstacl? Link TABLE I. Lngth (m Figur 3. Control procdur Mass (kg ROBOT PARAMETERS Robot Paramtrs Inrtia (kgm Location of cntr of mass (m Figur 5. Last and short cycl bfor stop Figur 6. Thr phass of stpping ovr obstacl 163

5 ICCKE011, Intrnational Confrnc on Computr and Knowldg Enginring Oct 13-14, 011, Frdowsi Univrsity of Mashhad, Mashhad, Iran Figur 7. Stpping ovr obstacl without contact V. CONCLUSION This papr proposd a control mthod for walking and stpping ovr obstacls by bipd robots. Th Nonlinar Modl Prdictiv Control (NMPC is usd for controlling th robot without trajctory planning. Du to advantags of NMPC, walking and crossing ovr obstacl of th bipd robot bcom vry similar to human walking stratgy. On of advantags of th proposd mthod is that th stp lngth is not fixd and th NMPC spcifis it by solving an optimization problm basd on dynamic stability of th robot. Unlik othr rportd mthods in litraturs, th robot can stp ovr a rlativly tall obstacl whil maintaining stability. Th bipd robot that is usd in this papr has a 49.5kg wight and is 1.73m tall and could cross ovr a 40 15cm obstacl in th sagittal plan (40% of th robot's lg lngth with imum 1m/s spd. Simulation rsults showd ffctivnss of th proposd mthod. Figur 8. Short gait aftr crossing ovr obstacl Figur 9. Normal walking aftr stpping ovr obstacl Figur 10. Joint Torqus REFERENCES [1] J. F. Sara, O. Lorch and G. Schmidt, Gaz control for goal-orintd humanoid walking, Intrnational Confrnc of umanoid Robots, 001. [] P. Michl, J. Chstnutt, J. Kuffnr and T. Kanad, Vision-guidd humanoid footstp planning for dynamic nvironmnts, Intrnational Confrnc of umanoid Robots, 005. [3] Y. Guan, N. E. Sian and K. Yokoi, Motion planning for humanoid robots stpping ovr obstacls, Intrnational Confrnc of Intllignt Robots and Systms, August 005. [4] M. Yagi and V. Lumlsky, Bipd robot locomotion in scns with unknown obstacls, IEEE Intrnational Confrnc on Robotics and Automation, May [5] R. Jafri, Q. uang, J. Yang, Z. Wang and T. Xiao, Motion planning for stpping on/off obstacls by humanoid robot, IEEE Intrnational Confrnc of Mchatronics and Automation, August 007. [6] Y. Guan, N. E. Sian and K. Yokoi, Stpping ovr obstacls with humanoids robots, IEEE Transactions on Robotics, vol., no. 5, pp , Octobr 006. [7] B. Vrrlst, K. Yokoi and O. Stass and A.. B. Vandrborght, Mobility of humanoid robots: stpping ovr larg obstacls dynamically, IEEE Intrnational Confrnc of Robotics and Automation, Jun 006. [8] C. L. wang,. C. Wu and M. L. Lin, Th stpping ovr an obstacl for th humanoid robot with th considration of dynamic balanc, in Procding of SICE Confrnc, August 010. [9] D. Kushida, F. Takmori and A. Kitamura, Stpping ovr xcss of obstacl for bipd robot basd on hybrid control, in Procding of 16 th IFAC World Congrss, vol. 16, part 1, 005. [10] O. Stass, B. Vrrlst, A.. B. Vandrborght and K. Yokoi, Stratgis for humanoid robots to dynamically walk ovr larg obstacls, IEEE Transactions on Robotics, vol. 5, no. 4, pp , August 009. [11]. Didam, D. Dimitrov, P. B. Wibr, K. Mombaur and M. Dihl, Onlin walking gait gnration with adaptiv foot positioning through linar modl prdictiv control, IEEE Intrnational Confrnc of Intllignt Robots and Systms, Sptmbr 008. [1] P. B. Wibr, Trajctory fr linar modl prdictiv control for stabl walking in th prsnc of strong prturbations, Intrnational Confrnc of umanoid Robots, Dcmbr 006. [13] C. Azvdo, P. Poignt and B. Espiau, On lin optimal control for bipd robots, in Procding of 15 th IFAC World Congrss, vol. 16, part 1,

6 ICCKE011, Intrnational Confrnc on Computr and Knowldg Enginring Oct 13-14, 011, Frdowsi Univrsity of Mashhad, Mashhad, Iran [14] Z. Zhu, Y. Wang and X. Chn, Ral-tim control of full actuatd bipd robot basd on nonlinar modl prdictiv control, Intllignt Robotics and Applications, Vol. 5314, pp , 008. [15] M. Parsa and M. Farrokhi, Robust nonlinar modl prdictiv trajctory fr control of bipd robots basd on nonlinar disturbanc obsrvr, in Procding of ICEE, May 010. [16] C. Y. A. Chn, Dynamic modling, control and simulation of a planar fiv-link bipdal walking systm, Mastr Thsis, Dpartmnt of mchanical and Industrial Enginring, Univrsity of Manitoba, Canada,