Sterowanie mechanizmów wieloczłonowych Wykład 2 - standardowe modele układów wieloczłonowych dr inż. Jakub Możaryn, mgr inż. Jan Klimaszewski

(1)

Sterowanie mechanizmów wieloczłonowych

Wykład 2 - standardowe modele układów wieloczłonowych

dr inż. Jakub Możaryn, mgr inż. Jan Klimaszewski

Instytut Automatyki i Robotyki

Warszawa, 2018

dr inż. Jakub Możaryn, mgr inż. Jan Klimaszewski Sterowanie mechanizmów wieloczłonowych

(2)

Model kinematyki - notacja DH

Rysunek:acrobot

i ai αi di θi

1 a1= l1 α1= 0 d1= 0 θ1

2 a2= l2 α2= 0 d2= 0 θ2

Tablica:Parametry DH.

(3)

Zadanie proste kinematyki

Dane: Q = [θ₁, θ₂];

Szukane: T₂⁰

T⁰₂(Q) = T⁰₁(θ₁) · T¹₂(θ₂) (1)

(4)

Zadanie proste kinematyki

Korzystając z notacji DH:

T₁⁰=







cos θ1 − sin θ1 0 l1· cos θ1

sin θ1 cos θ1 0 l1· sin θ1

0 0 1 0

0 0 0 1





 ⁽²⁾

T2¹=







cos θ2 − sin θ2 0 l2· cos θ2

sin θ2 cos θ2 0 l2· sin θ2

0 0 1 0

0 0 0 1





 ⁽³⁾

czyli

R⁰2 D2⁰

0 1

= T₂⁰= T₁⁰· T₂¹= (4)







cos(θ1+ θ2) − sin(θ1+ θ2) 0 l1· cos θ1+ l2· cos(θ1+ θ2) sin(θ1+ θ2) cos(θ1+ θ2) 0 l1· sin θ1+ l2· sin(θ1+ θ2)

0 0 1 0

0 0 0 1





 ⁽⁵⁾

(5)

Zadanie odwrotne kinematyki - ujęcie ogólne

Mając dane położenie i orientację ostatniego członu w postaci transformacji

H =R D 0 1

, H ∈ R^4×4, (6)

rozwiąż równanie (znajdź współrzędne uogólnione Q = [θ₁, θ₂]^T) T⁰₂(Q) =R⁰₂(Q) D₂⁰(Q)

0 1

= H (7)

(6)

Zadanie odwrotne kinematyki - podejście geometryczne

Rysunek:kinematyka odwrotna geometrycznie

D =dx

dy

(8)

Najpierw wyznaczamy kąt θ2:

d_x²+ d_y²= l₁²+ l₂²− 2 · l1· l2· cos(π − θ2) d_x²+ d_y²= l₁²+ l₂²+ 2 · l1· l2· cos θ2

cos θ₂=d_x²+ d_y²− l₁²− l₂² 2 · l1· l2

= L sin θ2= ±p

1 − L² θ2= arctg±√

1 − L² L

(9) Dalej wyznaczamy kąt θ₁:

θ₁= β − γ θ₁= arctgdy

d_x − arctg l2· sin θ2

l₁+ l₂· cos θ2

(10)

(7)

Zadanie proste kinematyki prędkości

Wyznaczenie prędkości V_n⁰i ω_n⁰ostatniego członu na podstawie znanych prędkości współrzędnych uogólnionych ˙Q = [ ˙θ1, ˙θ2]^T.

V_n⁰ ω⁰_n

= J(Q) · ˙Q =J_D(Q) J_R(Q)

· ˙Q (11)

gdzie: J - jakobian.

Jakobiany JD(Q) (przemieszczenia) oraz JR(Q) (obrotu) możemy wyznaczyć na podstawie znanych prędkości współrzędnych uogólnionych Q = [ ˙˙ θ₁, ˙θ₂]^T.

JD(Q) =h

∂D⁰₂

∂θ1

∂D⁰₂

∂θ2

i

(12) JR(Q) =ρ₁Z₀⁰ ρ2Z₁⁰

(13) gdzie Z₀⁰=0 0 1^T, Z₁⁰= R⁰₁0 0 1^T,ρ1= 1 (przegub

obrotowy), ρ2= 1 (przegub obrotowy).

(8)

Zadanie proste kinematyki prędkości

(d_x = l₁· cos θ₁+ l₂· cos(θ₁+ θ₂)

dy = l1· sin θ1+ l2· sin(θ1+ θ2) (14) ( ˙dx = vx = −l1· sin θ1· ˙θ1− l2· sin(θ1+ θ2)˙( ˙θ1+ ˙θ2)

d˙y = vy = l1· cos θ1· ˆθ1+ l2· cos(θ1+ θ2)˙( ˙θ1+ ˙θ2) (15)

_˙ dx

d˙y

=

vx

vy

=

−l1· sin θ1− l2· sin(θ1+ θ2) −l2· sin(θ1+ θ2) l1· cos θ1+ l2· cos(θ1+ θ2) l2· cos(θ1+ θ2)

·

_˙ θ1

θ˙2

(16) Można zauważyć, że:

JD =

−l1· sin θ1− l2· sin(θ1+ θ₂) −l2· sin(θ1+ θ₂) l₁· cos θ1+ l₂· cos(θ1+ θ₂) l₂· cos(θ1+ θ₂)

(17) wtedy

V₂⁰=

d˙_x d˙_y

=

v_x vy

= J_D·

θ˙₁ θ˙₂

(18)

(9)

Zadanie odwrotne kinematyki prędkości

Wyznaczenie prędkości współrzędnych uogólnionych ˙Q = [ ˙θ1, ˙θ2]^Tna pod- stawie znanych prędkości V i ω środka cięzkości ostatniego członu.

Q = [J(Q)]˙ ⁻¹V ω

(19)

(10)

Model dynamiki (1)

Jeśli przyjmiemy, że Q = [θ₁, θ₂]^T = [q₁, q₂]^T, energia kinetyczna:

K = K1+ K2 (20) K1=1

2I1q˙12

(21) K2= 1

2(m2l1²+ I2+ 2m2l1lc2c2) ˙q12

+1 2I2q˙22

+ (I2+ m2l1lc2c2) ˙q1q˙2 (22) Energia potencjalna:

P = −m1glc1c1− m2g (l1c1+ l2c12) (23) Stosując równania Lagrange’a

τi = d dt

δL δ ˙qi

− δL δqi

, i = 1, 2, . . . , n, L = K − P (24) otrzymujemy

(I1+ I2+ m2l₁²+ 2m2l1lc2c2) ¨q1+ (I2+ m2l1lc2c2) ¨q2− 2m2l1lc2s2q˙1q˙2

−m2l1lc2s2q˙22

+ (m1lc1+ m2l1)gs1+ m2gl2s12= 0 (25) (I2+ m2l1lc2c2) ¨q1+ I2q¨2+ m2l1lc2s2q˙12

+ m2gl2s12= τ2 (26)

(11)

Model dynamiki(2)

Po przekształceniu do postaci

τ = M(Q) ¨Q + V (Q, ˙Q) + G (Q) + F (Q, ˙Q) (27) otrzymujemy:

M(q) =I₁+ I₂+ m₂l₁²+ 2m₂l₁l_c2c₂ I₂+ m₂l₁l_c2c₂ I₂+ m₂l₁l_c2c₂ I₂

(28) V (q, ˙q) =−2m2l1lc2s2q˙2 −m2l1lc2s2q˙2

m2l1lc2s2q˙1 0

(29) G (q) =(m1lc1+ m2l1)gs1+ m2gl2s12

m2gl2s12

(30) τ = 0

τ2

(31)

(12)

Wahadło odwrócone - Opis 1/2

Wahadło odwrócone składa się z pręta swobodnie przymocowanego do wózka jeżdżącego po prowadnicy. Ruch wózkiem pozwala na realizację przemieszczenia liniowego wahadła, natomiast ruch kątowy pręta jest wy- nikiem działania sił bezwładności. Wahadło posiada punkt stabilności w dolnym położeniu pręta oraz punkt chwiejnej równowagi w pozycji górnej.

W momencie gdy układ jest utrzymywany w górnej pozycji, mówi się o wahadle odwróconym.

Z przedstawionego opisu wynika, że jest to obiekt regulacji typu SIMO (ang. single-input, multi-output) i stanowi przykad układu niedostero- wanego. Jako sygnał sterujący przyjmuje się moment silnika powodujący przemieszczenie wózka, natomiast sygnałami wyjściowymi jest położenie liniowe i kątowe wahadła.

(13)

Wahadło odwrócone - Opis 2/2

Model wahadła został wyznaczony w oparciu o schemat przedstawiony na rys.3. Początek układu współrzędnych przyjęto w początkowym położeniu wózka.

Rysunek:Schemat wahadła odwróconego

Wózek wahadła ma możliwość przemieszczania się w osi x . Pręt waha- dła zamocowany jest jednym końcem na wózku i obraca się wokół osi przechodzącej prostopadle przez wózek o kąt θ. W dalszych rozważaniach parametry związane z wózkiem zostały oznaczone indeksem_w, natomiast parametry związane z prętem indeksem_p.

(14)

Model kinematyki

Parametry DH wahadła odwróconego

i ai αi di θi

1 a1= 0 α1= 0 d1 θ1= 0 2 a2= 0 α2= π d2= 0 θ2

Macierze przekształceń

T⁰₁=





cos θ1 − cos α₁sin θ1 sin α1· sin θ₁ a1· cos θ₁ sin θ1 cos α1· cos θ₁ − sin α₁· cos θ₁ a1· sin θ₁

0 0 1 0

0 0 0 1



 (32)

T¹₂=





cos θ2 − cos α₂sin θ2 sin α2· sin θ₂ a2· cos θ₂ sin θ2 cos α2· cos θ₂ − sin α₂· cos θ₂ a2· sin θ₂

0 0 1 0

0 0 0 1



 ⁽³³⁾

(15)

Zadanie proste kinematyki

Dane: Q = [d₁, θ₂];

Szukane: T₂⁰

T⁰₂(Q) = T⁰₁(d₁) · T¹₂(θ₂) (34)

(16)

Zadanie odwrotne kinematyki

Mając dane położenie i orientację pręta wahadla w postaci transformacji H =R D

0 1

= T⁰₂(Q) = T⁰₁(d1) · T¹₂(θ2), H ∈ R^4×4, (35) rozwiąż równanie (znajdź współrzędne uogólnione d1, θ2)

T₂⁰(d1, θ2) = H (36)

(17)

Zadanie odwrotne kinematyki

Otrzymujemy układ równań skalarnych:











T11(d1, θ2) = h11, T12(d1, θ2) = h12, T13(d1, θ2) = h13, T₁₄(d₁, θ₂) = h₁₄, T₂₁(d₁, θ₂) = h₂₁, T₂₂(d₁, θ₂) = h₂₂, T₂₃(d₁, θ₂) = h₂₃, T₂₄(d₁, θ₂) = h₂₄,











T31(d1, θ2) = h31, T32(d1, θ2) = h32, T33(d1, θ2) = h33, T₃₄(d₁, θ₂) = h₃₄, T₄₁(d₁, θ₂) = h₄₁, T₄₂(d₁, θ₂) = h₄₂, T₄₃(d₁, θ₂) = h₄₃, T₄₄(d₁, θ₂) = h₄₄,

(37)

Układ może mieć wiele rozwiązań.

(18)

Zadanie proste kinematyki prędkości

Wyznaczenie prędkości V_n⁰i ω_n⁰pręta wahadła na podstawie znanych prędkości współrzędnych uogólnionych ˙Q = [ ˙d1, ˙θ2]^T.

V_n⁰ ω⁰_n

= J(Q) · ˙Q =J_D(Q) J_R(Q)

· ˙Q (38)

Jakobiany JD(Q) (przemieszczenia) oraz JR(Q) (obrotu) możemy wyznaczyć na podstawie znanych prędkości współrzędnych uogólnionych Q = [ ˙˙ d₁, ˙θ₂]^T.

JD(Q) =h

∂D⁰₂

∂d1

∂D⁰₂

∂θ2

i

(39) JR(Q) =ρ₁Z₀⁰ ρ2Z₁⁰

(40) gdzie Z₀⁰=0 0 1^T, Z₁⁰= R⁰₁0 0 1^T, ρ1= 1 (przegub

przesuwny), ρ2= 0 (przegub obrotowy).

(19)

Zadanie odwrotne kinematyki prędkości

Wyznaczenie prędkości współrzędnych uogólnionych ˙Q = [ ˙d1, ˙θ2]^T na pod- stawie znanych prędkości V i ω środka cięzkości pręta wahadła.

Q = [J(Q)]˙ ⁻¹V ω

(41)

(20)

Model dynamiki

Parametry masowe i bezwłasnościowe wahadła odwróconego i m_i x_i y_i z_i

1 m₁ x₁ y₁ z₁ 2 m₂ x₂ y₂ z₂

Tablica:Masy i położenia środków ciężkości.

i IXXi IYYi IZZi IXYi IXZi IYZi

1 IXX 1 IYY 1 IZZ 1 IXY 1 IXZ 1 IYZ 1

2 IXX 2 IYY 2 IZZ 2 IXY 2 IXZ 2 IYZ 2

Tablica:Momenty bezwładności.

(21)

Model dynamiki - jednorodne macierze inercji

Człon 1

J1=







−I_{XX 1}+ IYY 1+ IZZ 1

2 IXY 1 IXZ 1 m1x1

I_{XY 1} IXX 1− I_{YY 1}+ IZZ 1

2 I_{YZ 1} m1y₁

IXZ 1 IYZ 1

IXX 1+ IYY 1− I_{ZZ 1}

2 m1z1

m1x1 m1y₁ m1z1 m1







(42) Człon 2

J2=







−I_{XX 2}+ IYY 2+ IZZ 2

2 IXY 2 IXZ 2 m2x2

IXY 2

IXX 2− IYY 2+ IZZ 2

2 IYZ 2 m2y₂

IXZ 2 IYZ 2

IXX 2+ IYY 2− I_{ZZ 2}

2 m2z2

m2x2 m2y₂ m2z2 m2







(43)

(22)

Model dynamiki - macierze pomocnicze do liczenia pochodnych cząstkowych

Przegub 1 - przegub przesuwny

Si=







0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0







(44)

Przegub 2 - przegub obrotowy

S2=







0 −1 0 0

1 0 0 0

0 0 0 0







(45)

(23)

Model dynamiki - macierze pomocnicze do liczenia pochodnych cząstkowych

Do wyznaczania pochodnych cząstkowych w modelu dynamiki, wykorzystujemy następujące definicje

U_ij≡ δT⁰_i δqj

=

T⁰_{j −1}SjT^{j −1}_i , dla j ¬ i

0, dlaj > i (46)

U_ijk ≡ δUij

δq_k =







T⁰_{j −1}S_jT^{j −1}_k−1S_kT^k−1_i , dla j ¬ k ¬ i T⁰_k−1SkT^k−1_{j −1}SjT^{j −1}_i , dla k ¬ j ¬ i 0, dla j > i lub i < k

(47)

(24)

Model dynamiki - elmenty macierzy bezwładności

Elementy macierzy bezwładności wahadła odwróconego M(Q) =

m₁₁(Q) m₁₂(Q) m₂₁(Q) m₂₂

(48)

mik(Q) =

n

X

j =max(i ,k)

Tr (UjkJjU^T_jk), i , k = 1, 2, . . . , n (49)

gdzie: Tr (A) =Pn

i =1aii - ślad macierzy kwadratowej A ∈ R^n×n. wyznacza się korzystając z zależności:

mik(Q) =

n

X

j =max(i ,k)

Tr (UjkJjU^T_jk), i , k = 1, 2, . . . , n (50)

gdzie: Tr (A) =Pn

i =1a_ii - ślad macierzy kwadratowej A ∈ R^n×n.

(25)

Model dynamiki - elmenty macierzy bezwładności

Elementy macierzy bewładności wahadła odwróconego

m₁₁(Q) =

2

X

j =1

Tr (U_{j 1}J_jU^T_{j 1}) = Tr (U₁₁J₁U^T₁₁) + Tr (U₂₁J₂U^T₂₁) (51)

m₁₂(Q) = m₂₁(Q) = Tr (U₂₁J₂U^T₂₁) (52)

m22=

2

X

j =2

Tr (U22J2U^T₂₂) (53)

Uwaga: zgodnie w własnościami strukturalnymi modelu mechanizmu wieloczłonowego, element m22 ma wartość stałą, natomiast elementy m12(Q) = m21(Q).

(26)

Model dynamiki - elmenty wektora V (Q, ˙ Q)

Elementy wektora wyrazów zawierających składowe momentu zależne od sił odśrodkowych i Coriolisa

V(Q, ˙Q) =

v1(Q, ˙Q) v₂(Q, ˙Q)

(54)

można wyznaczyć korzystając z następujących zależności:

v_i(Q, ˙Q) = ˙q^TH_iq,˙ H_i∈ R^n×n, i = 1, 2, . . . , n (55)

hikl(Q) =

n

X

j =max(i ,k,l )

Tr (UjklJjU^T_ji), i , k, l = 1, 2, . . . , n (56)

Stąd

v₁(Q, ˙Q) = ˙q^TH₁(Q) ˙q

v2(Q, ˙Q) = ˙q^TH2(Q) ˙q (57)

(27)

Model dynamiki - elmenty wektora V (Q, ˙ Q)

Elementy macierzy H1(Q)

h₁₁₁(Q) =

2

X

j =1

Tr (U_{j 11}J_jU^T_{j 1}) = Tr (U₁₁₁J₁U^T₁₁) + Tr (U₂₁₁J₂U^T₂₁) (58)

h112(Q) = Tr (U212J2U^T₂₁) (59)

h121(Q) = Tr (U221J2U^T₂₁) (60)

h122= Tr (U222J2U^T₂₁) (61)

(28)

Model dynamiki - elmenty wektora V (Q, ˙ Q)

Elementy macierzy H2(Q)

h211(Q) = Tr (U211J2U^T₂₂ (62)

h₂₁₂(Q) = Tr (U₂₁₂J₂U^T₂₂) (63)

h₂₂₁(Q) = Tr (U₂₂₁J₂U^T₂₂) (64)

h222= Tr (U222J2U^T₂₂) (65)

(29)

Model dynamiki - elmenty wektora G (Q, ˙ Q)

Elementy wektora wyrazów zawierających składowe momentu zależne od sił grawitacji

G(Q) =

g1(Q) g₂(Q)

(66) można wyznaczyć korzystając z następujących zależności:

gi(Q) =

n

X

j =i

(−mjGr UijRj), i = 1, 2, . . . , n (67)

Stąd

g₁(Q) =

2

X

j =1

(−m_jGr U_ijR_j) = −m₁Gr U₁₁R₁− m2Gr U₁₂R₂ (68)

g₂(Q) = −m₂Gr U₂₂R₂ (69)

(30)

Model dynamiki - podsumowanie

Model dynamiki wahadła odwróconego ma postać τ =

τ1

=

f 0

=

m11(Q) m12(Q) m21(Q) m22

Q+¨

v1(Q, ˙Q) v₂(Q, ˙Q)

+

g1(Q) g2(Q)

(70) Wahadło odwrócone jest układem niedosterowanym. Tak więc

w przegubie 2 nie ma momentu napędowego: τ2= 0, na wozek działa siła napędowa: τ1= f .

(31)

Model kinematyki - parametry notacji DH

ai - odległość pomięzy osiami zi −1 i zi mierzona wzdłuż osi xi

αi - kąt pomięzy osiami zi −1i zi mierzony w płaszczyźnie normalnej do osi x_i, obrót jest prawoskrętny od z_{i −1}do z_i

d_i - odległość od o_{i −1}do przecięcia osi z_{i −1}i x_i mierzona wzdłuż osi z_{i −1}

θ - kąt pomięzy osiami x_{i −1}i x_i mierzony w płaszczyźnie normalnej do osi zi −1

(32)

Model kinematyki - notacja DH

Rysunek:noga robota RRR

i a_i α_i d_i θ_i

1 a1= 0 α1= −³₂π d1= 0 θ1

2 a2 α2= 0 d2 θ2

3 a3 α3= 0 d3 θ3

Oraz:

Q =θ1 θ2 θ3^T

(71)

(33)

Zadanie proste kinematyki - notacja DH

Korzystając z notacji DH:

T⁰₁= Rotz,θ₁Rotx ,α₁ (72) T¹₂= Rot_z,θ₂Trans_z,d₂Trans_{x ,a}₂ (73) T²₃= Rot_z,θ₃Trans_z,d₃Trans_{x ,a}₃ (74) czyli

T⁰3=

R⁰₃ D₃⁰

0 1

= T⁰1· T¹2· T²3 (75)

(34)

Zadanie odwrotne kinematyki

Mając dane położenie i orientację stopy w postaci transformacji H =R D

0 1

, H ∈ R^4×4, (76)

rozwiąż równanie (znajdź współrzędne uogólnione Q = [θ1, θ2, θ3]^T) T⁰₃(Q) =R⁰₃(Q) D₃⁰(Q)

0 1

= H (77)

(35)

Zadanie odwrotne kinematyki

Otrzymujemy układ równań skalarnych:











T11(θ1, θ2, θ3) = h11, T12(θ1, θ2, θ3) = h12, T13(θ1, θ2, θ3) = h13, T₁₄(θ₁, θ₂, θ₃) = h₁₄, T₂₁(θ₁, θ₂, θ₃) = h₂₁, T₂₂(θ₁, θ₂, θ₃) = h₂₂, T₂₃(θ₁, θ₂, θ₃) = h₂₃, T₂₄(θ₁, θ₂, θ₃) = h₂₄,











T31(θ1, θ2, θ3) = h31, T32(θ1, θ2, θ3) = h32, T33(θ1, θ2, θ3) = h33, T₃₄(θ₁, θ₂, θ₃) = h₃₄, T₄₁(θ₁, θ₂, θ₃) = h₄₁, T₄₂(θ₁, θ₂, θ₃) = h₄₂, T₄₃(θ₁, θ₂, θ₃) = h₄₃, T₄₄(θ₁, θ₂, θ₃) = h₄₄,

(78)

Układ może mieć wiele rozwiązań.

(36)

Zadanie proste kinematyki prędkości

Wyznaczenie prędkości V_n⁰i ω_n⁰stopy na podstawie znanych prędkości współrzędnych uogólnionych ˙Q = [ ˙θ1, ˙θ2, ˙θ3]^T.

V_n⁰ ω⁰_n

= J(Q) · ˙Q =J_D(Q) J_R(Q)

· ˙Q (79)

Jakobiany J_D(Q) (przemieszczenia) oraz J_R(Q) (obrotu) możemy wyznaczyć na podstawie znanych prędkości współrzędnych uogólnionych Q = [ ˙˙ θ₁, ˙θ₂, ˙θ₃]^T.

JD(Q) =h

∂D₃⁰

∂θ₁

∂D₃⁰

∂θ₂

∂D₃⁰

∂θ₃

i

(80) J_R(Q) =ρ₁Z₀⁰ ρ₂Z₁⁰ ρ₃Z₂⁰

(81) gdzie Z₀⁰=0 0 1^T, Z₁⁰= R⁰₁0 0 1^T,

Z₂⁰= R⁰₂0 0 1^T,ρ₁= 1 (przegub obrotowy), ρ₂= 1 (przegub obrotowy), ρ₃= 1 (przegub obrotowy).

(37)

Zadanie odwrotne kinematyki prędkości

Wyznaczenie prędkości współrzędnych uogólnionych ˙Q = [ ˙θ1, ˙θ2, ˙θ3]^T na podstawie znanych prędkości V i ω środka cięzkości stopy.

Q = [J(Q)]˙ ⁻¹V ω

(82)

(38)

Model dynamiki

Parametry masowe i bezwłasnościowe nogi robota i mi xi yi zi

1 m1 x1 y1 z1

2 m2 x2 y2 z2

2 m3 x3 y3 z3

Tablica:Masy i położenia środków ciężkości.

i I_XXi I_YYi I_ZZi I_XYi I_XZi I_YZi 1 I_{XX 1} I_{YY 1} I_{ZZ 1} I_{XY 1} I_{XZ 1} I_{YZ 1} 2 I_{XX 2} I_{YY 2} I_{ZZ 2} I_{XY 2} I_{XZ 2} I_{YZ 2} 2 I_{XX 3} I_{YY 3} I_{ZZ 3} I_{XY 3} I_{XZ 3} I_{YZ 3c}

Tablica:Momenty bezwładności.

(39)

Model dynamiki - jednorodne macierze inercji

Człon 1

J₁=







−IXX 1+ I_{YY 1}+ I_{ZZ 1}

2 I_{XY 1} I_{XZ 1} m₁x₁

I_{XY 1} I_{XX 1}− IYY 1+ I_{ZZ 1}

2 I_{YZ 1} m1y₁

IXZ 1 IYZ 1

I_{XX 1}+ I_{YY 1}− I_{ZZ 1}

2 m1z1

m₁x₁ m₁y₁ m₁z₁ m₁







(83)

Człon 2

J₂=







−IXX 2+ IYY 2+ IZZ 2

2 I_{XY 2} I_{XZ 2} m₂x₂

I_{XY 2} I_{XX 2}− IYY 2+ I_{ZZ 2}

2 I_{YZ 2} m₂y₂

IXZ 2 IYZ 2

I_{XX 2}+ I_{YY 2}− IZZ 2

2 m2z2

m₂x₂ m₂y₂ m₂z₂ m₂







(84)

Człon 3

J₃=







−IXX 3+ IYY 3+ IZZ 3

2 I_{XY 3} I_{XZ 3} m₃x₃

I_{XY 3} I_{XX 3}− IYY 3+ I_{ZZ 3}

2 I_{YZ 3} m₃y₃

I_{XZ 3} I_{YZ 3} I_{XX 3}+ I_{YY 3}− IZZ 3

2 m3z3

m₃x₃ m₃y₃ m₃z₃ m₃







(85)

(40)

Model dynamiki - macierze pomocnicze do liczenia pochodnych cząstkowych

S1=







0 −1 0 0

1 0 0 0

0 0 0 0





 ⁽⁸⁶⁾

S2=







0 −1 0 0

1 0 0 0

0 0 0 0





 ⁽⁸⁷⁾

S3=







0 −1 0 0

1 0 0 0

0 0 0 0





 ⁽⁸⁸⁾

(41)

Model dynamiki - macierze pomocnicze do liczenia pochodnych cząstkowych

Do wyznaczania pochodnych cząstkowych w modelu dynamiki, wykorzystujemy następujące definicje

Uij≡ δT⁰_i δqj

=

T⁰_{j −1}SjT^{j −1}_i , dla j ¬ i

0, dlaj > i (89)

U_ijk ≡ δU_ij δqk

=







T⁰_{j −1}S_jT^{j −1}_k−1S_kT^k−1_i , dla j ¬ k ¬ i T⁰_k−1SkT^k−1_{j −1}SjT^{j −1}_i , dla k ¬ j ¬ i 0, dla j > i lub i < k

(90)

Dlej wyznaczenie modelu analogicznie do modelu Acrobot.