Budapesti M szaki És Gazdaságtudományi Egyetem Gépészmérnöki Kar M szaki Mechanikai Tanszék Denavit-Hartenberg konvenció alkalmazása térbeli 3DoF nyílt kinematikai láncú hengerkoordinátás és gömbi koordinátás robotra Berényi Dániel, Lakatos Andor, Samu Bence István, Zelei Ambrus Utolsó módosítás dátuma: 218. október 9. /MSC/Mechanisms-Mechanizmusok/Calc/Denavit-Hartenberg/D-H_Khalil-Dombre-22_v2_Cylindric3DoFExample.nb /MSC/Mechanisms-Mechanizmusok/Calc/Denavit-Hartenberg/D-H_Khalil-Dombre-22_v2_3DoFExample.nb 1
Tartalomjegyzék 1. Hengerkoordinátás (PRP) robot 3 1.1. Célkit zés................................................ 3 1.2. Denavit-Hartenberg paraméterek, homogén transzformációk és direkt kinematika........ 3 1.3. Inverz kinematikai számítások..................................... 4 1.3.1. Pozíció szinten......................................... 4 1.3.2. Sebesség szinten........................................ 5 1.4. Az általános tömegmátrix számítása................................. 5 1.4.1. A súlyponti pozíciókra vonatkozó Jakobi mátrixok..................... 5 1.4.2. A szögsebességek Jakobi mátrixai............................... 6 1.4.3. Általános tömegmátrix..................................... 7 2. Gömbi koordinátás (RRP) robot 8 2.1. Célkit zés................................................ 8 2.2. Denavit-Hartenberg paraméterek, homogén transzformációk és direkt kinematika........ 8 2.3. Inverz kinematikai számítások..................................... 9 2.3.1. Pozíció szinten......................................... 1 2.3.2. Sebesség szinten........................................ 1 2.4. A forgatás különböz reprezentációinak szemléltetése........................ 11 2.4.1. A forgatási mátrix közvetlen felírása a bázisvektorok alapján............... 12 2.4.2. Tengely-szög reprezentáció................................... 12 2.4.3. Az orientáció reprezentációja Euler-szögekkel (zxz eset).................. 12 2.4.4. Egységkvaterniók........................................ 13 2.5. Az általános tömegmátrix számítása................................. 13 2.5.1. A súlyponti pozíciókra vonatkozó Jakobi mátrixok..................... 13 2.5.2. A szögsebességek Jakobi mátrixai............................... 15 2.5.3. Általános tömegmátrix..................................... 16 2
1. Hengerkoordinátás (PRP) robot 1.1. Célkit zés Egy három szabadsági fokú (3 DoF) hengerkoordinátás robot példáján mutatjuk be a Denavit-Hartenberg módszer alkalmazását és néhány forgatási reprezentációt. 1.2. Denavit-Hartenberg paraméterek, homogén transzformációk és direkt kinematika Az alábbiakban áttekintjük a robot szabadsági fokait az 1. ábra alapján: q 3 koordináta: eltolás d 3 távolsággal z 3 mentén (x 3 távolsága x 2 -t l mérve), q 2 koordináta: forgatás ϑ 2 szöggel z 2 körül (x 2 szöge x 1 -t l mérve), q 1 koordináta: eltolás d 1 távolsággal z 1 mentén (x 1 távolsága x -t l mérve). 1. ábra. Balra: a robot szerkezeti felépítésének vázlata; jobbra: a szegmensekhez kötött koordináta-rendszerek (minden koordináta-rendszer jobbsodrású) 1. táblázat. A Denavit-Hartenberg paraméterek táblázata. i α i a i ϑ i d i 1. q 1 (t) 2. q 2 (t) 3. 9 q 3 (t) A homogén transzformációs mátrix általános alakját az (1) adja, amelybe a D-H táblázat értékeit behelyettesítve megkapjuk a T 1, a 1 T 2 és a 2 T 3 transzformációs mátrixokat, amelyek a szomszédos koordináta-rendszerek közt adnak átjárást: i 1 T i = T 1 = c ϑi s ϑi a i c αi s ϑi c αi c ϑi s αi d i s αi s αi s ϑi s αi c ϑi c αi d i c αi 1 q 1, 1 T 2 =, (1) c 2 s 2 s 2 c 2 1 3, 2 T 3 = 1 q 3. (2)
Megjegyzés: a rövidebb írásmód érdekében bevezetjük a s αi := sin (α i ), c αi := cos (α i ), s i := sin (q i ), c i := cos (q i ), (3) El állíthatóak azok az T i transzformációs mátrixok, amelyek az egyes i-edik lokális koordináta-rendszerekb l a globálisba képeznek: T 1 = T 1 = 1 q 1. (4) T 2 = T 1 1 T 2 = T 3 = T 1 1 T 2 2 T 3 = c 2 s 2 s 2 c 2 1 q 1 c 2 s 2 q 3 s 2 s 2 c 2 q 3 c 2 1 q 1. (5). (6) A T 3 transzformációval felírható a végberendezés r TCP pozícióvektora a globális rendszerben q 3 s 2 r TCP = T 3 3 r TCP = T 3 = q 3 c 2 q 1, (7) 1 1 ahol 3 r TCP a végberendezés lokális koordináta-rendszerben megadott helyvektora. Ezzel készen áll a direkt kinematikai számítás a végberendezésre. A kapott eredmény egyezik a szemlélettel. 1.3. Inverz kinematikai számítások A végberendezés r d TCP (t) pozíciója a globális koordináta-rendszerben el van írva (el írt pálya). A q i csuklóváltozókra 3 db nemlineáris algebrai egyenlet adódik a (7) alapján: 1.3.1. Pozíció szinten q 3 sin q 2 = x d TCP(t), (8) q 3 cos q 2 = y d TCP(t), (9) q 1 = z d TCP(t). (1) A (8)-(1) nemlineáris algebrai egyenletrendszer most zárt alakban megoldható. Az els két egyenlet hányadosából: amellyel kifejezhet ek az általános koordináták: tan q 2 = xd TCP ytcp d, (11) q 1 = z d TCP, (12) q 2 = atan2(x d TCP, ytcp), d (13) q 3 = (x d TCP )2 + (ytcp d )2. (14) 4
Bonyolultabb esetben Newton-Raphson iterációval oldhatóak meg az egyenletek, de a gyakorlatban tipikusabb a következ alfejezetben bemutatott sebesség szint inverz kinematikai megoldás. 1.3.2. Sebesség szinten Sebesség szinten lineáris egyenletrendszert kapunk a q i csuklósebességekre. Általánosan megfogalmazva a (8)- (1) egyenletrendszert: r TCP (q) = r d TCP(t), (15) amit id szerint deriválva kapjuk: r TCP (q) q q = ṙ d TCP(t), azaz J TCP q = v d TCP, amivel q = J 1 TCP v d TCP. (16) A végberendezés pozíciójára vonatkozó Jakobi mátrix a példánk esetén: J TCP = q r 3 c 2 s 2 TCP = q 3 s 2 c 2 (17) q Most egyszer eset áll fenn, mert J TCP négyzetes (3x3) és det( J TCP ) = q 3, ami csak q 3 szinguláris. = esetben 1.4. Az általános tömegmátrix számítása 1.4.1. A súlyponti pozíciókra vonatkozó Jakobi mátrixok A tömegmátrix el állításához szükséges a súlyponti pozíciókra vonatkozó Jakobi mátrixok felírása. 2. ábra. Az egyes tagok súlypontjainak pozíciói Megadjuk a pozícióvektorok a lokális koordináta-rendszerekben, majd a (4)-(6) transzformációs mátrixok segítségével felírjuk a globális rendszerben (h 1, h 2 és h 3 geometriai paraméterekkel): 5
1 r C1 = 2 r C2 = 3 r C3 = h 1 h 2 h 3, (18), (19) (2) r C1 = T 1 1 r C1 = r C2 = T 2 2 r C2 = r C3 = T 3 3 r C3 = q 1 h 1 h 2 s 2 h 2 c 2 q 1 (q 3 h 3 )s 2 (q 3 h 3 )c 2 q 1, (21), (22) (23) A súlyponti pozíciók ismeretében felírhatók a súlyponti pozíciókra vonatkozó Jakobi mátrixok. Azok segítségével pedig könnyen számíthatóak a súlyponti sebességek: J v1 = r C1 q = J v2 = r C2 q = J v3 = r C3 q = h 2 c 2 h 2 s 2, (24) (q 3 h 3 )c 2 s 2 (q 3 h 3 )s 2 c 2, (25), (26) v C1 = J v1 q, (27) v C2 = J v2 q, (28) v C3 = J v3 q. (29) 1.4.2. A szögsebességek Jakobi mátrixai A szögsebességek felírásához szükségünk van a forgatási mátrixokra. Ezeket a T i transzrmációs mátrixokból vehetünk ki, mivel ezek felépítése mindig [ R ] T i = i p i. (3) 1 A forgatási mátrixok és id szerinti deriváltjaik: R 1 = R 2 = R 3 = 1 1 c 2 s 2 s 2 c 2 1 c 2 s 2 s 2 c 2 1, (31), (32), (33) Ṙ 1 = Ṙ 2 = Ṙ 3 = s 2 c 2 c 2 c 2 s 2 c 2 c 2 s 2, (34) q 2, (35) q 2. (36) A szögsebesség tenzorokat a forgatási mátrixból és annak deriváltjából számítjuk. A forgatási tenzor elemeib l pedig megalkothatók a szögsebesség vektorok, végül a koordináták szerinti deriválással a szögsebességekre vonatkozó Jakobi mátrixok: [ω ] i = ( Ṙ ) T i R i, [ω ] i = ω z ω y ω z ω x ω y ω x 6, J ωi = ω i q. (37)
[ω ] 1 = [ω ] 2 = [ω ] 3 = q 2 q 2 q 2 q 2, (38), (39), (4) ω 1 = ω 2 = ω 3 = q 2 q 2, (41), (42), (43) J ω1 = J ω2 = J ω3 = 1 1, (44), (45). (46) Ezek után a szögsebesség már a koordináták lineáris függvényeként számítható: ω i = J ωi q. (47) Vegyük észre, hogy ω 2 = ω 3, hiszen a 2. és 3. tag között prizmatikus megvezetés van, ami nem tesz lehet vé relatív szögelfordulást. A szögsebességek Jakobi mátrixa az ε i csuklótípust jelöl paraméter (ε i = prizmatikus csuklóra, ε i = 1 rotációs csuklóra), és a z i globális koordináta-rendszerben megadott bázisvektorok segítségével írható fel algoritmikusan: J ωi = [ ε 1 z 1 ; ε k z k ;... ε i z i ; ;... ]. (48) 1.4.3. Általános tömegmátrix A T = 1/2 q T H q kinetikus energia segítségével levezettük a tömegmátrix számításának módját: H = 3 m i J v i J vi + 3 J ω i R i i Θ Ci R i J ωi = (49) i=1 i=1 = H v + H ω = (5) = H v1 + H v2 + H v3 + H ω1 + H ω2 + H ω3. (51) A sebességekre és szögsebességekre vonatkozó része a tömegmátrixnak: m 1 + m 2 + m 3 H v = h 2 2 m 2 + (h 3 q 3 ) 2 m 3, (52) m 3 H ω = Θ 2z + Θ 3y, (53) amihez felhasználtuk, hogy a súlypontra számított tehetetlenségi nyomatéki mátrix a lokális koordináta-rendszerekben: Θ ix i Θ Ci = Θ iy, (54) Θ iz 7
2. Gömbi koordinátás (RRP) robot 2.1. Célkit zés Egy három szabadsági fokú (3 DoF) gömbi koordinátás robot példáján mutatjuk be a Denavit-Hartenberg módszer alkalmazását. 2.2. Denavit-Hartenberg paraméterek, homogén transzformációk és direkt kinematika Az alábbiakban áttekintjük a robot szabadsági fokait az 3. ábra alapján: q 3 koordináta: eltolás d 3 távolsággal z 3 mentén (x 3 távolsága x 2 -t l mérve), q 2 koordináta: forgatás ϑ 2 szöggel z 2 körül (x 2 szöge x 1 -t l mérve), q 1 koordináta: forgatás ϑ 1 szöggel z 1 körül (x 1 szöge x -t l mérve). 3. ábra. Balra fent: a robot szerkezeti felépítésének vázlata alaphelyzetben és > csuklókoordinátákkal; jobbra fent: a szegmensekhez kötött koordináta-rendszerek (minden koordináta-rendszer jobbsodrású); alul: oldalnézet 2. táblázat. A Denavit-Hartenberg paraméterek táblázata. i α i a i ϑ i d i 1. q 1 (t) d 1 2. 9 q 2 (t) 3. 9 q 3 (t) A homogén transzformációs mátrix általános alakját az (55) adja, amelybe a D-H táblázat értékeit behelyettesítve megkapjuk a T 1, a 1 T 2 és a 2 T 3 transzformációs mátrixokat, amelyek a szomszédos koordináta- 8
rendszerek közt adnak átjárást: i 1 T i = T 1 = c ϑi s ϑi a i c αi s ϑi c αi c ϑi s αi d i s αi s αi s ϑi s αi c ϑi c αi d i c αi c 1 s s 1 c 1 d 1, 1 T 2 =, (55) c 2 s 2 1 s 2 c 2 Megjegyzés: a rövidebb írásmód érdekében bevezetjük a, 2 T 3 = 1 q 3. (56) s αi := sin (α i ), c αi := cos (α i ), s i := sin (q i ), c i := cos (q i ), (57) El állíthatóak azok az T i transzformációs mátrixok, amelyek az egyes i-edik lokális koordináta-rendszerekb l a globálisba képeznek: T 1 = T 1 = T 2 = T 1 1 T 2 = c 1 s s 1 c 1 d 1 T 3 = T 1 1 T 2 2 T 3 = c 1 c 2 c 1 s 2 s 1 s 1 c 2 s 1 s 2 c 1 s 2 c 2 d 1. (58) c 1 c 2 s 1 c 1 s 2 q 3 c 1 s 2 s 1 c 2 c 1 s 1 s 2 q 3 s 1 s 2 s 2 c 2 d 1 + q 3 c 2. (59). (6) A T 3 transzformációval felírható a végberendezés r TCP pozícióvektora a globális rendszerben (a + q 3 )c 1 s 2 r TCP = T 3 3 r TCP = T 3 a = (a + q 3 )s 1 s 2 d 1 + (a + q 3 )c 2, (61) 1 1 ahol 3 r TCP a végberendezés lokális koordináta-rendszerben megadott helyvektora a =.2[m] geometriai paraméterrel. Ezzel készen áll a direkt kinematikai számítás a végberendezésre. A kapott eredmény egyezik a szemlélettel. 2.3. Inverz kinematikai számítások A végberendezés r d TCP (t) pozíciója a globális koordináta-rendszerben el van írva (el írt pálya). A q i csuklóváltozókra 3 db nemlineáris algebrai egyenlet adódik a (61) alapján: (a + q 3 )c 1 s 2 = x d TCP(t) (62) (a + q 3 )s 1 s 2 = y d TCP(t) (63) d 1 + (a + q 3 )c 2 = z d TCP(t) (64) 9
4. ábra. Balra: TCP (szerszámközéppont) pozíció a szerkezeti ábrán (koordináta rendszerek a középs ábrán); középen: a koordináta-rendszerek a 2.3.1. fejezetben megadott r d TCP (t) = [, 1,.5] végpont pozícióhoz tartozó speciális helyzetben; jobbra: inverz kinematika d 1 = esetén 2.3.1. Pozíció szinten A (62)-(64) nemlineáris algebrai egyenletrendszer zárt alakú megoldása a jelen példában létezik. Speciális helyzetben (a szemléltetés céljából), amikor az el írt pozíció a 4. középs ábrának megfelel en r d TCP(t) = 1.5 [m], akkor a csuklóváltozókra a megoldás: q 1 = 9, q 2 = 9, q 3 =.8 [m]. Általános esetre, paraméteresen is megoldható az inverz kinematikai feladat: ( ) q 1 = cos 1 x = atan2(y, x) π, (66) x 2 + y 2 (65) ( ) q 2 = cos 1 z d 1, x 2 + y 2 + (z d 1 ) 2 (67) q 3 = x 2 + y 2 + (z d 1 ) 2 a (68) az x := x d TCP, y := y d TCP és z := z d TCP rövidítésekkel. A d 1 = egyszer sítéssel a 4. ábra szemlélteti az inverz kinematikai számítást. Bonyolult esetben Newton-Raphson iterációval érdemes megoldani az inverz kinematikai egyenleteket, de a gyakorlatban tipikusabb a következ alfejezetben bemutatott sebesség szint inverz kinematikai megoldás. 2.3.2. Sebesség szinten Sebesség szinten lineáris egyenletrendszert kapunk a q i csuklósebességekre. Általánosan megfogalmazva a (62)- (64) egyenletrendszert: r TCP (q) = r d TCP(t), (69) amit id szerint deriválva kapjuk: r TCP (q) q q = ṙ d TCP(t), azaz J TCP q = v d TCP, amivel q = J 1 TCP v d TCP. (7) 1
A végberendezés pozíciójára vonatkozó Jakobi mátrix a példánk esetén: J TCP = (a + q r 3 )s 1 s 2 (a + q 3 )c 1 c 2 c 1 s 2 TCP = (a + q 3 )c 1 s 2 (a + q 3 )s 1 s 2 s 1 s 2 q (a + q 3 )s 2 c 2. (71) Most egyszer eset áll fenn, mert J TCP négyzetes (3x3), tehát az esetek többségében invertálható. Szinguláris helyzetben azonban nem invertálható. A szinguláris helyzetet az adja meg, amikor a Jakobi mátrix determinánsa nulla, azaz det( J TCP ) = (a + q 3 ) 2 s 2 =, (72) ami csak q 3 = a esetben fordul el (ami zikailag nem lehetséges) vagy q 2 = szöghelyzetben. A szingularitás szemléletes magyarázata q 2 = esetben az, hogy ebben a teljesen kinyújtott helyzetben a q 1 szöget hiába változtatjuk, az nem idézi el a szerszámközéppont pozícióváltozását (lásd: 5. ábra). 5. ábra. Szinguláris konguráció q 2 = esetén Megjegyzés: ha az 3 r TCP lokális helyvektor nem csak z komponenseket tartalmaz, akkor, más lesz a szinguláris konguráció. 2.4. A forgatás különböz reprezentációinak szemléltetése A forgatás reprezentációit a 6. ábrán látható speciális kongurációban vizsgáljuk (lásd: 2.3.1. fejezetben megadott konguráció). 6. ábra. Balra: a robot szerkezeti felépítésének vázlata a 2.3.1. fejezetben megadott kongurációban; jobbra: a koordináta rendszerek és a szerszámközéppont helyvektora 11
2.4.1. A forgatási mátrix közvetlen felírása a bázisvektorok alapján A 3-as koordináta-rendszerb l a -ás koordináta-rendszerbe forgató mátrixot kapjuk, ha a 3-as krdsz bázisait megadjuk a -ás krdsz-ben, és egy 3 3-as mátrixba rendezzük: x 3 = 1, x 3 = 2.4.2. Tengely-szög reprezentáció 1, x 3 = 1, R 3 = 1 1. (73) A Rodrigues-formula a forgatási tengelyt megadó egységvektorból és a forgatási szögb l megadja az R 3 ( n, ϑ) forgatási mátrixot. A Rodrigues-formula megfordításával a forgatási mátrixból kiszámítható a forgatási szög ( Tr( ϑ = cos 1 ) ( ) R 3 ) 1 1 = cos 1 = 12, (74) 2 2 majd a ϑ forgatási szög ismeretében a forgatás tengelyéhez tartozó keresztszorzat mátrix: R 3 T R 3 [n ] = = 1 1 1 1 1 = 1 1 1 1 1. (75) 2 sin ϑ 2 1 1 3 1 1 3 2 Ebb l annak ismeretében, hogy [n ] = n z n y n z n x n y n x, a forgatást megadó egységvektor: n = 1/ 3 1/ 3 1/ 3 (76) formában adódik, ahogy ezt a 7. ábra is mutatja. A q 1 és q 2 koordinátákkal való 9 -os forgatás kompozíciója egy 12 -os forgatás. 7. ábra. A q 1 és q 2 menti 9 -os forgatások kompozíciója. 2.4.3. Az orientáció reprezentációja Euler-szögekkel (zxz eset) A zxz forgatási sorrend esetén az Euler-paraméterekkel el állított forgatási mátrix a következ R(α, β, γ) = c α c γ s α c β s γ c α s γ s α c β c γ s α s β s α c γ + c α c β s γ s α s γ + c α c β c γ c α s β s β s γ s β c γ c β, (77) 12
amely az egyes forgatások R(α, β, γ) = R α R β R γ kompozíciójaként áll el (els forgatás z körül: R α, második forgatás x körül: R β, harmadik forgatás z körül: R γ ): R α (α) = c α s α s α c α 1, R β (β) = c β s β s β c β, R γ (γ) = c γ s γ s γ c γ 1. (78) Az orientációt reprezentáló α, β és γ szögek az (77) forgatási mátrix bekeretezett elemeib l számíthatók: β = atan2( R 2 3 (1, 3) + R 2 3 (2, 3), R 3 (3, 3)) = q 2 = 9, (79) α = atan2( R 3 (1, 3)/s β, R 3 (2, 3)/s β ) =, (8) γ = atan2( R 3 (3, 1)/s β, R 3 (3, 2)/s β ) = 9. (81) 8. ábra. Balra: a robot szegmenseihez kötött lokális koordináta-rendszerek; jobbra: az egyes Euler-forgatások koordináta-rendszerei Megjegyzés: létezik olyan forgatási sorrend választás, amikor az Euler-szögek a csuklószögekkel egyeznek meg: mindig z körül kell ehhez forgatnunk. 2.4.4. Egységkvaterniók Alkalmazzuk a korábban levezetett formulákat: e = Tr( R 3 ) + 1 = 1 4 2 2 R 3 (1, 1) + 1 Tr( R 3 ) (82) e 1 = 4 2 R 3 (2, 2) + 1 Tr( R 3 ) = 1 2 (83) e 2 = 4 2 R 3 (3, 3) + 1 Tr( R 3 ) = 1 2 (84) e 3 = 4 = 1 2 (85) (86) 2.5. Az általános tömegmátrix számítása 2.5.1. A súlyponti pozíciókra vonatkozó Jakobi mátrixok A tömegmátrix el állításához szükséges a súlyponti pozíciókra vonatkozó Jakobi mátrixok felírása. 13
9. ábra. Az egyes tagok súlypontjainak pozíciói (a 3. és a 4. ábráknak megfelel kongurációkban szemléltetve) Megadjuk a pozícióvektorok a lokális koordináta-rendszerekben, majd a (58)-(6) transzformációs mátrixok segítségével felírjuk a globális rendszerben (d 1, p 2 és p 3 geometriai paraméterekkel): 1 r C1 = 2 r C2 = 3 r C3 = d 1 /2 p 2 p 3, (87), (88), (89) r C1 = T 1 1 r C1 = r C2 = T 2 2 r C2 = r C3 = T 3 3 r C3 = d 1 /2 p 2 c 1 s 2 p 2 s 1 s 2 d 1 + p 2 c 2, (9) (q 3 + p 3 )c 1 s 2 (q 3 + p 3 )s 1 s 2 d 1 + (q 3 + p 3 )c 2, (91). (92) A súlyponti pozíciók ismeretében felírhatók a súlyponti pozíciókra vonatkozó Jakobi mátrixok. Azok segítségével pedig könnyen számíthatóak a súlyponti sebességek: J v1 = r C1 q = J v2 = r C2 q = J v3 = r C3 q =, (93) p 2 s 1 s 2 p 2 c 1 c 2 p 2 c 1 s 2 p 2 s 1 c 2 p 2 s 2, (94) (q 3 + p 3 )s 1 s 2 (q 3 + p 3 )c 1 c 2 c 1 s 2 (q 3 + p 3 )c 1 s 2 (q 3 + p 3 )s 1 c 2 s 1 s 2 (q 3 + p 3 )s 2 c 2, (95) v C1 = J v1 q, (96) v C2 = J v2 q, (97) v C3 = J v3 q. (98) 14
2.5.2. A szögsebességek Jakobi mátrixai A szögsebességek felírásához szükségünk van a forgatási mátrixokra. Ezeket a T i transzrmációs mátrixokból vehetünk ki, mivel ezek felépítése mindig T i = [ R i p i 1 ]. (99) A forgatási mátrixok és id szerinti deriváltjaik: R 1 = R 2 = R 3 = c 1 s 1 s 1 c 1 1 c 1 c 2 c 1 s 2 s 1 s 1 c 2 s 1 s 2 c 1 s 2 c 2 c 1 c 2 s 1 c 1 s 2 s 1 c 2 c 1 s 1 s 2 s 2 c 2. (1).(11).(12) Ṙ 1 = Ṙ 2 = Ṙ 3 = q 1 s 1 q 1 c 1 q 1 c 1 q 1 s 1, (13) q 1 s 1 c 2 q 2 c 1 s 2 q 1 s 1 s 2 q 2 c 1 c 2 c 1 q 1 q 1 c 1 c 2 q 2 s 1 s 2 q 1 c 1 s 2 q 2 s 1 c 2 s 1 q 1 q 2 c 2 q 2 s 2 q 1 s 1 c 2 q 2 c 1 s 2 c 1 q 1 q 1 s 1 s 2 q 2 c 1 c 2 q 1 c 1 c 2 q 2 s 1 s 2 s 1 q 1 q 1 c 1 s 2 q 2 s 1 c 2 q 2 c 2 q 2 s 2,(14).(15) A szögsebesség tenzorokat a forgatási mátrixból és annak deriváltjából számítjuk. A forgatási tenzor elemeib l pedig megalkothatók a szögsebesség vektorok, végül a koordináták szerinti deriválással a szögsebességekre vonatkozó Jakobi mátrixok: [ω ] i = ( Ṙ ) T i R i, [ω ] i = ω z ω y ω z ω x ω y ω x, J ωi = ω i q. (16) [ω ] 1 = [ω ] 2 = [ω ] 3 = q 1 q q 1 q 2 c 1 q 1 q 2 s 1 q 2 c 1 q 2 s 1 q 1 q 2 c 1 q 1 q 2 s 1 q 2 c 1 q 2 s 1, (17),(18),(19) ω 1 = ω 2 = ω 3 = q 1 q 2 s 1 q 2 c 1 q 1 q 2 s 1 q 2 c 1 q 1, (11),(111),(112) J ω1 = J ω2 = J ω3 = s 1 c 1 s 1 c 1, (113),(114).(115) Ezek után a szögsebesség már a koordináták lineáris függvényeként számítható: ω i = J ωi q. (116) Vegyük észre, hogy ω 2 = ω 3, hiszen a 2. és 3. tag között prizmatikus megvezetés van, ami nem tesz lehet vé relatív szögelfordulást. A szögsebességek Jakobi mátrixa az ε i csuklótípust jelöl paraméter (ε i = prizmatikus csuklóra, ε i = 1 rotációs csuklóra), és a z i globális koordináta-rendszerben megadott bázisvektorok segítségével írható fel algoritmikusan: J ωi = [ ε 1 z 1 ; ε k z k ;... ε i z i ; ;... ]. (117) 15
2.5.3. Általános tömegmátrix A T = 1/2 q T H q kinetikus energia segítségével levezettük a tömegmátrix számításának módját: H = 3 m i J v i J vi + 3 J ω i R i i Θ Ci R i J ωi = (118) i=1 i=1 = H v + H ω = (119) = H v1 + H v2 + H v3 + H ω1 + H ω2 + H ω3. (12) A sebességekre és szögsebességekre vonatkozó része a tömegmátrixnak: m 2 p 2 2 s2 2 + m 3(q 3 + p 3 ) 2 s 2 2 H v = m 2 p 2 2 + m 3(q 3 + p 3 ) 2, (121) m 3 Θ 1z + Θ 2x s 2 2 + Θ 2yc 2 2 + Θ 3xs 2 2 + Θ 3zc 2 2 H ω = Θ 2z + Θ 3y, (122) amihez felhasználtuk, hogy a súlypontra számított tehetetlenségi nyomatéki mátrix a lokális koordináta-rendszerekben: Θ ix i Θ Ci = Θ iy, (123) Θ iz 16