Simulation of a 2-link Brachiating Robot with Open-Loop Controllers

Transcription

1 Simulation of a -link Bachiating Robot with Open-oop Contolles David Uffod Nothwesten Univesit June 009

2 . Poject Oveview The goal of this poject was to wite a complete simulation of a -link swinging obot the MonkeBot. The simulation models swinging, fee flight, and impacts with useinputted paametes. A seconda goal of this poject was to attempt to identif the stabilit of the MonkeBot with an open-loop contolle. Gaits fo hoizontal swinging and thei dependence on initial conditions wee eamined.. MonkeBot Oveview The MonkeBot is a two-link obot, cuentl in the pototpe stage, which is designed to climb along vetical walls. It attaches to the wall suface using electomagnets at the end of each link. These magnets ae configued to allow the device to pivot about the attachment point. The electomagnets can be simultaneousl o individuall activated as needed. The two links ae connected b an actuated otating joint. With pope contol, the moto at this joint can leveage the -link swinging dnamics to ceate desied motion. A combination of swinging the links and changing the activated electomagnet will allow the obot to move about the wall. The MonkeBot is modeled as two massive links connected b a cental joint. The links of length, each have mass m, m and otational inetia I, I. The cente of gavit CG is located aiall a distance, along the link. Figue : Diagam of -link MonkeBot model with paametes. If we contain the device to plana movement, the sstem can be full epessed in tems of fou genealized coodinates, contained in vecto.

3 , is the coodinate location of the top link. is the angle of the top link elative to vetical, and is the elative angle between the links. The MonkeBot is capable of engage in thee geneal tpes of motion phases that ae defined b the configuation of active magnets. Fee flight - no magnets ae active, sstem has fou degees of feedom.. Swinging one magnet is activated, sstem has two degees of feedom. 3. Fied both magnets activated so that the obot cannot move. Dnamics duing this state ae tiviall zeo, and ae not analzed. Tansitioning to the swinging o fied phase involves activating the magnet on one o both links. The esultant clamping of the magnet to the wall suface is a collision that must be accounted fo in dnamics and motion planning.

4 3. MonkeBot Dnamics 3. Motion Dnamics The simplest wa to obtain the mechanical dnamics of ou sstem is via agangian dnamics. The agangian of the sstem is defined as the diffeence between the kinetic K and potential V enegies of the sstem. V K,, Fo ou sstem, the KE and PE tems can be epessed as: m m g V I I v m m v K v and v ae velocities of the CGs epessed in the - coodinate sstem. ikewise and ae the vetical positions of the CG in the - sstem. The ae used in the pevious euations fo simplicit and conciseness, and ae eplaced b vaiables in the coodinate sstem using the following tansfomations: cos cos cos v v Once the substitutions ae made in K and V fo the genealized coodinates, we can geneate the Eule-agange euations of motion, witten below. F is the vecto of applied etenal foces in the genealized sstem. F dt d The Eule-agange gives us a sstem of 4 diffeential euations, one fo each coodinate in. The euations can then be manipulated and gouped into the standadized fom:, g C M F * Whee M is the mass o inetia mati, C is a mati that contains centifugal and coiolis tems, and g is a vecto of gavitational foces. Fo ou -link sstem, these matices ae: i

5 m m 0 M m m c m c m c m m m m s m s 0 m s I I m m c m c m m s m s m m m m m c I m c m m c m c m s I m I whee s, c cos, c cos, etc. The etenal foce vecto, F allows us to appl fiction and moto toue to the sstem. Moto toue, if an, is inseted into the thid ow of the F vecto, coesponding to a foce applied to the elative angle between the joints. These euations descibe the -link sstem with fou degees of feedom. Numeical integation of these geneal euation * with the given matices will esult in a simulated MonkeBot in unesticted fee-flight motion. 3. Reduced Dnamics fo Swinging State While the fee-flight dnamics full descibe the unesticted sstem, we need to contstain one of the link ends in ode to take advantage of the MonkeBot s swinging popeties. In the swinging state, the obot is assumed to otate aound the fied end at,. Instead of appling constaints to the euations of 3., the sstem can be educed to onl two genealized coodinates, [ ] T. Solving this educed sstem follows the pocedue above though the algeba is much simple. The esultant matices fo the standadized fom * in this swinging state ae:

6 This swinging state is applicable to an bachiating tpe motion. The obot might swing with onl one magnet attached to the wall - a situation that is descibed b these educed euations. At the end of the swing, the obot could elease the magnet and ente a feeflight phase, which would instead be descibed b the full 4 degee of feedom sstem in section 3. Smooth tansitions between these states is citical to an accuate simulation. 3.3 Impact Dnamics To move an significant distance acoss the wall, the MonkeBot needs to tansition its holding points b changing which magnet is fied. The tansitions involve impact and coesponding loss of eneg as the magnet attaches to the wall suface. It is assumed the fiction and holding foce of the magnets is high, so we model these collisions as puel plastic. If both magnets ae simultaneousl active, then the solution to this impact is tivial and uninteesting all motion and velocities ae halted. As such, we will onl conside impacts in the case of tansition fom fee-flight to swinging dnamics o, euivalentl, an instantaneous change fom one magnet to the othe. Using the end of the top link as the attachment point, we appoimate the impact as an unknown finite impulse applied at the, position. The impulse is constained so that the fied end velocities in the post-impact state ae zeo plastic collision. ii J 0 * J is a Jacobian mati that tansfoms the genealized velocities d into a seconda coodinate sstem consisting onl of the velocities we want to become zeo: [d d] T. d and d ae identical in both sstems, so J is simpl: J 0 0 0

7 Impulse applied to a sstem causes a change in momentum mass*velocit. The change in momentum is eflected b a change in velocit, dd - d -, whee d - and d ae the pe- and post- impact velocities espectivel. λ T J M ** The paamete λ in ** epesents the unknown impulse applied to the fied end duing. Combining euations * and **, and solving to eliminate the paamete λ, gives us the solution to ou impact poblem. The post-impact velocities ae in tems of the pe-impact state and a pojection mati P: J J JM J M I P P T T * The above euation applies when the end at, becomes fied. In the altenate case whee we desie to activate the othe magent at the end of the second/bottom link, we can simpl tansfom the coodinates so that, ae located about the desied point. Appling geomet to and d, the bottom and top links ae effectivel intechanged. These tansfomations ae: cos cos cos cos π flipped flipped flipped flipped We must also ensue to swap the paametes fo the links, to ensue the flipped sstem is consistent with the old one. The impact euations * can then be coectl applied to d flipped.

8 4. Simulation 4. Code High-evel Oveview The pima objective of the code was to povide a simulation fo MonkeBot. The code, witten in Matlab, handles euations of motions fo all possible motion tpes: feeflight, swinging, and fied. It also smoothl tansitions between motion tpes, modeling impacts as necessa. Paametes fo the MonkeBot ae use modifiable. The simulation function, monkesim is called with the initial conditions and sstem paametes. It will etun esults of the simulation at each time step.00seconds. Numeical integation is done with Matlab s ode45, using the standadized euation * and the matices applicable to the motion state / magnet configuation. Solving the euations of motion fo each time step is handled b the function DiffEWappe. The contol algoithm is mostl modula and contained with contolalgoithm.m. It can be easil eplaced o modified. The attached.zip file contains a folde with the simulation code, and a sample scipt file main.m to demonstate its use. The oveall algoithm is as follows:. Setup scipt: main.m sets up the initial conditions, sstem paametes, and contolle paametes. Simulation function: monkesim is then called with all necessa paametes 3. Values fo geneating the matices M, C, and G ae pe-calculated to impove speed 4. ode45 is called to numeicall integate the euations of motion 5. Fo each time step in ode45, the euations of motion ae solved b DiffEWappe a. Reduced matices DOF used when swinging with onl one magnet attached b. Full matices 4 DOF used when neithe magnet is attached. 6. Fo each time step specified.00seconds, a monitoing function, DiffEMonito is called b ode45 a. Plots the location and state of the MonkeBot, so that use can watch simulation pogess b. Contol algoithm is called to check cuent state i. Contol algoithm eceives cuent sstem state Q ii. Can adjust moto toue o change magnet states 7. Integation continues, but is halted when magnet states ae tansitioned a. Sstem state is flipped bottom link becomes top, end point becomes new base / b. Pojection mati calculates post impact velocities fom fee-flight dnamics. 8. Repeat integation with new initial conditions fom post-impact velocities

9 4. Adjusting Contol Algoithm and Sstem Paametes Sstem paametes available fo modification include m, I,, and fo each link, as well as the gavit constant g. These paametes ae passed in vecto fom e.g. M[m m ] to the simulation function. Demonstation of these paametes is shown in main.m. The contol algoithm setup was designed to be isolated fom the est of the simulation. The function contolalgoithm is called fo each time step, and is passed the cuent time and sstem state. An changes made to the global contol vaiables motostate, magnetstate will be eflected as the simulation pogesses. The cuent code package includes a simple time-based open-loop contolle. Modification of initial_t, moto_t, swap_t, and mamotopowe will give demonstation of vaing simulation esults. See main.m and contolalgoithm.m. Additional paametes can be fine tuned in vaious pats of the pogam: o Step size cuentl.00 can be changed in monkesim.m to change the contolle esolution o numbe of epoted values. Note: This does not affect the accuac of the integation as ode45 chooses its own step size then intepolates to etun values at the desied points. o Fiction cuentl 0. DiffEWappe includes paametes to appl both constant and viscous fiction to the joints at and. o Feuenc of gaphing can be changed in DiffEMonito. This has a lage impact on simulation time. i M, C,d, G matices fo both dnamic sstems fee-flight/4 DOF and swinging/ DOF obtained fom Nelson Rosa. ii Euations fo impact dnamics obtained fom Kevin nch.