Trajectory Optimization of Flexible Mobile Manipulators Using Open-Loop Optimal Control method

Transcription

1 rajectory Optmzaton o Flexble Moble Manpulators Usng Open-Loop Optmal Control method M.H. Korayem and H. Rahm Nohooj Robotc Lab, Mechancal Department, Iran Unversty o Scence and echnology hkorayem@ust.ac.r, hamedrahm.n@gmal.com Abstract. In ths paper Open-loop optmal control method s proposed as an approach or trajectory optmzaton o lexble moble manpulator or a gven two-end-pont task n pont-to-pont moton Dynamc equatons are derved usng combned Euler Lagrange ormulaton and assumed modes method. o solve the optmal control problem an ndrect method va establshng the Hamltonan uncton and dervng the optmalty condton rom Pontryagn's mnmum prncple s employed. he obtaned equatons provde a two pont boundary value problem whch s solved by numercal technques. he man advantage o ths method s obtanng varous optmal trajectores wth derent characterstcs by changng the penalty matrces values whch able the desgner to choose the best trajectory. Fnally, a two-lnk lexble manpulator wth moble base s smulated to llustrate the perormance o the method. Keywords: Moble Manpulator, Flexble Lnk, Optmal rajectory, Optmal Control. Introducton Moble manpulators due to ther extended workspace oer an ecent applcaton or wde areas. But these systems are usually power on board wth lmted capacty. So because o heavy component such as lnks, the moble manpulator needs greater motor, base, and source o energy whch leads to consume more energy or the same movement. Hence, usng lght and small platorms and motor actuators n order to mnmze the nerta and gravty eects on actuators wll help a moble manpulator to work n an energy- ecent manner. But the lnk delecton s unavodable when the lnks are lght and long, so t leads us to use the elastc manpulators. Korayem and Gharblu used Iteratve Lnear Programmng (ILP) method or the maxmum allowable dynamc load (MADL) calculaton o a rgd moble manpulator []. Korayem and Nkoobn, used the optmal Control Approach to nd the maxmum load carryng capacty o rgd moble manpulators or a gven two-end-pont task []. he assumed mode expanson method s used by Sasadek and Green [3,,5] to derve the dynamc equaton o xed base lexble manpulator. In above mentoned works only moblty o base or lexblty o lnks have been consdered, and the synthess o moble base wth lexble lnks has not been studed. In [6,7] a computatonal algorthm to MADL determnaton va lnearzng the dynamc equaton and constrants s presented on the bass o ILP approach or lexble C. Xong et al. (Eds.): ICIRA 8, Part I, LNAI 53, pp. 5 63, 8. Sprnger-Verlag Berln Hedelberg 8

2 rajectory Optmzaton o Flexble Moble Manpulators 55 moble manpulators. But, n ILP method, the lnearzng procedure and ts convergence to the proper answer s a challengng ssue, especally when nonlnear terms are large and luctuatng. As a result n none o these papers the lnk lexblty has been consdered ether n the dynamc equaton or smulaton procedure. he man contrbuton o ths paper s to propose open loop optmal control method or path plannng o wheeled moble manpulator wth lexble lnk. he dynamc equaton o lexble lnk manpulator s derved by usng the generalzed Euler-Lagrange ormulaton and assumed modes method. And the extra DOFs arose rom base moblty are solved by usng addtonal constrant unctons and the augmented Jacoban matrx. Hamltonan uncton or a proper objectve uncton s ormed, and then necessary condtons or optmalty are obtaned rom the Pontryagn's mnmum prncple. he obtaned equatons establsh a wo Pont Boundary Value Problem (PBVP) solved by numercal technques. he general ormulaton to nd the optmal path at pont-to-pont moton s derved. In comparson wth other method the open-loop optmal control method does not requre lnearzng the equatons, derentatng wth respect to jont parameters and usng o a xed-order polynomal as the soluton orm. he remander o the paper s the smulaton or a two-lnk moble manpulator wth lexble lnks n order to nvestgate the ecency o the presented method. Modelng o a Manpulator wth Multple Flexble Lnks For general n-lnk lexble robots, the vbraton v( η, t) o each lnk can be obtaned through truncated modal expanson, under the planar small delecton assumpton o the lnk. n v ( η, t) = φj ( η ) ej ( t), =,, n j= () () j where n s the number o modes used to descrbe the delecton o lnk ; φ j ( x ) and e t are the j th mode shape uncton and j th modal dsplacement or the th lnk, respectvely. In moble manpulators manpulator degrees o reedom s denoted by n m and the base degrees o reedom by n b, then the overall system degrees o reedom wll be n = nm + nb. Meanwhle, the end eector degrees o reedom n cartesan space s denoted by m. It s well known that n most moble manpulator systems, we have n > m. As a result, the system has knematc redundancy or extra degrees o reedom on ts moton equal to r = n m. here s a well-known method o redundancy resolutons that apples addtonal sutable knematc constrant equatons to system dynamcs and results n smple and on-lne coordnaton o the moble manpulator durng the moton. hs method borrows rom the extended Jacoban matrx concept. By usng the Lagrangan assumed modes method the dynamc equaton o lexble moble manpulator n compact orm could be obtaned as ollows : Mq + H ( q, q ) + G( q) = U ()

3 56 M.H. Korayem and H.R. Nohooj where M s the mass matrx, H s the vector o Corols and centrugal orces and G descrbes the gravty eects and q = ( qb, qr, q ) s generalzed coordnate o the system that q b, q r, q are denng the moble base moton, rgd body moton o lnks and lexblty o lnks respectvely. Consder a n DOFs moble manpulator wth generalzed coordnates q = [ q ], =,,..., n, and a task descrbed by m task coordnates rj, j =,,..., m wth m< n. By applyng r holonomc constrants and c non-holonomc constrants to the system, r+c redundant DOFs o the system can be drectly determned. hereore m DOFs o the system s remaned to accomplsh the desred task. As a result, we can decomposed the generalzed coordnate vector as [ ] r c q = qr q, where nr qr R + s the redundant generalzed coordnate vector determned by applyng constrants and m qr R s the reman generalzed coordnate vector. he system dynamcs can also be decomposed nto two parts: one s correspondng to redundant set o varables, q r, and another s correspondng to non-redundant set o them,. hat s, q nr by denng the state vector as: U M M q C + G r rr, rnr, r r r U = M nr rnr, M + nrnr, q nr Cnr G + nr [ ] [ ] nr nr X = X X = q q () non-redundant part o Eq.(3) can be rewrtten n state space orm as: X = X X = [ X N( X) + D( X) U] (5) where D = M and N = M ( H( X, X) + G( X)). hen optmal control problem s to determne the poston and velocty varable X () t and X () t, and the jont torque Ut () whch optmze a well-dened perormance measure when the model s gven n Eq.(5). (3) 3 Formulaton o the Optmal Control Problem he basc dea to mprove the ormulaton s to nd the optmal path or a speced payload, and then maxmum payload s obtaned va an teratve algorthm. For the sake o ths, the ollowng objectve uncton s consdered where U() t t Mnmze J = L( X, U ) dt (6) t LXU (, ) X X U = + W + W R (7)

4 rajectory Optmzaton o Flexble Moble Manpulators 57 Integrand L(.) s a smooth, derentable uncton n the arguments, X K = X KX s the generalzed squared norm, W and W are symmetrc, postve sem-dente (m m) weghtng matrces and R s symmetrc, postve dente (m m) matrces. he objectve uncton speced by Eqs. (6) and (7) s mnmzed over the entre duraton o the moton. he desgner can decde on the relatve mportance among the angular poston, angular velocty and control eort by the numercal choce o W, W, and R whch can also be used to convert the dmensons o the terms to consstent unts. Accordng to the Pontryagn's mnmum prncple, the ollowng condtons must be satsed X = H / ψ, ψ = H / X, = H / U (8) where the Hamltonan uncton wth dene the nonzero costate vector ψ = ψ ψ s dened as: ( X X U ) X [ N X D X U] W W R Η(X, U, ψ) = ψ + ψ ( ) + ( ) (9) So, accordng to Eq. (8), the optmalty condtons can be obtaned by derentatng the Hamltonan uncton wth respect to states, costates and control.he control values are lmted wth upper and lower bounds, so the optmal control s gven by > U = R D U < R D < U + + U R D ψ U + ψ ψ U R D ψ < U he actuators whch are used or medum and small sze manpulators are the permanent magnet D.C. motor. he torque speed characterstc o such D.C. motors may be represented by the ollowng lnear equaton: + () U = K K X, U = K K X () s s sn = s m, sn mn θ = θ θ θ n, τ s the stall s ω s the maxmum no-load speed o the motor. m In ths ormulaton, or a speced payload value, m derental equatons s where K = [ τ τ τ ], K dg[ τ ω τ ω ] torque and gven n order to determne the m state and costate varables. Control equatons wth ths set o derental equatons and the boundary condtons construct a standard orm o PBVP, whch s solvable wth avalable commands n derent sotware such as MALAB, C++ or FORRAN. Smulaton or a Flexble Planar Wheeled Moble Manpulator A two-lnk planar lexble manpulator s mounted on a derentally drven moble base at pont F on the man axs o the base as shown n Fg.. A concentrated payload o mass m p s connected to the second lnk.

5 58 M.H. Korayem and H.R. Nohooj Fg.. wo-lnk moble manpulator wth lexble lnks All requred parameters o the robot manpulator are gven n able. able. Smulaton Parameters Parameter Value Unt Length o Lnks L=L=.5 m Mass m=3, m=3 Kg Moment o area (Lnk) I=I= e- m Module o elastcty (Lnk) E=E=e Kg.m Max. No Load Speed ωs=ωs=3.5 Rad/s Actuator Stall orque τs=τs= N.m he load must be carred rom an ntal pont wth coordnate ( x e =.56m, y e =.5m) to the nal pont wth coordnate ( x e =.76m, y e =.36m) durng the overall tme t = s such that optmal trajectory between these two ponts at a speced tme s desred. It should be noted that nal load poston s not easble wthout the base moton. Smultaneously, the moble base s ntally at pont ( x = m, y = m, θ ()=) and moves to nal poston ( x = m, y =.5m, θ (end)=.35 ) and Suppose that L s cm []. In order to dene the moble manpulator wth lexble lnk wth consderng one mode shape or each lnk the mechancal system generalzed coordnates can be

6 rajectory Optmzaton o Flexble Moble Manpulators 59 chosen as: q [ q q q3] x y θ θ θ e e are generalzed coordnates o the base and q [ θ θ] q = [ e e ] are modal dsplacement o lnk. So accordng to Eq.(), (, ) 3 expressed as ollows: = =, where q = x y θ = are lnks angles and v x t can be = φ ( ) ( ), = φ ( ) ( ) () v x e t v x e t where wth consderng the smply support mode shape [7], φ can be computed as: j jπ x (3) φj ( x ) = sn, = and j = L Snce the moton s n horzontal plan the gravty eects ( Gr, G ) wll be zero. And the operatonal coordnated o the end eector can be speced by pee = [ xe ye ] and the end eector degrees o reedom n the cartesan coordnate system wll be m =. he system degree o reedom s equal to n=5, hence the system has redundancy o order R = n-m = 3 and needs three addtonal knematcal constrants or proper coordnaton. Meanwhle, the moble base has one nonholonomc constrant (c=).e. the rollng wthout slppng condton or the drven wheels: x sn( θ) y cos( θ) + L θ =. Hence, the number o knematcal constrants whch must be appled to system or redundancy resoluton s equal to r = R-c =. In ths case, wth the prevously speced base trajectory durng the moton, the user-speced addtonal constrants can be consdered as the base poston coordnates at pont F( x, y ), whch gves x = Xz ; y = X, where z X z and X z are unctons n terms o tme whch by derentatng them wth can also be obtaned. A th order polynomal uncton s consdered or the base trajectory along a straght-lne path rom (, ) to (,.5) durng the overall tme s. Velocty at start and stop tme s consdered to be zero. From the base moton, x and y are known, thereore the base angle at ntal tme θ ( t) be speced, angle and angular velocty o the base ( θ( t), θ( t)), can be determned by solvng nonholonomc constrant equatons. Here the ntal base angle s consdered to be zero, θ () =, thereore nal base angle wll be obtaned as θ ( end).35 = Rad. By denng the state vectors as ollows: [ ] [ ] X Q x x x x, X Q x x x x. () = = = = 6 8 he sate space orm o Eq. () can be wrtten as x = x, x = F( ) ; =,, (5) where F () can be obtaned rom Eq.(7). And the boundary condton can be expressed as ollows: x x = 9, x = x = ; x = 3, x = x( ) = x( ) =, =... = (6)

7 6 M.H. Korayem and H.R. Nohooj In order to derve the equatons assocated wth optmalty condtons, penalty matrces can be selected as ollows: = (,,, ), = (,,, ); = (, ) (7) W dag w w w w W dag w w w w R dag r r hen, by consderng the costate vector as ψ = [ ϕ ], =,,8. By derentatng the Hamltonan uncton wth respect to the states as ollows: H ϕ =, =,,8 x Control unctons are computng by derentatng the Hamltonan uncton wth respect to control and settng the dervatve equal to zero. Ater usng the extrmal bound o control or each motor, by substtutng the obtaned control equatons nto (5) and (6), these equatons orms 6 nonlnear ordnary derental equatons that wth 6 boundary condtons gven n Eq. (6), constructs a two pont boundary value problem. hs problem can be solved usng the BVPC command n MALAB. In ths case, the payload s consdered to be kg and the purpose s to nd the optmal path between ntal and nal pont o payload n such a way that the smallest amount control value can be appled and the angular velocty values o motors be bounded n ±.8 rad / s. By consderng the penalty matrces as: W = [], w = w =., w6 = w8 = and R = dag{.,.} the optmal path wth mnmum eort can be obtaned, but the angular veloctes are greater than.8 rad/s. hereore or decreasng the veloctes, w must be ncreased. A range o values o w and w whch s used n smulaton are gven n able 3. W, R, 6 wthout changes. able.he values o W used n smulaton w 8 reman case w Dag(.) Dag() 3 Dag() Dag() he end-eector trajectores n XY plane are shown n Fg. 6 or these cases. Fg. shows the angular poston o jonts wth respect to tme. hs graph shows that by ncreasng the w, the angular poston change to approach approxmately to a straght lne. Fg. 3 shows the angular veloctes o the rst and second jonts. It can be ound that by ncreasng the w, extermum values o angular velocty reduce rom -. rad/s to -.8 rad/s. By growng the w, the angular veloctes reduce greatly or the rst to second cases whereas at the thrd case a lttle reducton has been occurred n spte o great ncrease n w. he computed torque s plotted n Fg.. As t can be seen, ncreasng the w and w causes to rase the torques, so that or the last case the torque curves reach to ther

8 rajectory Optmzaton o Flexble Moble Manpulators 6 bounds at the begnnng and end o the path. hs result s predctable, because ncreasng the w, decreases the proporton o R and the result o ths s ncreasng the control values. he mode shapes s plotted n Fg. 5 shows the lexble deormaton o system n case3.at last Arm motons wth related end-eector trajectory n ths case are plotted n Fg. 7. (n each gure -, --, -.,.., denotes cases,, 3 and respectvely) angular poston o lnk.5 angular poston o lnk jont angle (rad).5.5 jont angle (rad) tme (s).5.5 tme (s) Fg.. Angular postons o jont and angular velocty o lnk angular velocty o lnk jont velocty (rad/s) tme (s) jont velocty (rad/s) tme (s) Fg. 3. Angular veloctes o jont and hereore, the rst path s the optmal path wth the least control values, whereas ts angular velocty s the largest magntude. Fnally, the optmal path s the thrd path whch ts velocty magntude s bounded n ±.6 rad / s nterval and the torque values s the lowest. On the bass o the objectve contrast prncple, there s not the soluton that satses all the desred objectves smultaneously e.g. the optmal path wth mnmum eort has maxmum velocty and the optmal path wth mnmum velocty has maxmum eort. Consequently, n ths method, desgner compromses between derent objectves by consderng the proper penalty matrces.

9 6 M.H. Korayem and H.R. Nohooj orques o motor orques o motor orque (N-m) orque (N-m) me (s) me (s) Fg.. orques o motor and x -3 mode shape o Lnk x -3 mode shape o Lnk mode shape Lnk tme (s) mode shape Lnk - - Fg. 5. mode shape o lnk and tme (s).5 end eector trajectory.5 lnks postons y drecton.5 y drecton x drecton x drecton Fg. 6. End eector trajectory n XY plane Fg. 7. Arm moton n XY plane

10 rajectory Optmzaton o Flexble Moble Manpulators 63 5 Concluson In ths paper, ormulaton o the trajectory optmzaton or moble lexble lnks manpulator n pont-to-pont moton, based on the open-loop optmal control approach s presented. In ths method, the complete orm o the obtaned nonlnear equaton s used, and unlke the prevous works lnearzng the equatons or usng o a xed-order polynomal as the soluton orm s not requred. hs ormulaton can be used or path plannng o lexble moble manpulators va denng the proper objectve uncton and changng the penalty matrces to acheve the desred requrements. hereore, an ecent algorthm on the bass o PBVP soluton s proposed to optmze the path n order to acheve the predened objectve. One o the advantage o ths method that desgner can compromse between derent objectves by consderng the proper penalty matrces and s able to choose the proper trajectory among the varous paths. Reerences. Korayem, M.H., Gharblu, H.: Maxmum allowable load o moble manpulator or two gven end ponts o end-eector. Int. J. o AM (9-), (). Korayem, M.H., Nkoobn, A.: Maxmum Load Carrng Capacty o Moble Manpulators usng Optmal Control Approach. Robotca Journal (accepted or publcaton ) 3. Sasadek, J.Z.: wo lnk lght weght lexble manpulator. In: Proceedngs o IASED Internatonal Conerence on Advances n Robotcs, vol., pp Acta Press, Anhem (985). Green, A., Sasadek, J.Z.: Dynamcs and trajectory trackng control o a two-lnk robot manpulator. Journal o Vbraton and Control (), 5 () 5. Green, A., Sasadek, J.Z.: Robot Manpulator control or rgd and assumed mode lexble dynamcs models. In: AIAA Gudance, Navgaton, and Control Conerence and Exhbt (3) 6. Korayem, M.H., Garblu, H.: Analyss o wheeled moble lexble manpulator dynamc motons wth maxmum load carryng capactes. Robotcs and Autonomous Systems 8(-3), () 7. Garblu, H., Korayem, M.H.: rajectory optmzaton o lexble moble manpulators. Robotca (3), (6)