Hybrid System Design for Singularityless Task Level Robot Controllers *

Transcription

1 Proceedings of he 2000 IEEE Inernaional Conference on Roboics & Auomaion San Francisco, CA April 2000 Hybrid Sysem Design for Singulariyless Task Level Robo Conrollers * Jindong Tan and Ning Xi Deparmen of Elecrical and Compuer Engineering Michigan Sae Universiy Eas Lansing, MI 48824, USA Absrac This paper presens a hybrid sysem approach in he design of a singulariyless ask level conroller. To achieve a singulariyless moion conrol in he neighborhood of singulariy, he hybrid sysem approach is used o inegrae he ask level conroller and join level conroller. Firs, a hybrid sysem model is developed for he singulariyless ask level conroller. A Max-Plus dynamic model is used o inegrae he discree swiching conrol and coninuous moion conrol in he conroller. Based on his model, a smooh rajecory and conrol command /or he hybrid sysem can be obained. The Lyapunov heory was used o prove he sabiliy of he singulariyless conroller. The new singulariyless ask level conroller has been experimenally implemened and esed on a PUMA 560 robo manipulaor. Experimenal resuls have been employed o verify he heoreical conclusions, hus clearly demonsraing he advanages of he new ask level conrol mehod. 1 Inroducion A robo ask is usually described in is ask space. The direc implemenaion of a ask level conroller can provide significan applicaion efficiency and flexibiliy o a roboic operaion. I becomes even more imporan when robos are working coordinaely wih humans. The human inuiion on a ask is always represened in he ask space. However, he major problem in he applicaion of he ask level conroller is he exisence of singulariies. While approaching a singular poin, he ask level conroller generaes high join orques, which resul in insabiliy or large errors in he ask space. The ask level conroller is no only * Research Parially suppored under NSF Gran [IS and IIS invalid a he singular poins, bu also cause insabiliy in cerain neighborhood of he singular poins. Therefore, a number of mehods have been proposed o solve he problem. A general idea is o avoid singulariies by reducing he orque applied o individual joins. The basic damped leas squares(dls) mehod [1] modifies he end-effecor pah in erms of velociy. The inverse or he psudoinverse of he Jacobian marix used in he ask level conroller does no exiss and resuls in unreasonable values a or near he singular poins. Insead of exac moion resuling large orques, he DLS mehod provides approximae moion close o he desired Caresian rajecory. S. Chiaverini e al [2] refined he basic DLS mehod o improve deviaion errors from he desired pah by varying damping facors. A variable feedback gain is hen se o zero wihin he viciniy and inerpolaion is used o achieve a smooh soluion of he feedback gain [2]. An alernae mehod was developed incorporae he dynamics poles of he sysem. M. Sampei e al. [3] proposed a ime re-scale ransformaion mehod for designing a robo conroller, which achieves slow poles in he viciniy of singulariy and fas poles in regular area. B. Bishop e al.[6] proposed a swiching conrol mehod for redundancy resoluion of redundan robos. The damped velociy mehod was used o maximize he manipulabiliy. Torques and join velociies are limied in uilizing he redundancy of he robo. A variable feedback gain, appropriae adjused damping facors and he ime reparameerizing mehods resul in similar Caresian error dynamics. By hese mehods, he sysem is sill unsable a he singular poins. Obviously, i is no accepable for many applicaions. M. Kirdanshi e al. [7] heoreically proved ha a he singular poins, he resolved acceleraion conrol of robos has unsable saddles /00/$ IEEE 3007

2 However, if robos are conrolled a join level, he end-effecor can sar from or pass by any poins in he workspace including he singular poins caused by inverse kinemaics. In he region ha he ask level conroller does no work, he join level conroller is inegraed and Singulariyless Task Level Conroller(STLC) is proposed. 2 Singulariies in Robos Task Level Conroller This secion firs describes he conrollers for robos a join level and ask level. The singulariies exised in he ask level conroller is analyzed. The corresponding robo configuraions for singulariies are also presened. 2.1 Join level and ask level conrollers as The dynamic model for a robo arm can be wrien D(q)~ + e(q,o) + 9(q) = u (2.1) where q is he 6 x 1 vecor of join displacemens, 0 is he 6 x 1 vecor of join velociies, u is he 6 x 1 vecor of applied orques, D(q) is he 6 x 6 posiive definie manipulaor ineria marix, e(q, q) is he 6 x 1 cenripeal and coriolis orques, and g(q) is he 6 x 1 vecor of graviy erm. If he ask is given in join space, a compued orque conroller a join level can be derived and denoed by Ul -~ D(~ d + Kvle2 + Kplel) + e + g (2.2) where qd is he ask in join space, el = qd _ q, e2 = 0 d -- q, Kvl and Kpa are feedback gain marices. However, he desired ask for he end-effecor is usually specified in ask space in mos of he robo applicaions. By he relaion beween join space and ask space, he dynamic model (2.1) can be changed ino dynamic model in ask space. Le y E ~6 be a ask space vecor defined by y = (px,py,pz,o,a,t) T. (px,pu,pz) r denoes he posiion of he end-effecor in he Caresian space, (0, A,T) T denoes an orienaion represenaion(orienaion, Aliude, Tool angles). The robo dynamics a ask level can herefore be described as follows: DJ~ -I(y - Joq) + c(q, q) + g(q) = u (2.3) The desired pah in ask space is denoed as yd. Le xl = yd _ y, x2 : yd _ y. The ask level conroller for (2.3) is a nonlinear feedback conrol law. u2 = DJ~-(-:O a +,1o0 + Kvx2 + KpXl) + C-1"-9 (2.4) 2.2 Analysis of he singulariies I can be seen ha he exisence of conroller (2.4) depends on he exisence of j~-l. The robo configuraions in which he rank of Jo decreases are called singular configuraions. A singular configuraions, jg- does no exis. For a 6-DOF manipulaor, which consiss of a 3-DOF forearm and a 3-DOF spherical wris, Jo is a 6 x 6 marix and a configuraion is singular if and only if de(jo) = 0. Singular configuraions can be decoupled ino arm singulariies resuling from he moion of he arm and wris singulariies resuling from he moion of spherical wris. Insead of sudying he deerminan of Jo, he Jacobian could be decouplea [4] [9]: o=[1 0] 0 I 0 J21,/22 Since he deerminans of I, and U can no be zeroes, he singulariy condiions are de(jl) = 0 and de(j22) = 0. Jn = 0 sands for he forearm singulariy. Two singulariy condiions can be obained from he forearm singulariy. One is he so called boundary singulariy. I occurs while "~b = d4c3 - a383 = O. (2.5) This siuaion occurs when he elbow is fully exended or reraced. The oher one is he inerior singulariy. I happens when 7i = d a2c2 + a3c O. (2.6) The wris singulariy can be idenified by checking he deerminan of he marix J22 o see if 7w = -s5 = 0. (2.7) Wris singulariy happens when wo join axes are collinear. Here si,ci represen sinqi,cosqi respecively. The neighborhood of singulariy is defined by posiive consans eb, c i, ew, which could be expressed as where Jo is called he OAT Jacobian marix. 3008

3 3 Hybrid Sysem Approach for Singulariyless Task Level Conroller By analyzing he singular configuraions, i can be found ha all he configuraions can no be avoided. The ask level conroller (2.4) is invalid in he neighborhood of singulariies. However, he join level conroller works in he whole work space. Combining he he ask level conroller and he join level conroller, a hybrid sysem approach is proposed o avoid singulariies. This secion firs briefly inroduces he Singulariyless Task Level Conroller(STLC) in a general form. The swiching condiions in he neighborhood of singulariies is hen exploied. 3.1 Hybrid Sysem Model Hybrid conrol sysems involve boh coninuous and discree dynamic sysems. The coninuous sae variables denoed by x E ~n describe he end-effecor posiion and orienaion. The discree sae variables, denoed by m E ~l or m E {mx,m2,...m}, represens he closeness o he singular poins. The hybrid sysem model of he robo can be described by x() = f(x(),m(),u()) (3.1) re() = g(x(),m(-)) (3.2) and he singulariyless ask level conroller is given by u() = h(x(), re()) (3.3) The dynamics, g, is governed by he singulariy condiions. Depending on he curren discree sae of he robo and coninuous saes, 9 gives he nex discree saes, - denoes ha re() is piecewise coninuous from he righ, g discreizes he coninuous saes and swiches he local conrollers. The deail of (3.3) is given in he nex subsecion. Here Max-Plus algebra is used o describe he discree even evoluion. Please refer [8] for deails of Max-Plus algebra. 3.2 Hybrid Conroller for Robos If he singular condiions in (2.8) are saisfied, he inverse Jacobian is modified by an improved Damped Leas Squares(DLS) mehod in cerain neighborhood of singulariies. However, feasibiliy analysis[7] of his mehod show ha in he region a or very close o singular configuraions, sabiliy is no guaraneed. The conroller swiches ino join level conroller. The hybrid conroller (3.3) is illusraed in his subsecion. The firs sep is o define he swich in he neighborhood defined by (2.8). In his region, conroller 3009 (2.4) is used and he inverse Jacobian is modified by varying damping facors. by The inverse Jacobian j~-i in (2.4) can be replaced [, o]1[, so =S3 o, o z Define Jw= J21 J22 (3.4) where J~ is he inverse of Jw if i is nonsingular. In he neighborhood of singular configuraions, he inverse of J~ can be compued by he damped leas squares echnique [1], J~ = (J,,. J~ + I. msl()) -1. J~ (3.5) where m~l() is a vecor of variable damping facors whose elemens are posiive. Based on he analysis of he inverse of J~[4] [1], ms1 can be defined as follows. where 81() = ml() m2() m3() mr() m () me() kb = kb0(1 --Ivbl/cb) ki = ki0(1 -ltil/~) k~ = kw0(1-17wl/~w) ki 0 ki O kb O 0 kb O0 kw 0 0 kw 0 and m81 is he posiive damping facor vecor. The damping facors depend on he closeness o he singular configuraions. The elemens of m81 are zeroes a nonsingular region. The conroller swiches smoohly beween singular and regular region. The second sep is o define a smaller neighborhood of singular poins, where ei < Ci, b ~ Cb, w < gw. The conroller swiches ino join level conroller in his region. Furher observaion of he singular condiions finds ha he wris singulariy does no have any effec on he arm. Two discree variables, mr and ms, are defined o represen arm and wris singulariies respecively. A swiching marix, m82, is defined by. m7 () = mr (-) sgn((ei - 7i) sgn((eb -- 75) 0 m8 () = m8 (-) sgn(e'+6-7~')g0 sgn(ew - %0) 0 (3.6) m 2()= [ mt()i3 3 o ] 0 (rot() ms())i3 3

4 where ci, b, w, he experimenally deermined consans, define he viciniy of he singular poins ha he DLS mehod fails o have sable equilibrium poins. The join level conroller is used insead of he ask level conroller in his region. The value of m~ and ms can eiher be 1 or 0. In order o avoid sliding mode a swiching surface, a dwell region, 5, is inroduced. The values of ms2 depend on no only he singulariy condiions, bu also he value of m~u (-). Afer he conroller swiches ino join space conroller, i can no swich back o ask space conroller unless he robo reach a larger region. This sraegy can effecively avoid chaering. The robo conroller is summarized by u = (I - m~2)ul + rn~2u2 (3.7) The framework of he conroller is shown below. Figure 1: Hybrid sysem conroller framework 3.3 Pah Planning for Swiching Surfaces To smooh he swiching rajecory, he rajecory given in Caresian space needs o be ransformed ino join space. The join rajecory needs o be replanned and synchronized o form he desired rajecory in ask space. A join level, he rajecory in ask space are mapped o join space by he following condiions. mq~n{ly d - h(qd)] + Iq d -- ql} min{y d -- JO d} The firs consrain minimizes he deviaion afer he ask is mapped ino join space, and ry o find a qd such ha he leas join movemen is needed. The second consrain ensures he planned join velociies are wihin he join velociy limis. The pah from q o qd is planned based on he desired velociy obained from he second and he hird consrains. The coninuiy of join velociies and ask level velociies are considered in he planning. A join level, he iniial velociy for every join is he join velociy prior o swiching. However, he pah for every join can no be planned separaely. The rajecory needed o be raveled by every join should be planned o finish simulaneously in join space. Before swiching back o he ask level conroller, he robo eners a region when reasonable join velociies can be obained by eiher inverse Jacobian or damped inverse Jacobian. This ensures he coninuiy of he join velociy afer swiching back o he ask level. 4 Sabiliy Analysis Before analyzing he sabiliy of he conroller, he following noaions are inroduced. The join space Q c ~6 is divided ino wo subspaces: (1) singular se Q~ such ha if q E Qs c Q hen I%1 < ~i and/or I~bl < Cb and/or ]~wl < ew; (2) regular se Qr such ha if q E Qr c Q hen 1"7il > ei and I%1 > eb and 1'Twl > ~w. An inersecion se Qrs is defined such ha ifqeqrs C Qhen I%1 <ei+hand/or I%1 < eb+5 and/or I~wl < ew +5. I is easy o see ha Qr~ c Qr. The ask space Y C ~6 is a se defined by he nonlinear ransformaion y = h(q). Corresponding o Qs, Qr, Qr~, Y~ c Y, Yr c Y, Yrs c Y can be defined in ask space. Basically, if y E Yr\Yrs, robo conroller works a ask level. If y E Y~, he robo works a join level. Depending on he curren values of ms2, ask level conroller or join level conroller or boh can work in Yrs. Lyapunov funcions on join level and ask level are defined as follows, ul(x) =' r, T V2(x) ~e 1 gplel +2e2 e2 1 T ~(kx, + x2)uk~x + x2) + ~1 Kx~ I is sraighforward o show ha V1, V2 are posiive definie, ~ is negaive definie in Y, V2 is negaive definie in Yr- Therefore he join level conroller and ask level conroller are asympoically sable in region Y and Yr respecively. To prove he sabiliy along swiching surface, he following lemmas are considered. Lemma 1 If boh he iniial posiion y(o) E Y~ and he desired posiion yd E Ys, he ransiion of y saisfies y E Y~ U Y~ under he join level conroller for appropriae seleced Kpl, K~i and 5. Proof. According o he coninuous consrains, righ afer he conroller swiches from ask space conroller o join space conroller, e2(0) = 0 d - q = O. 3010

5 Since he join error dynamics are decoupled, he ransiion of sysem can be analyzed join by join. Le kv = 2~wn, kp = W2n, ~ < 1 be one of he elemens of diagonal marices K,,Kp, he error ransiion in corresponding direcion is in he form of e() = e(o)e-~ ~/flsin(~nfl + ) where e(0) is he iniial error. Therefore for q(o) E Qs and qd E Q~, we have y() E Y~. For he ransiion ha el(0) 7 ~ 0 and e2(0) ~ 0, he error is in he form of e() = e(o)e-~/3sin(cz~z + ~,) + ~(O)/(confl)e-~ ~sin(canl~) where sine = ~3, cos~p = ~. Therefore, e() is bounded by le(0)] + ]~(0)[. Define 5 such ha q E Qs u Qrs for any q E Q~, q~ E Q~, hen in he ransiion y E ~ ~ ~. Lemma 2 If boh he iniial posiion y(o) E Yr\Yrs and desired posiion yd E Y~\Y~, he ransiion of y saisfies y E Y~ under he ask level conroller for appropriae seleced Kp, IKd and 6. Proof. The proof is similar o he proof of Lemma ]. <1 Theorem Along he swiching pah discussed in (3.6), conroller u is asympoically sable in racking he desired pah. Proof During he racking of a desired pah, four cases ha may happen on racking he desired pah are discussed here: (1) y E Yr and yd E Ys, (2) y E Y~ andydey~, (3) yey~ andydey~, (4) yey~ and yde~. (1): For his case, y E Y~,q E Q~. Before he end-effecor eners he singular area, he ask space conroller is running and he sysem is approaching he desired posiion yd G Ys. As long as he end-effecor ges ino he singular area, i changes ino case (4) (2): A his siuaion, he end-effecor eners Y~ firs before he end-effecor eners Yr\Yrs. The join space conroller works when y E YsUY~ and i changes ino ask space conroller afer y E Yr\Y~s. According o he definiion of m~2, he conroller will no change back o join space conroller unil he end-effecor hi he boundary of Y~. This case is hen changed ino case (3). (3) According o Lemma 2, he sysem is sable in he subse Yr- I is also rue for case (4) according o Lemma 1. 5 Experimenal Implemenaion and Tesing The proposed hybrid conroller is implemened on a PUMA 560 robo. The conroller runs under QNX 9 10 I 3~ -0 ~ce -071 : :. - -0:,5-0 ' ~ 2', 2 " Figure 2: Pah wihou swiching a wris singulariy -I S~2 -, 7~ i l r5 5 -, 7 57 o I ~ TA 1 ~ ~b)a =ng~ Figure 3: Hybrid sysem conroller wih wris singulariy operaion sysem, which is a realime operaion sysem. Choose zi = c~ = 0.15,Eb = 0.25,5 = 0.04,~ = 0.016, cb = 0.080, c~ = 0.03, kio = ki~ = 0.2, k~b = 1. The uni for posiions is meer, i is radius for angles. The uni for singulariy condiions is based on he compuaion of formulas (2.4,2.5,2.6). Four of he experimenal resuls are repored here. The firs wo repor he comparison of he DLS conroller and he hybrid conroller. The hird one records he rajecory of he end-effecor when inerior singulariy occurs. The forh one is a esing of moving he end-effecor ino he boundary singular configuraion. Figure 2 and figure 3 compares he he conroller wih swiching and he one wihou swiching. In figure 2, he pah a z direcion is a sraigh line from -0.3m o 0.35m. The oher direcions and he OAT angles are fixed. I can be seen ha he end-effecor is chaering a a small magniude when he wris is very close o he configuraion poins. Figure (d) shows he value for singulariy condiion 'Tw. Figure 3 shows ha he hybrid conroller is sable even a he singular poins, x is from 0.3m o 3011

6 i o o: 6 Conclusions 405 ~ o s96 ~1~ o o,'o N I -Io ~0 m Figure 4: Hybrid sysem conroller wih inerior singulariy a This paper presens a hybrid conroller for manipulaors which can work a or near singular configuraions. Furhermore, he sabiliy of he hybrid conroller has been heoreically proved. The primary conribuion of his work is ha he use of hybrid conroller o saisfy differen consrains in differen workspace of he robo and he sabiliy analysis for hese cases. The discree dynamics model described in Max-Plus algebra is suiable for analyzing consrained hybrid sysems. References -0is =0 ~ 3o = 2 z oh o~ i I c.~ ~ Figure 5: Hybrid sysem conroller wih boundary singulariy -0.35m. The oher direcions and he OAT angles are fixed. The end-effecor passes he wris singular configuraion a abou x = -0.05m, which is also very close o he inerior singular configuraion. There are deviaions from he desired pah, bu i is sable. The end-effecor can pass by he singular configuraions. To compare i wih Figure 2, he horizonal axis is no real ime a he region 7w E (0.016,-0.020). When he end-effecor passes he inerior configuraions, Figure 4 shows he pah wih respec o ime. The conroller swiches ino join level a = 5s, where 7i : 0.016, and i swiches back o ask level a = 18s, where ~i = Boh he desired posiion and he acual posiion are changing slower, since he pah is replanned in join space. Figure 5 illusraes he rajecory of moving he end-effecor by he spaceball o make he arm fully exended. I can be seen ha he end-effecor is sable a he singulariy poin. The conroller swiches ino join space conroller a % = [1] Y. Nakamura, H. Hanafusa,"Inverse kinemaic Soluions Wih Singulariy Robusness for Robo Manipulaor Conrol," Journal of Dynamic Sysems, Measuremen, and Conrol, Vol. 108, pp , Sep., [2] S. Chiaverini, B. Siciliano, O. Egeland,"Review of he Damped Leas-Squares Inverse Kinemaics wih Experimens on an Indusrial Robo Manipulaor," IEEE Trans. Conrol Sysem Technology, Vol. 2, No.2 pp , Sep., [3] M. Sampei and K. Furuaa,"Robo Conrol in he Neighbourhood of Singular poins," IEEE Journal Roboics and Auomaion, Vol. 4, No. 3, pp , June, [4] F. Cheng, T. Hour, Y. Sun, T. H. Chen, "Sudy and Rosoluion of Singulariies for a 6 DOF PUMA Manipulaors,"IEEE T. Sys. Man. Cyberneics B: Cyberneics,Vol.27,No.2, April 1997 [5] M. W. Spong, "Applicaions of Swiching Conrol in Robo Locomoion," Workshop on Inelligen Conrol in Roboics and Auomaion, IFAC World Congress,Beijing, China, July 1999 [6] B. E. Bishop, M. W. Spong, "Conrol of Redunden Manipulaors Using Logic-Based Swiching, "Proceedings of IEEE Conference on Decision and Conrol, Leuven, Belgium, pp , May 1998 [7] M. Kirdanski, N. Kirdanshi, D. Lekovid, "An experimenal sudy of resolved Acceleraion Conrol of Robos a Singulariies: Damped Leas-Squares Approach," Journal of Dynamic Sysems, Measuremen, and Conrol, Vol. 119, pp , March, [8] F. L. Baccelli, G. Cohen, G. J. Olsder, J. Quadra, Synchronizaion and Lineariy, an Algebra for Discree Even Sysems, John Wiley & Sons, [9] T. J. Tarn, A. K. Bejczy, Z. T. Marh, A. K. Ramadorai, Kinemaic Characerizaion of he PUMA 560 Manipulaors, Washingon Universiy,