5783291677

20 1. Wolfram Mathematica

massn — masa n-tego przegubu,

center of mass — współrzędne środka masy (dla każdego przegubu), wyrażone w lokalnym układzie współrzędnych, związanym z końcem danego przegubu, inertia matrix — sześć nieredundantnych składowych macierzy inercji przegubu, także w lokalnym układzie współrzędnych. Należy przyjąć następującą, symetryczną postać macierzy inercji:

(1.3)

j-3 *5 *6.

Następnie należy wywołać funkcję ELDynamics, w ramach której wyznaczone zostają macierze do równań dynamiki:

MU — macierz sił bezwładności,

CM — macierz sił Coriolisa i odśrodkowych,

G — wektor sił grawitacji.

Macierze te pozwalają zdefiniować dynamikę manipulatora postaci

(1.4)

MUq + CMq + G = u.

W opisany wyżej sposób dla dwuwahadła otrzymano następujące macierze:

MU =

\ (l\rri2 + if(mi + 4ra₂) + 4hkm,2 cos(ę₂)) \km₂{k + 2li cos(ę₂))

\km₂(k + 2Zi cos(ę₂))

(1.5)

( -\lil₂m₂ sin(ę₂)<?₂(t) -\hkm2 sin(<?₂) (qi(t) + q₂(t)) V lhl2m₂sm(q₂)qi(t)    0

G _ (    + 2m₂) cos(?i) + l₂m₂ cos(gi + q₂))

\    \gkm₂ cos(ęj + q₂)

W zależności od przyjętego modelu dynamiki, możliwa jest modyfikacja wyliczonych macierzy, np. dodanie do elementów na przekątnej macierzy MU momentów bezwładności silników /„.

Kolejnym etapem jest skonstruowanie równań różniczkowych dynamiki, które można numerycznie następnie rozwiązać za pomocą funkcji NDSolve. W celu poprawnego zinterpretowania równań przez tę funkcję, konieczna jest ich pewna modyfikacja, przedstawiona na wydruku poniżej. Następnie, po otrzymaniu rozwiązania równań dynamiki, możliwa jest ich prezentacja w formie wykresu od czasu, wykresu parametrycznego bądź animacji. Kod umożliwiający wykonanie opisanych powyżej operacji przedstawiony jest na wydruku 1.5.

Wydruk 1.5. Rozwiązanie równań dynamiki manipulatora sztywnego oraz wizualizacja rozwiązania

q[t_] :={ql[t] , q2 [t] > ; u[t]={ul [t] ,u2[t]>;

11=3;12=2;ml=10;m2=5;11=0.5;12=0.25;g=9.81;

MU [ [1 ,1]] =MU [[1 , 1] ] +11 ;MU [ [2,2] ] =MU [ [2,2] ] +12 ;

MU=MU/.{ql->ql [t] ,q2->q2[t]>;