Chapter 6: Dynamic Simulation Environment

Transcription

1 Chapter 6: Dnac Suaton Envronent Prevous neatc and dnac anass has deonstrated the snguart-free worspace and the hgh force-bearng characterstcs of the Wrst. Ths chapter copetes the dnac ode of the Carpa Wrst b presentng a cosed-for souton and agorth to the te response (forward dnac) probe. Wth ths ode suaton of the Wrst n an appcaton envronent can be perfored to generate varous contro-sste agorths. An eape of such an appcaton woud be to create a neura networ contro devce traned va the forward dnac ode. Ths forward dnac ode w be deveoped fro the equatons of oton whch have been derved n cosed for b drect appcaton of Lagrange s equatons to the neatc ode. The dnac ode assues a reatve assve too and consders a gravtatona nerta Coros and groscopc effects. The forward dnacs presents the equatons of oton n a for that s evauated over te to deterne trajector nforaton based on actuator nputs. The resuts of these equatons s a suaton envronent that can represent the Wrst n appcaton for odeng desgn and contro purposes. 6. Introducton Ths chapter presents a souton to the forward dnac anass for the Carpa Wrst. Ths te-response or forward dnac anass s based on the equatons of oton deveoped usng Lagrange s equatons and the Wrst neatcs. The resuts of ths anass w be a procedure that ncudes the forward dnac equatons and soves for the jont space trajector gven the otor-torque te hstor. The goa of ths forward dnac ode s to provde a suaton envronent that can be used for severa purposes ncudng deveopng a hgh-eve Wrst contro. For eape the suaton envronent can be used to tran a neura networ contro sste based on varous operatng crtera. Another eape deonstrated w be to use the suaton envronent as part of an optzaton routne sovng trajector snthess probes. Ths chapter evoves the dnacs of the Carpa Wrst nto a for that can sove for the jont acceeratons epct. The startng pont are the current equatons of oton descrbng the nverse dnacs (sovng requred otor nputs epct) whch were derved usng a Lagrangan approach wth the generazed coordnate sste chosen as the nput jont anges and the neatcs deveoped n canonca for. The Lagrangan epressed as a functon of the generazed coordnates depends on the anpuator energ state.e. the potenta and netc energ. The Lagrangan was cacuated fro poston and veoct nforaton whch are avaabe n cosed for.. Thus the Lagrangan a aso be epressed n cosed for as shown n Chap. 5. The Lagrangan foruaton was used because of ts abt to reove nterna constrant forces fro the equatons of oton and because t aowed the choce of generazed coordnates n ths case the jont-space coordnate sste. In deveopng the nta equatons of oton tensor subscrpt notaton was used and w contnue to be used throughout wth the standard Ensten suaton conventon assued (Frederc and Chang 965). 59

2 Stephen L. Canfed Chapter 6: Dnac Suaton Mode Wrst Dnacs The Wrst dnacs are based on the neatc ode deveoped n Chap. whch provdes a appng between the nput space jont anges (jont space) and the output space too pose (too space). Ths appng s represented n Eq. 6.: ( α β p) f ( ) ( ) f ( α β p) (6.) where: α β and p represent the output coordnates - represent the nput space coordnates and the functons f f represent the forward and nverse neatc appngs respectve. 6.. Anguar Veoct of the Dsta Frae Reca the rotaton of the dsta pate (and the rgd attached too Fg. 6.) s caused b the te rate of change of the dpane nora or correspondng the change of the orentaton anges α and β (Chap. 5). The vector ω was defned as the anguar veoct of the {D} frae reatve to the {B} frae descrbed n {B} frae coordnates and gven as: 0 cα sα 0 0 sαβ ω sα cα 0 0 β cαβ (6.) α α 6.. Transatona Veoct of the Too Center of Mass Referrng to the frae assgnent fro Chap. 5 Fg. 5. and the ounted too shown n Fg. 6. v s the veoct of the too center of ass G wth respect to frae {B} epressed n frae z D {D} G G z B {B} B Fgure 6.: Wrst Mounted Too

3 Stephen L. Canfed Chapter 6: Dnac Suaton Mode 6 {B} coordnates ( ) D v δ R J ω R p ω R G (6.) j j j j d j where ε s the perutaton tensor and δ s the Kronecer deta. 6. Lagrange s Equatons for the Wrst-Isoated Probe Lagrange s equatons are agan epressed for the parae structure Carpa Robotc Wrst as n Chap. 5. In ths chapter the equatons w be epanded to sove epct for jont space acceeratons. Startng wth Lagranges equatons: d dt q q Q (6.4) where q represents the generazed coordnates and Q the generazed forces the three nput jont paraeters are chosen as the generazed coordnates. The generazed forces assocated wth ths choce of coordnates are the nput actuator torques M. Lagrange s equatons becoe: d M dt (6.5) wth: T V. (6.6) The netc and potenta energes are gven b: T vv ωrji jω (6.7) V g[ pd R pd R jgj] where: I j s the oent of nerta tensor of the too epressed n the {D} frae and G s the vector ocatng the center of ass of the too and dsta pate wth respect to the center of the dsta pate epressed n the {D} frae (Fg. 6.). 6.. Dervatves of Knetc and Potenta Energ The Lagrangan ust be dfferentated wth respect to the nputs to the sste the te rate of change of these nputs and te. The necessar dervatves of both the potenta and netc energ are: T j v Rj I jω ω I ω j (6.8) T v ω Rj I j d dt T T T

4 Stephen L. Canfed Chapter 6: Dnac Suaton Mode 6 d dt v v v [ ( ) ( ω ) ω ] [ ]( j j ) j I jω Rj I j ( ( ) ) T v R I ω p R j [ ( ) j ] V g R p G Puttng these nto Lagrange s equaton Eq. 6 gves the equatons of oton:: (6.9). (6.0) v v ω ω [ ( ) ( ) ] [ ]( j j ) ω j ( I jω Rj I j ( ) ) M v R I v RI j j Rj ω ω I ω p j [ ( ) j ] g R p G Equaton 6. s epanded to soate the acceeraton coponents as deonstrated n the foowng equatons: v v v [( ) ( ) ] ( ) j ( RI j jω) I j RI ( ω j j j ) j j M v v R I ω R I p j [ ( ) j ] g R p G v ω ( ) ω v ω ( ) ( j j ) M v R I R I j (6.) j j j j v R I ω ω I ω j j j j ( j j j ) v RI ω j I ω RI p j [ ( ) j] R j j jω ω jω v R I I g R p G v ω [ ( ) ω ] v v R ω j ( ) ( j j ) M v R I R I { j j j j ( j j j ) v R I ω I ω R I Rj v R ω ω ω j I j I j p j g R p G } [ ( ) d j ] (6.) (6.) (6.4) j

5 Stephen L. Canfed Chapter 6: Dnac Suaton Mode 6 v v ω [ ( ) j j ] ( ) ( j j ) ω j v R j ( I jω Rj I j ) v Rj I jω ω I jω p j [ ( ) d j ]} M RI v RI { g R p G (6.5) Equaton 6.5 gves the equatons of oton n a for that soates the acceeraton of the jont anges wth coeffcents that are functons of jont poston on. Ths resut s rewrtten as: M A( ) B( ) C( ) (6.6) [ ] where: M s the th nput otor oent A() represents the nerta coponents of the wrst and too a functon of and sued over B( ) represents the Coros and centrfuga nerta ters and C() represents the gravtatona ters both functons of and sued over. Equaton 6.6 presents the equatons of oton n a for whch deonstrates the effect of each otor oent on the anpuator s oton (nverse dnacs). When the anpuator path s nown the requred oent or torque for each otor can be deterned. Aso the bendng stresses that occur n the Wrst eg ebers are gven b these equatons. ote that ths s for the bendng stress occurrng n the pane defned for each eg b the basa and dsta revoute aes generated fro the actuators n overcong statc and dnac nerta oadng. Bendng stress n the pane of the Wrst eg ebers s generated fro oent reactons at the ocaton of the basa and dsta revoute aes. Detaed anass of these out-of-pane bendng oads as we as shear and aa oads n the Wrst ebers has been presented b Canfed et a. 995 for the parae anpuator archtecture and b Ganno 996 specfca for the Carpa Wrst. Thus Eq. 6.6 s portant both n szng the actuators provdng votage contro to the otors durng operaton and n generatng dnac stress nforaton for echanca desgn of the wrst coponents. The focus of ths chapter s to create a suaton ode of the parae archtecture Wrst. The suaton ode conssts of the equatons of oton cast n the for of a souton for the te response probe.e. a for that can be soved epct for the jont acceeratons. These equatons caed here the forward dnac equatons w gve the jont acceeratons as a functon of a nput oents as we as groscopc Coros and gravtatona effects. Therefore the equatons gven n 6.6 need to be soved for the jont acceeraton. The souton proceeds as foows. Frst the acceeraton coponents are soated as: M B C A. (6.7) [ ( ) ()] [ () ] Epandng the ndependent acceeraton ters: M B ( ) C () A A A. (6.8) [ ] () () ()

6 Stephen L. Canfed Chapter 6: Dnac Suaton Mode 64 Equaton 6.8 gves the th equaton of oton the nput of one otor. To sove unque for a the jont acceeratons - the effects of a the nputs are gven -: [ ( ) ()] () () () [ ( ) ()] () () () B ( ) () M B C A A A M B C A A A. (6.9) M Wrtng n atr for where: and [ ] () () () C A A A A A A A A A A A A A A M (6.0) () () () () () () () () () [ ( ) ()] [ ( ) () ] ( ) () M B C Μ M B C M [ B C ] Fna the jont acceeratons are soved: A M. (6.) 6... Epandng the Equatons of Moton Dervatves of the anguar and transatona veoctes are requred n the equatons of oton. Foowng the conventon apped n tang dervatves of the energ functons parta dervatves are taen wth respect to the generazed coordnates or jont paraeters and. For the veoct vector v the dervatves are gven n Eqs D v δ R J ω R p ω R G (6.) ( ) ( n ) j j d j n p j ω R ω p R p J j j d d ( δ R ) J j (6.) Rn D j D ωj Gn Rn G n (6.4) j j D ( δ R ) J j ( R pd Rn Gn )

7 Stephen L. Canfed Chapter 6: Dnac Suaton Mode 65 v ( δ R ) J J j j p j R j R pd R p D j n j D Gn R G n n ω ( ( )) j D R p R Gn (6.5) v 0 0 j d n. (6.6) Parta dervatves of the anguar veoct vector wth respect to the generazed coordnates are gven n Eqs sα β ω cα β (6.7) α α sα cα β α cα sα β (6.8) α sα α c α β sα α c β cα sα α sα cα β β α α α (6.9) (6.0) 0 0. (6.) 0 Parta Dervatves of the too space anguar coordnate veoctes α and β are aso taen wth respect to the generazed coordnates and. Frst the too space anguar veoctes are gven as a functon of the dpane nora vector : α β (6.) ( ) The vector n s defned to represent the nteredate veoct coponents:

8 Stephen L. Canfed Chapter 6: Dnac Suaton Mode 66 then: n p (6.) or n J J jj p (6.4) and parta dervatves of n reduce to eeents of the Jacoban as: n j δ Jj Jj j J (6.5) ow the parta dervatves of the too space coordnates are epressed n Eqs α J J (6.6) α J J J J β ( )( J J ) α ( ) ( ) (6.7) 0 (6.8) J J ( ) J J J J ( )( )( ( J J ) ) ( ) ( ) β (6.9) (6.40) 0 (6.4) The rotaton atr that rotates the dsta frae nto base frae coordnates B D R s a functon of the jont space paraeters. The parta dervatve reatve to the jont space paraeters s gven n Eq. 6.4.

9 Stephen L. Canfed Chapter 6: Dnac Suaton Mode 67 cαsβ sαcβ cα cαcβ sαsβ sαsβ cαcβ sα sαcβ cαsβ cβ 0 sβ α α α α α α (6.4) The fna eeents of the equatons of oton are the parta dervatves of the Wrst Jacoban atr J. These are perfored as n the equatons above and as deonstrated n Chap Sovng the Forward Dnacs The equatons of oton cast n the forward dnac for are gven n Eq. 6.. Wth poston veoct and the nput otor torques the resutng jont space acceeratons are cacuated. Snce poston and veoct are sar a functon of acceeraton the souton resut s nstantaneous. In the te response suaton ode a sa te nterva or step t w be ntroduced. The nstantaneous acceeraton resut w then be assued vad over the te nterva t. Poston and veoct nforaton s cacuated at the end of the te nterva fro the acceeraton resut and the nta path nforaton. Wth the updated path poston the process s repeated cacuatng a new acceeraton and updatng the path nforaton. The resut s deonstrated n the foowng equatons: Let ndcate an nta path poston and a successve path poston separated b the te step t. Then the paraeters over ths path nterva are defned as: t the te at poston t the te at poston t (t - t ) the te step. and the poston veoct acceeraton paraeters: the jont space poston vector at path postons and respectve. the jont space veoct vector at path postons and respectve. the jont-space acceeraton vector over the path fro poston to. M M the vector of otor oents at path postons and respectve. To begn the te response suaton gven nforaton are the poston and veoct epressed n jont-space coordnates at the nta poston t 0 () 0 () 0 (6.4) 0 0 Aso gven are the otor torque te hstores M (t) -. Fro these functons torque vaues are evauated at the th path poston for otor as M M t (6.44) Startng at poston and as we as the otor torques are used to sove : ( ) A M (6.)

10 Stephen L. Canfed Chapter 6: Dnac Suaton Mode 68 where the subscrpt denotes the path poston. Poston and veoct nforaton are updated as: t t (6.45) t t t Fna the subscrpt s updated to trace the path poston: t t (6.46) The resuts of ths technque s a te hstor of the anpuator path trajector. Ths resut can be epressed n too space coordnates usng the neatc reatons deveoped. In deveopng the te response ode a ture of too-space and jont-space coordnates were used. The functona reatonshp between these fraes as we as there dervatves are epressed as: αααβββ ppp g (6.47) ( ) ( ) and have been deveoped n ths and prevous wor (Chaps. 4 Canfed et a. 996). The edate resut of the te response ode s the path trajector te hstor epressed n jont-space coordnates. Snce these are tpca desred n too coordnates the above neatc functon w be epoed to transfor the path as desred. 6.5 Resuts and Concusons Usng Haton s Prncpe a forward dnac anass was carred out for a paraearchtecture robotc wrst wth three nputs actuated reatve to a fed base. Fro ths anass a ode was deveoped for fu anpuator suaton. Path trajector te hstor was deveoped for an eape appcaton. The resuts of ths suaton ode coud aso be deonstrated n creatng a hgh-eve controer (Artfca eura etwor).