Inverzní kinematická a statická úloha manipulátoru AGEBOT

Transkript

1 Technická zpráva Kaedra kyberneiky, Fakula aplikovaných věd Západočeská univerzia v Plzni Inverzní kinemaická a saická úloha manipuláoru AGEBOT Marin Švejda msvejda@kky.zcu.cz

2 Obsah 1 Úvod 3 2 Analýza sériového manipuláoru Přímá a inverzní kinemaická úloha (PKÚ, IKÚ) Přímá a inverzní okamžiá kinemaická úloha (POKÚ, IOKÚ) Kompenzace graviačních sil Simulační výsledky pro sériový manipuláor Analýza paralelního manipuláoru Popis i-ého kinemaického řeězce: Popis paralelního manipuláoru Inverzní kinemaická úloha (IKÚ) Inverzní okamžiá kinemaická úloha (IOKÚ) Kompenzace graviačních sil Simulační výsledky pro paralelní manipuláor Závěr 27 2

3 1 Úvod Zpráva se zabývá inverzní kinemaickou a saickou úlohu sério-paralelního manipuláor AGE- BOT (AGressive Environmenal robot) pro použií v aplikacích průmyslového čišění (odmašťování, odlakování, ad.). Manipuláor je vořen sériovou čásí se 4 supni volnosi (DoF) koncového efekoru a speciální paralelním sférickým zápěsím se 3 DoF koncového efekoru. Paralelní sférické zápěsí je konsruováno akovým způsobem, aby bylo možné snadno hermeicky odděli elekrické vybavení manipuláoru od agresivního prosředí uvniř průmyslových mycích linek. V exu je uveden algorimus výpoču přímé a inverzní kinemaické úlohy a přímé a inverzní okamžié kinemaické úlohy pro sériový manipuláor. Je ukázáno, že eno yp úloh vede na analyické řešení. Pro paralelní manipuláor je řešena pouze inverzní kinemaická a inverzní okamžiá kinemaická úloha. Lze ukáza, že přímá kinemaická úloha nelze pro paralelní manipuláor řeši v uzavřeném varu a může obsahova až 8 různých řešení, viz [5]. Výsledky v předkládané zprávě byly využiy při návrhu řízení prooypu AGEBOTU. 2 Analýza sériového manipuláoru Manipuláor je vořen sériovým kinemaickým řeězcem ypu PRRR (všechny klouby manipuláoru jsou akivní, zasávají edy úlohu akuáorů). Koncový efekor manipuláoru má 4 DoF (3 ranslační DoF - libovolná ranslace v prosoru a 1 roační DoF - úhel svírající rameno koncového efekoru s osou y S ). Obrázek 1 znázorňuje 3D CAD výkres a příslušné schémaické uspořádání manipuláoru. Kloubové souřadnice manipuláoru: Θ S = [ d 1 θ 1 θ 2 θ 3 ] T (1) Zobecněné souřadnice manipuláoru (poloha koncového efekoru): [ O S ] X S = 4 = [ x y z φ ] T φ (2) kde O S 4 označuje souřadnice bodu O 4 vyjádřené v souřadném sysému (s.s.) F S 1. Návrhové paramery manipuláoru: Kinemaické paramery: ξ S = [ L 1 L 2 L 3 L 4 ] T (3) Dynamické paramery (význam dynamických paramerů viz Obrázek 2): Poloha ěžišě je chápána v s.s. příslušného ramene, např. pro rameno Link 2 je poloha jeho ěžišě udána v souřadnicích s.s. F 2, kerý je s ramenem pevně spojen. M S = [ m 1 m 2 m 3 m 4 m e ] T (4) CG S = [ ] cg 1x cg 2x cg 3x cg 4x T cg 1 cg 2 cg 3 cg 4 = cg 1y cg 2y cg 3y cg 4y (5) cd 1z cg 2z cg 3z cg 4z Kde m i značí hmonos ramene Link i a m e značí hmonos břemene s ěžišěm v bodě O 4. 1 Označení s.s. F i = O i x iy i z i 3

4 Link 4 roační kloub (yp R) prismaický kloub (yp P) (akuáory manipuláoru) Osa 4 Join 4 Link 3 Join 3 Osa 3 Link 2 Osa 2 Join 2 Link 1 Osa 1 Join 1 Obrázek 1: Sériová čás AGEBOTu Link 4 Link 3 Link 2 ěžišě Link 1 Obrázek 2: Dynamické paramery sériové čási AGEBOTu (Poznamenejme, že z-ové souřadnice polohy ěžišť jsou vzhledem k možnému pohybu manipuláoru nepodsané.) 4

5 2.1 Přímá a inverzní kinemaická úloha (PKÚ, IKÚ) Inverzní (IKÚ) a přímá (PKÚ) kinemaická úloha obnáší výpoče polohy akivních kloubových souřadnic manipuláoru Θ S na základě polohy zobecněných souřadnic X S a naopak. Polohové závislosi lze popsa následujícím způsobem, kerý vychází z modifikovaného modelu manipuláoru Agebo popsaného v [4]. Vhodným zavedením souřadných sysémů (s.s.), keré reprezenují polohu jednolivých ramen (Link i) manipuláoru, lze kinemaickou ransformaci mezi dvěma s.s. F i = O i x i y i z i popsa s pomocí homogenních ransformačních maic T j i R4 4, kde T j i [1 : 3, 1 : 3] = Rj i reprezenuje2 maici roace mezi s.s. F i a F j T j i [1 : 3, 4] = Oj i reprezenuje vekor posunuí mezi s.s. vzhledem k s.s. F j ransformace lze skláda prosým posupným násobením ransformačních maic T 3 = T 1 T 1 2 T 2 3, T 1 3 = T 1 2 T 2 3, ad. Poznamenejme, že zavedení s.s. je provedeno dle Denavi-Harenbergovy úmluvy, více viz [2]. Pro sériový manipuláor edy plaí: 1 cos (θ 1 ) sin (θ 1 ) L 2 cos (θ 1 ) T s 1 L 1 1 = T 1 sin (θ 1 ) cos (θ 1 ) L 2 sin (θ 1 ) 2 = 1 d (6) cos (θ 2 ) sin (θ 2 ) L 3 cos (θ 2 ) cos (θ 3 ) sin (θ 3 ) L 4 cos (θ 3 ) T 2 sin (θ 2 ) cos (θ 2 ) L 3 sin (θ 2 ) 3 = T 3 sin (θ 3 ) cos (θ 3 ) L 4 sin (θ 3 ) 4 = Výsledná ransformační maice T s 4 lze edy psá 3 : s θ1,2,3 c θ1,2,3 L 4 s θ1,2,3 L 3 s θ1,2 L 2 s θ1 4 T s 4 = T i 1 i (θ i 1 ) = c θ1,2,3 s θ1,2,3 L 4 c θ1,2,3 + L 3 c θ1,2 + L 2 c θ1 1 d 1 = (7) i=2 1 = s φ c φ x c φ s φ y 1 z PKÚ: Z rovnice (7) je ihned vidě řešení přímé kinemaické úlohy (PKÚ) sériového manipuláoru, neboť: [ ] T x y z = O s 4 = T s 4 [1 : 3, 4] (8) φ = θ 1 + θ 2 + θ 3 2 Značení A[a : b, c : d] odpovídá submaici získané z maice A vybráním řádku a... b a sloupce c... d. Případný samosaný symbol ":" reprezenuje všechny řádky/sloupce. 3 Značení s θ1,2,3 odpovídá sin(θ 1 + θ 2 + θ 3). 5

6 IKÚ: Při řešení IKÚ je zřejmé, že kloubová souřadnice ranslačního pojezdu d 1 je přímo rovna hodnoě zobecněné souřadnice z. Dále lze edy posupova jako při řešení IKÚ pro planární manipuláor ypu 3R (Link 2, 3, 4 ), viz Obrázek 3, jehož PKÚ popisuje homogenní ransformační maice T 1 4(θ 1, θ 2, θ 3 ). Hodnou éo maice lze vyjádři se znalosí zobecněných souřadnic X S, edy se známou ransformační maicí T s 4, viz (7) : T 1 4 = T s 4 (T s 1 ) 1 = c φ s φ y L 1 s φ c φ x 1 z 1 O1 4 = y L 1 x (9) Poloha počáku s.s. F 3 v s.s. F 1 lze vypočía jako: w x L 4 c φ y L 1 L 4 c φ O 1 3 = w y = O 1 4 L 4 s φ = x L 4 s φ (1) Zároveň plaí: T 1 3(θ 1, θ 2 ) = 3 i=2 c θ1,2 s θ1,2 L 3 c θ1,2 + L 2 c θ1 T i 1 i (θ i 1 ) = s θ1,2 c θ1,2 L 3 s θ1,2 + L 2 s θ1 1 (11) 1 Porovnáním příslušejících prvků rovnic (1) a (11) dosáváme sousavu dvou nelineárních rovnic pro dvě neznámé θ 1, θ 2 : Umocněním a sečením dosáváme: w x = L 3 cos (θ 1 + θ 2 ) + L 2 cos θ 1 (12) w y = L 3 sin (θ 1 + θ 2 ) + L 2 sin θ 1 w 2 x + w 2 y = L L L 2 L 3 cos θ 2 cos θ 2 = w2 x + w 2 y L 2 2 L2 3 2L 2 L 3 (13) sin 2 θ 2 + cos 2 θ 2 = 1 sin θ 2 = ± 1 cos 2 θ 2 (14) Tedy: θ 2 = aan2(sin θ 2, cos θ 2 ) (15) Řešením sousavy rovnic (12) pro neznámé sin θ 1, cos θ 1 dosáváme: sin θ 1 = L 3 sin θ 2 w x + (L 2 + L 3 cos θ 2 )w y w 2 x + w 2 y cos θ 1 = (L 2 + L 3 cos θ 2 )w x + L 3 sin θ 2 w y w 2 x + w 2 y (16) (17) Tedy: θ 1 = aan2(sin θ 1, cos θ 1 ) (18) Kloubová souřadnice θ 3 je vypočena z rozdílu: θ 3 = φ θ 1 θ 2 (19) 6

7 Obrázek 3: Planární manipuláor ypu 3R 2.2 Přímá a inverzní okamžiá kinemaická úloha (POKÚ, IOKÚ) Inverzní (IOKÚ) a přímá (POKÚ) okamžiá kinemaická úloha obnáší výpoče rychlosi Θ S a zrychlení Θ S akivních kloubových souřadnic manipuláoru na základě rychlosi Ẋ S a zrychlení Ẍ S zobecněných souřadnic a naopak. Rychlosi kloubových a zobecněných souřadnic lze popsa následující relací: Ẋ S = J(Θ S ) Θ S (POKÚ) (2) Θ S = J 1 (Θ S ) Ẋ S (IOKÚ) (21) Kde J respekive J 1 označuje jakobián respekive inverzní jakobián. Zrychlení kloubových a zobecněných souřadnic lze popsa následující relací: Ẍ S = J(Θ S ) Θ S + J(Θ S ) Θ S (POKÚ) (22) Θ S = J 1 (Θ S ) (ẌS J(Θ S ) Θ ) S (IOKÚ) (23) Kde J respekive J 1 označuje časovou derivaci jakobiánu respekive inverzního jakobiánu. Jakobián J lze počía analyicky v podsaě dvěma způsoby. Buď přímou časovou derivací polohových závislosí kloubových souřadnic, nebo na základě sysemaického geomerického přísupu k výpoču polohy koncového efekoru. První způsob výpoču je inuiivní maemaickým aparáem, nicméně jeho použií je vhodné pouze pro jednoduché kinemaické archiekury manipuláorů, neboť přímá časová derivace kloubových či zobecněných souřadnic časo vede na složié mnohačlenné vzahy. Geomerický přísup výpoču předkládá zcela obecný jednoduše algorimizovaelný posup založený na rekurzivním schémau. Zabývejme se proo nadále právě ímo přísupem. Lze ukáza, viz [5], že úplný kinemaický jakobián 4 J s i full vzhledem k s.s. F s lze pro sériový manipuláor s n klouby ypu P (ranslační) a R (roační) vyjádři pouze na základě hodno 4 kinemaický jakobián dává do souvislosi rychlosi kloubových souřadnic s ranslační a úhlovou rychlosí koncového efekoru, pouze jakobián může realizova ransformaci rychlosí, kde se na sraně rychlosí koncového efekoru vyskyují například časové derivace Eulerových úhlů (nikoliv vekor úhlové rychlosi) 7

8 homogenních ransformačních maic T j i jako: [Ȯs ] [ i j p 1... j p j... j p ] θ 1 i ω s = j o i 1... j o j... j o. i }{{} θ i J s i full (Θ S) }{{} Θ S i = n (24) Kde plaí: A zároveň z s j [ j p] [ j z s j o = j [ j p] = j j o j j 1 ] [ z s j 1 rs z s j 1 (pro kloub ypu P) j 1,i ] (pro kloub ypu R) a r s s j,i lze sanovi z posloupnosi ransformačních maic T 1, T s 2, T s 3, T s 4 jako: z s j = T s j [1 : 3, 3] a r s j,i = T s i [1 : 3, 4] T s j [1 : 3, 4] Úplný kinemaický jakobián J s i full našeho sériového manipuláoru (i = 4) lze edy vyjádři s pomocí (6) a (24) jako: L 4 d 2 L 3 c 2 L 2 b 2 L 4 d 2 L 3 c 2 L 4 d 2 [Ȯs ] 4 ω s = 4 L 4 d 1 L 3 c 1 L 2 b 1 L 4 d 1 L 3 c 1 L 4 d 1 d 1 1 θ 1 θ 2 θ 3 }{{} Θ S }{{} J s 4 full (Θ S) Kde b 1 = sin θ 1, b 2 = cos θ 1, c 1 = sin θ 1,2, c 2 = cos θ 1,2, d 1 = sin θ 1,2,3, d 2 = cos θ 1,2,3. Vzhledem k omu, že koncový efekor může roova pouze v ose z s.s. F s, úplný kinemaický jakobián J s s 4 full lze redukova pouze na kinemaický jakobián J 4 : L 4 d 2 L 3 c 2 L 2 b 2 L 4 d 2 L 3 c 2 L 4 d 2 ] [Ȯ Ẋ S = 4s L 4 d 1 L 3 c 1 L 2 b 1 L 4 d 1 L 3 c 1 L 4 d 1 = φ 1 S (26) } 1 {{ 1 1 } J s 4 (Θ S) Výsledný výpoče inverzní úlohy (rychlosi): (25) Θ S = (J s 4 ) 1 Ẋ S (27) Z vzahů pro výpoče zrychlení (22), (23) je parné, že pořebujeme navíc zná ješě derivaci kinemaického jakobiánu. Tu lze vyjádři, viz [5]. [ ] J s i (Θ S, Θ jp 1... jp j... jp i S ) = j o, i = n (28) 1... jo j... jo i 8

9 Kde plaí: [ ] jp j j o j [ ] jp j j o j = = [żs j 1 ] (pro kloub ypu P) [żs j 1 rs j 1,i + zs j 1 ṙs ż s j 1 j 1,i ] (pro kloub ypu R) A zároveň: ż s i Kde ω s i, Ȯs i lze vypočía z (24). = ω s i Časovou derivaci úplného kinemaického jakobiánu lze edy vyjádři s pomocí (6) a (28) jako 5 : J s z s i a ṙ s j,i = Ȯs i Ȯs j J s i full našeho sériového manipuláoru (i = 4) 4 full(θ S, Θ S ) = (29) ( ) ( ) ( ) ( ) ( ) θ1 L 2 b 1 + θ1,2 L 3 c 1 + θ1,2,3 L 4 d 1 θ1,2 L 3 c 1 + θ1,2,3 L 4 d 1 θ1,2,3 L 4 d 1 θ ( ) ( ) ( ) ( ) ( ) 1 L 2 b 2 θ1,2 L 3 c 2 θ1,2,3 L 4 d 2 θ1,2 L 3 c 2 θ1,2,3 L 4 d 2 θ1,2,3 L 4 d 2 = (3) Redukcí opě získáme derivaci kinemaického jakobiánu jako: J s 4 (Θ S, Θ S ) = (31) ( ) ( ) ( ) ( ) ( ) θ1 L 2 b 1 + θ1,2 L 3 c 1 + θ1,2,3 L 4 d 1 θ1,2 L 3 c 1 + θ1,2,3 L 4 d 1 θ1,2,3 L 4 d 1 = θ ( ) ( ) ( ) ( ) ( ) 1 L 2 b 2 θ1,2 L 3 c 2 θ1,2,3 L 4 d 2 θ1,2 L 3 c 2 θ1,2,3 L 4 d 2 θ1,2,3 L 4 d 2 (32) Výsledný výpoče inverzní úlohy (zrychlení): Θ S = (J s 4 ) 1 ( Ẍ S J s 4 Θ S ) (33) 2.3 Kompenzace graviačních sil Kompenzací graviačních sil manipuláoru rozumíme úlohu výpoču pořebných sil/momenů v akuáorech manipuláoru, keré zajisí klid manipuláoru při působení graviačních sil na ěžišě koncového efekoru a ěžišě jednolivých ramen manipuláoru. Z Obrázku 2 je parné, že 5 Značení θ 1,2,3 odpovídá θ 1 + θ 2 + θ 3. 9

10 polohu ěžišě i-ého ramene Link i vzhledem k s.s. F i 1 lze vyjádři prosřednicvím ransformační maice Ti i 1 a dynamických návrhových paramerů CG S následující ransformační maicí T i i 1 cg : [ R T i 1 i 1 i cg = i r i 1 i 1,i + ] Ri 1 i cg i (34) kde maice roace R i 1 i = T i 1 i [1 : 3, 1 : 3] a vekor posunuí s.s. r i 1 i 1,i = Oi 1 i = T i 1 i [1 : 3, 4]. Lze ukáza, viz [6], [3], že vzah mezi saickými silami a momeny F = [ F x F y F z M x M y M z ] T působícími na koncový efekor manipuláoru lze ransformova do sil/momenů τ kloubových souřadnic pomocí kinemaického jakobiánu J k následovně 6 : τ = J T k F (35) Rovnici (35) můžeme přirozeně využí nejen pro sanovení příspěvku kloubových sil/momenů od působení graviace na koncový efekor, ale aké pro sanovení příspěvků od ěžišť jednolivých ramen manipuláoru. V případě našeho sériového manipuláoru lze saické kompenzační momeny M i, i = v roačních kloubech manipuláoru (Join 2-4 ) sanovi následovně: M 2 M 3 = M 4 ( ) J 1 T ( ) 2 cg J 1 T 3 cg 1 3 ( J 1 4 cg ) T ( J 1 4 ) T F 1 g2 F 1 g3 F 1 g4 F 1 ge (36) Kde J 1 4 je redukovaný kinemaický jakobián vypočený analogicky jako kinemaický jakobián J s 4 z rovnice (24) vzhledem k s.s. F 1, zn. kinemaický jakobián J 1 4 je vyjádřen z posloupnosi ransformačních maic T 1 2, T 1 3, T 1 4, viz (6). Redukované kinemaické jakobiány J 1 i cg jsou počíány opě analogicky jako v (24) s ím rozdílem, že posloupnosi ransformačních maic jsou následující: Pro J 1 2 cg: T 1 2 cg Pro J 1 3 cg: T 1 2, T 1 3 cg = T 1 2 T 2 3 cg Pro J 1 4 cg: T 1 2, T 1 3, T 1 4 cg = T 1 2 T 2 3 T 3 4 cg Kde T i 1 i cg jsou ransformační maice dány rovnicí (34). Poznamenejme, že redukovaným jakobiánem, např. J 1 4 v omo případě rozumíme pouze první ři řádky úplného kinemaického jakobiánu J 1 4 full, neboť chceme zná pouze zobrazení ranslačních sil F x, F y, F z (graviace) do vekoru momenů kloubových souřadnic τ. 6 Někdy bývá vzah nazýván jako saická rovnováha manipuláoru či podmínka saické rovnováhy. 1

11 Výsledné redukované kinemaické jakobiány mají ak následující var: L 2 b 1 b 1 cg 2x b 2 cg 2y J 1 2 cg = L 2 b 2 + b 2 cg 2x b 1 cg 2y (37) J 1 3 cg = L 3 c 1 c 1 cg 3x c 2 cg 3y L 2 b 1 L 3 c 2 + c 2 cg 3x c 1 cg 3y + L 2 b 2 L 3 c 1 c 1 cg 3x c 2 cg 3y L 3 c 2 + c 2 cg 3x c 1 cg 3y L 4 d 1 d 1 cg 4x d 2 cg 4y L 3 c 1 L 2 b 1 L 4 d 1 d 1 cg 4x d 2 cg 4y L 3 c 1 L 4 d 1 d 1 cg 4x d 2 cg 4y J 1 4 cg = L 4 d 2 + d 2 cg 4x d 1 cg 4y + L 3 c 2 + L 2 b 2 L 4 d 2 + d 2 cg 4x d 1 cg 4y + L 3 c 2 L 4 d 2 + d 2 cg 4x d 1 cg 4y L 4 d 1 L 3 c 1 L 2 b 1 L 4 d 1 L 3 c 1 L 4 d 1 J 1 4 = L 4 d 2 + L 3 c 2 + L 2 b 2 L 4 d 2 + L 3 c 2 L 4 d 2 Záporně vzaé (kompenzace) graviační síly v ěžiších ramen F gi a koncového efekoru F ge vyjádřené v s.s. F 1 jsou dány jako: F 1 gi = [ 9.81 m i ] T, i = (38) F 1 ge = [ 9.81 m e ] T 2.4 Simulační výsledky pro sériový manipuláor Simulace manipuláoru byla realizována v programu Malab v prosředí Simulink/SimMechanics. V SimMechanicsu byl vyvořen komplení dynamický model a režim simulace byl nasaven na inverzní dynamiku. Tzn. simulační model generuje komplení požadované síly/momeny v akivních kloubech manipuláoru ak, aby polohy, rychlosi a zrychlení kloubových souřadnic odpovídali požadovaným hodnoám, viz Obrázek5. Požadované hodnoy polohy, rychlosi a zrychlení kloubových souřadnic jsou získávány prosřednicvím IKÚ a IOKÚ z požadovaných poloh, rychlosí a zrychleních zobecněných souřadnic (požadovaná rajekorie pohybu koncového efekoru), viz Obrázek 4. Vzhledem ke známému algorimu výpoču kompenzace graviace v podobě saických kompenzačních momenů akivních kloubů lze snadno porovna momenové požadavky na akivní klouby manipuláoru z hlediska poměru mezi dynamickými silami (způsobené pohybem manipuláoru) a saickými silami (způsobené graviačním působením na jednolivé čási manipuláoru), viz Obrázek 6. Paramery manipuláoru: ξ S = [ ] T M S = [ ] T e CG S = Maice servačnosi ramen v ěžiších (pro simulační model v SimMechanicsu): I S1 = I S2 = , I S 3 = , I S 4 =

12 Požadovaná rajekorie pohybu koncového efekoru v zobecněných souřadnicích manipuláoru byla zvolena jako po čásech přímkový pohyb mezi body A-F, jejichž souřadnice reprezenují zobecněné souřadnice manipuláoru X S. Pohyb po přímkách mezi jednolivými body rajekorie je časově opimální s ohledem na maximální povolenou rychlos v max a zrychlení a max, více o opimálním časovém plánování rajekorie s omezením lze naléz např. v [1], [3], [6] 7 : v max = 1, a max = 1 Požadovaná přímková rajekorie mezi body A-F : A = [.5.75 π ] T [ ] T [, B = pi, C =.7.75 π 2 D = [ ] π T [ ] , E = π T [ ] 2, F =.7.75 π T 2 ] T, (39) 2 1 s v a 1.5 X[1] X[2] X[3] X[4] Obrázek 4: Polohy, rychlosi a zrychlení zobecněných souřadnic X S 7 Poznamenjme, že plánovaná rajekorie je časově opimální vzhledem ke všem čyřem zobecněným souřadnicím X P (3 souřadnice o jednokách [m], 1 souřadnice o jednokách [rad]). Hodnoy v max a a max mohou bý chápány v jednokách [m/s], [m/s 2 ] pouze za předpokladu konsanní hodnoy naočení zobecněné souřadnice φ mezi dvěma po sobě jdoucími body přímkové rajekorie. 12

13 osa 1 pohyb max. [v,a] = [1, 1][m/s,m/s 2 ] s v a osa 2 pohyb 1 5 max. [v,a] = [2.2852, ][rad/s,rad/s 2 ] max. [v,a] = [1.4958, 5.234][rad/s,rad/s 2 ] max. [v,a] = [2.869, 8.942][rad/s,rad/s 2 ] osa 3 pohyb 5 osa 4 pohyb Obrázek 5: Polohy, rychlosi a zrychlení kloubových souřadnic Θ S 13

14 osa 1 F[N] max. F = [N] required forces saic required forces (graviy) osa 2 M[Nm] max. M = [Nm] max. M = [Nm] max. M = [Nm] osa 3 M[Nm] osa 4 M[Nm] Obrázek 6: Požadované síly/momeny kloubových souřadnic F 1 (lineární pojezd) M 2, M 3, M 4 (roační akuáory), saická i dynamická složka požadovaných sil/momenů ze simulačního modelu v inverzní dynamice (modře), saické kompenzační momeny-kompenzace graviačních sil (černě) 3 Analýza paralelního manipuláoru Manipuláor je vořen řemi nezávislými akivními kinemaickými řeězci ypu PUS (jeden lineární akuáor pro každý řeězec) a jedním kinemaickým řeězcem ypu S, kerý voří pasivní sabilizační elemen a omezuje ak poče supňů volnosi koncového efekoru na 3 roační DoF. Obrázek 7 znázorňuje 3D CAD výkres a příslušné schémaické uspořádání manipuláoru. 14

15 (pasivní klouby manipuláoru) Obrázek 7: Paralelní čás AGEBOTu (3 DoF) (lineární akuáory manipuláoru) Předložený paralelní manipuláor spadá do skupiny manipuláorů, kde je koncový efekor připojen k základně klouby ypu S. Paralelní manipuláor lze ak dekomponova na ři kinemaické řeězce B i C i D i reprezenující sériové manipuláory ypu P RR. Kinemaický model ěcho sériových manipuláorů lze odvodi prosřednicvím Denavi-Harenbergovy (D-H) úmluvy, kerá jednoznačně definuje uspořádání jejich souřadných sysémů, viz Obrázek Popis i-ého kinemaického řeězce: Každý kinemaický řeězec lze popsa nezávislými kloubovými a zobecněnými souřadnicemi a sadou kinemaických a dynamických návrhových paramerů, viz Obrázek 8(b). Kloubové souřadnice kin. řeězce Θ i = [ l i1 θ i1 θ i2 ] T (4) Zobecněné souřadnice kin. řeězce X i = D i (41) Návrhové paramery kin. řeězce: Kinemaické paramery: ξ i = C i D i = l i2 (42) 15

16 Dynamické paramery: Poloha ěžišě ramen kin. řeězců je opě chápána v s.s. příslušného ramene (F i1 pro body i1, F i3 pro body i3). Rameno body i2 je chápáno jako nehmoné: M i = [ m i1 m i3 ] T (43) CG i = [ ] T cg i1 cg i3 = cg i1x cg i1y cg i3x cg i3y (44) cg i1z cg i3z 3.2 Popis paralelního manipuláoru Předpokládejme nadále, že kinemaické řeězce paralelního manipuláoru jsou idenické (sejné kinemaické i dynamické návrhové paramery) a akuáory manipuláoru jsou reprezenovány pouze lineárními výsuvy s akivními kloubovými souřadnicemi l i1. Výsledný paralelní manipuláor lze edy popsa následovně: Kloubové souřadnice manipuláoru: Θ P = [ l 11 l 21 l 31 ] T (45) Zobecněné souřadnice manipuláoru (poloha koncového efekoru): X P = [ α β γ ] T (46) Kde α, β, γ označují posupné roace koncového efekoru manipuláoru okolo os x, y a z (Eulerovy úhly podle schémau roace XY Z). Návrhové paramery manipuláoru: Kinemaické paramery: ξ P = [ a 1 a 2 l v ψ ] T kde a 1 respekive a 2 je délka srany rovnosranného rojúhelníka reprezenující základnu respekive koncový efekor manipuláoru, l = l 12 = l 22 = l 32 je dáno paramery kin. řeězců, v je výška manipuláoru a ψ je naočení manipuláoru okolo osy z b v jeho domovské poloze l i1 = l pro i = Dynamické paramery (význam dynamických paramerů viz Obrázek 8): Poloha ěžišě koncového efekoru je dána v s.s. F e. (47) M P = [ m 1 m 3 m e ] T (48) Kde m 1 = m 11 = m 21 = m 31 a m 3 = m 13 = m 23 = m 33 CG P = [ ] cg 1x cg 3x cg ex T cg 1 cg 3 cg e = cg 1y cg 3y cg ey (49) cg 1z cg 3z cg ez Kde cg 1 = cg 11 = cg 21 = cg 31 a cg 3 = cg 13 = cg 23 = cg 33 16

17 body i3 body i2 (nehmoné) Kloub ypu R body i1 Kloub ypu R Kloub ypu P (a) Umísění kinemaických řeězců na základně manipuláoru (b) Umísěný s.s. kin. řeězce dle D-H úmluvy Obrázek 8: Dekompozice paralelního manipuláoru na kin. řeězce 3.3 Inverzní kinemaická úloha (IKÚ) Pro paralelní manipuláory, pro keré plaí, že je základna manipuláoru připojena ke koncovému efekoru dvěma a více nezávislými kinemaickými řeězci prosřednicvím sférického kloubu S, lze řešení IKÚ provéz dekompozicí paralelního manipuláoru na sériové kinemaické řeězce. Polohy, rychlosi a zrychlení pasivních θ i1, θ i2 a akivních l i1 kloubových souřadnic Θ i lze řeši poé odděleně pro každý kinemaický řeězec, jelikož polohy, rychlosi a zrychlení koncových bodů kinemaických řeězců (v našem případě D i ) jsou jednoznačně dány polohou X P, rychlosí Ẋ P a zrychlením ẌP koncového efekoru paralelního manipuláoru. Nechť F i = B i x i y i z i pro i = označuje s.s. kinemaického řeězce B i C i D i a jeho orienace je shodná se s.s. základny manipuláoru F b = b x b y b z b, viz Obrázek 8(a). Počáek O e s.s. koncového efekoru F e lze vyjádři v s.s. jednolivých kinemaických řeězců jako: [ O 1 e = 3 6 a 1 a 1 2 v ] T [, O 2 e = 3 6 a 1 a 1 2 v ] T [ 3, O 3 e = 3 a 1 v Přípojné body koncového efekoru v jeho s.s. F e = x e y e z e lze vyjádři jako: D e 1 = [ 3 6 a 2 a 2 2 ] T [, D e 2 = 3 3 a 2 ] T, D e 3 = [ 3 6 a 2 a 2 2 ] T (5) ] T (51) Vekory B i D i reprezenující polohu koncového efekoru dílčích kinemaických řeězců v příslušejícím s.s. F i lze ak vyjádři jako: B i D i i = O i e + R i e D e i (52) 17

18 Kde R i e = R b e(θ P ) pro i = je známá maice vzájemné roace s.s. základny F b a koncového efekoru F e podle schémau XYZ o hodnoy Eulerových úhlů α, β, γ předsavující zobecněné souřadnice paralelního manipuláoru: R b e = c β c γ c β s γ s β s α s β c γ + c α s γ s α s β s γ + c α c γ s α c β c α s β c γ + s α s γ c α s β s γ + s α c γ c α c β (53) Kde s α = sin(α), c α = cos(α), s β = sin(β), c β = cos(β), s γ = sin(γ), c γ = cos(γ). IKÚ lze nyní sanovi přímo z podmínky na konsanní délku vekoru C i D i = l i2 = l řešením sousavy rovnic, viz [4]: C i D i i 2 = l 2, i = (54) Kde C i D i i = B i D i i [ ] T l i1. Lze ukáza, že exisují dvě řešení IKÚ. Polohu kloubových souřadnic Θ P lze poom psá: l i1 = B i D i i [3] ± l 2 B i D i i [1] 2 B i D i i [2] 2, i = (55) Výpoče pasivních kloubových souřadnic kinemaických řeězců Každý dílčí i-ý kinemaický řeězec B i C i D i lze popsa prosřednicvím D-H úmluvy pomocí homogenních ransformačních maic mezi s.s. F i, F i1, F i2, F i3, viz Obrázek 8(b): 1 c i1 s i1 c i2 s i2 l c i2 T i i1(l i ) = 1 1 l i1, T i1 i2(θ i1 ) = s i1 c i1 1, T i2 i3(θ i2 ) = s i2 c i2 l s i (56) Kde s i1 = sin(θ i1 ), c i1 = cos(θ i1 ), s i2 = sin(θ i2 ), c i2 = cos(θ i2 ). Ze znalosi akivních kloubových souřadnic l i1 lze dopočía pasivní souřadnice θ i1, θ i2 například porovnáním směrového vekoru x-ové osy s.s. F i3 vyjádřeného v s.s. F i1 a normovaného již známého vekoru C i D i vyjádřeného v oožném s.s.: c i1 c i2 T i1 i3 = T i1 i2 T i2 i3 = s i1 c i2 c i1 c i2 s i2 s i1 c i2! C i D i1 i C i D i cd ix i = = cd iy (57) l l s 1 i2 cd iz Pasivní kloubové souřadnice lze edy urči jako: s i2 = cd iz, c i2 = ± cd 2 ix + cd2 iy θ i2 = aan2(s i2, c i2 ) s i1 = cd iy c i2, c i1 = cd ix c i2 θ i1 = aan2(s i1, c i1 ) (58) 18

19 3.4 Inverzní okamžiá kinemaická úloha (IOKÚ) Vzhledem k omu, že poloha X P, rychlos ẊP i zrychlení ẌP zobecněných souřadnic manipuláoru jsou známé, lze sanovi i rychlosi a zrychlení přípojných bodů D i kinemaických řeězců ke koncovému efekoru manipuláoru: Ḋ i i D i i = ω i e = ω i e O e D i i = ω i e R i e D e i (59) R i e D e i + ω i e Ṙi e D e i Kde derivace maice roace Ṙi e, vekor úhlové rychlosi ω i e lze vyjádři následovně, viz [5]: a zrychlení ω i e koncového efekoru Eulerovy kinemaické rovnice pro Eulerovy úhly se schémaem roace XY Z: 1 s β ω i e = ω b e = c α s α c β Ẋ P s α c α c β Derivace maice roace: Ṙ i e = Ṙb e = S(ω b e) R b e kde S(ω b e) = ω b e[3] ω b e[2] ω b e[3] ω b e[1] ω b e[2] ω b e[1] Přímá časová derivace vekoru úhlové rychlosi: c β β γ 1 s β ω i e = ω b e = s β α β c α c β α γ + s α s β β γ + c α s α c β Ẍ P c α α β s α c β α γ c α s β β γ s α c α c β Rychlosi a zrychlení kloubových souřadnic (pasivních i akivních) všech ří kinemaických řeězců B i C i D i lze vyjádři analogicky jako rychlosi a zrychlení kloubových souřadnic pro sériový manipuláor v Kapiole 2.1. Kinemaické jakobiány a jejich derivace pro dílčí sériové kinemaické řeězce B i C i D i lze urči podobným způsobem jako v rovnicích (24), (28) a mají následující var: J i i3 = s i1 lc i2 c i1 ls i2 c i1 lc i2 s i1 ls i2 1 lc i2 (6) Tedy rychlosi pasivních a akivních kloubových souřadnic i-ého kinemaického řeězce lze vyjádři jako: Θ i = l i1 θ i1 θ i2 = (J i i3) 1 Ḋ i i (61) Derivaci kinemaického jakobiánu poom lze psá jako: l ( θ i1 c i1 c i2 + θ ) i2 s i1 s i2 J i i3 = l ( θi2 c i1 s i2 + θ ) i1 s i1 c i2 l θ i2 s i2 l ( θi1 s i1 s i2 θ ) i2 c i1 c i2 l ( θi1 c i1 s i2 + θ ) i2 s i1 c i2 (62) 19

20 Zrychlení pasivních a akivních kloubových souřadnic i-ého kinemaického řeězce lze edy psá jako: ( Θ i = = (J i 1 i i3) Di i J i3 Θ ) i (63) li1 θ i1 θ i2 Rychlosi Θ P a zrychlení Θ P akivních kloubových souřadnic paralelního manipuláoru lze pak snadno získa výběrem z řešení (61), (63). Poznamenejme, že inverzní kinemaický jakobián (J b e) 1 kompleního paralelního manipuláoru lze získa vyjádřením složek vekoru úhlové rychlosi ω b e z rovnic rychlosi pohybu přípojných bodů Ḋi i koncového efekoru, viz rovnice (59) následovně: Ḋ i i = ˆD i i ω b e (64) Kde: ˆD i i = S(R i e D e i ) = z i y i z i x i (65) y i x i x 1 = 1/6 c β a 2 ( c γ 3 3 sγ ) y 1 = 1/6a 2 ( 3s α s β c γ + 3c α s γ 3s α s β s γ + 3c α c γ ) z 1 = 1/6 a 2 ( 3cα s β c γ 3s α s γ 3 c α s β s γ 3 s α c γ ) x 2 = 1/3 c β c γ a 2 3 y 2 = 1/3 (s α s β c γ + c α s γ ) a 2 3 z 2 = 1/3 a 2 (c α s β c γ s α s γ ) 3 ( ) x 3 = 1/6 c β a 2 c γ sγ y 3 = 1/6 a 2 ( 3sα s β c γ + 3c α s γ + 3 s α s β s γ 3 c α c γ ) z 3 = 1/6 a 2 ( 3cα s β c γ 3s α s γ + 3 c α s β s γ + 3 s α c γ ) Tedy, známe-li kinemaické jakobiány dílčích kinemaických řeězců J i i3, viz rovnice (6), (61), lze inverzní kinemaický jakobián kompleního paralelního manipuláoru (J b e) 1 vyjádři jako: Q P = l 11 l 21 l 31 Θ i = l i1 θ i1 θ i2 ( ( = ( = (J i 1 i3) ˆDi i ω b e (66) (J ) ˆD1 1 (J ) ˆD2 2 (J ) ˆD3 3 } {{ } (J b e) 1 ) [1, :] ) [1, :] ω b e (67) ) [1, :] 3.5 Kompenzace graviačních sil Problém kompenzace graviačních sil pro paralelní manipuláory je poněkud složiější než pro manipuláory sériové, neboť příspěvky od graviačních sil dílčích kinemaických řeězců jsou 2

21 mezi sebou navzájem vázány prosřednicvím připojení na koncový efekor. Nelze edy, jako v případě kompenzace graviace u sériových manipuláorů, viz Kapiola 2.3, připočíáva vliv působení graviačních sil na daný kinemaický řeězec pouze do sil a momenů v kloubech ohoo řeězce. Základní myšlenka výpoču kompenzace graviačních sil pro paralelní manipuláory lze formulova následovně: 1. Pro každý samosaný kinemaický řeězec přepoči působení graviace na jejich ramena do sil a momenů kloubových souřadnic řeězce 2. Přepoči působení ěcho získaných kloubových sil a momenů na sílu/momen působící na koncový efekor v přípojném bodě uvažovaného kinemaického řeězce 3. Silové/momenové působení v přípojných bodech kinemaických řeězců přepoči na sílu- /momen, kerá působí na koncový efekor (vzhledem k uvažovaným zobecněným souřadnicím) 4. Vypoči vliv graviace na koncový efekor a výsledné síly/momeny přiči k získaným hodnoám sil/momenům z předchozího bodu známe komplení silové/momenové působení na koncový efekor vlivem působení graviace na všechny čási manipuláoru. 5. Přepoči silové/momenové působení na koncový efekor na momeny/síly akivních kloubových souřadnic manipuláoru Add 1: Analogicky jako v Kapiole 2.3, rovnice (36) lze sanovi pořebné síly F i1 akivních a momeny M i1, M i2 pasivních kloubových souřadnic každého i-ého kinemaického řeězce: ] τ i = F i1 M i1 M i2 = (J i i1 cg) T 1 3 (J i 1 3 i3 cg) T [ F i gi1 F i gi3 kde (J i i1 cg) T je redukovaný kinemaický jakobián i-ého kinemaického řeězce vzhledem k s.s. F i vyjádřený z ransformační maice T i i1 cg, (J i i3 cg) T je redukovaný kinemaický jakobián i-ého kinemaického řeězce vzhledem k s.s. F i vyjádřený z posloupnosi ransformačních maic T i i1, T i1 i2, T i2 i3 cg, viz rovnice (24), ransformační maice T i(k 1) kde maice roace R i(k 1) ik T i(k 1) ik T i(k 1) ik cg = [ R i(k 1) ik (68) ik cg jsou dány analogicky jako v rovnici (34): r i(k 1) i(k 1),ik + Ri(k 1) ik cg ik ] (69) = T i(k 1) ik [1 : 3, 1 : 3] a vekor posunuí s.s. r i(k 1) i(k 1),ik = Oi(k 1) ik = [1 : 3, 4] jsou dány ransformačními maicemi sériových kinemaických řeězců manipuláoru z rovnice (56). cg ik jsou polohy ěžišť a m ik hmonosi i-ého kinemaického řeězce, viz rovnice (43), (48). Záporně vzaé graviační síly v ěžiších ramen i-ého kinemaického řeězce jsou dány: F ik gik = [ 9.81 m ik ] T (7) 21

22 Redukované kinemaické jakobiány mají pak následující var: J i i1 cg = 1 (( l cg 3 [1]) c i2 + s i2 cg 3 [2]) s i1 + c i1 cg 3 [3] c i1 ((l + cg 3 [1]) s i2 + c i2 cg 3 [2]) J i i3 cg = ((l + cg 3 [1]) c i2 s i2 cg 3 [2]) c i1 + s i1 cg 3 [3] s i1 ((l + cg 3 [1]) s i2 + c i2 cg 3 [2]) 1 (l + cg 3 [1]) c i2 s i2 cg 3 [2] (71) Poznamenejme, že z kinemaického jakobiánu J i i1 cg je parné, že poloha ěžišě cg 1 neovlivní výpoče kompenzačních sil. Add 2: Prosřednicvím známého kinemaického jakobiánu J i i3 i-ého kinemaického řeězce, viz rovnice (6), lze přepočía vliv působení graviace z kloubů kinemaického řeězce τ i na sílu F Di působící na koncový efekor manipuláoru v přípojném bodě D i následovně, dle rovnice (35): F Di = ( (J i i3) T ) 1 τ i (72) Add 3: Vzhledem k zavedeným zobecněným souřadnicím X P manipuláoru (Eulerovy úhly) je pohyb koncového efekoru omezen pouze na roaci dle schémau XYZ. Na koncový efekor bude edy, vlivem graviačních účinků na ramena kinemaických řeězců k efekoru připojených, působi silový momen, kerý lze ze známé polohy přípojných bodů D i urči vzhledem k s.s. F b součem: M b e cg chains = 3 R b e D e i F Di (73) i=1 kde R b e je známá maice roace, viz (53) a D e i jsou známé konsanní polohy přípojných bodů vzhledem k s.s. F e, viz (51). Add 4: Silový momen působící na vlasní koncový efekoru manipuláoru v důsledku graviace lze vyjádři jako: M b e cg = R b e cg e F b ge (74) kde cg e je poloha ěžišě v s.s. F e, viz (48) a záporně vzaá graviační síla v ěžiši koncového efekoru o hmonosi m e je: F b ge = [ 9.81 m e ] T Celkový vliv graviace od kinemaických řeězců i vlasního koncového efekoru lze vyjádři výsledným silovým momenem M b e, kerý působí na koncoví efekor a kerý musí bý kompenzován vhodnými reakčními silami v akivních kloubech manipuláoru. M b e = M b e cg chains + M b e cg (75) 22

23 Add 5: Prosřednicvím vzahu pro saickou rovnováhu paralelního manipuláoru, kerý je formálně shodný se vzahem pro saickou rovnováhu manipuláorů sériových, viz (35), lze díky známému inverzního kinemaickému jakobiánu kompleního paralelního manipuláoru (J b e) 1 z rovnice (66) vyjádři pořebné síly v akivních kloubech (yp P) paralelního manipuláoru jako: F 1 F 2 = (J b e) T M b e (76) F 3 kde F i jsou saické kompenzační síly v prismaických kloubech manipuláoru. 3.6 Simulační výsledky pro paralelní manipuláor Simulace manipuláoru byla provedena sejným způsobem jako u manipuláoru sériového z Kapioly 2.4. Generáor požadované rajekorie koncového efekoru manipuláoru byl realizován ak, aby generované zobecněné souřadnice X P odpovídaly posupnému naočení osy z s.s. F e ve směru zadaných směrových vekorů f i pro i = Naočení kolem z-ové osy (zobecněná souřadnice γ) zůsala konsanní. Rozmísění polohy směrových vekorů f i je dáno paramery ψ i a η i dle Obrázku 9. Označíme-li úhel mezi dvěma po sobě jdoucími směrovými vekory f i a f i+1 jako ω i,i+1, omezení na rychlos v max a zrychlení a max změny polohy koncového efekoru je chápáno jako omezení na rychlos a zrychlení úhlu ω i,i+1 pro i = [ ψ1 ψ 2 ψ 3 ] T = [ ] T, [ η1 η 2 η 3 ] T = [ ] T v max = 1 [rad/s], a max = 1 [rad/s 2 ] 23

24 Obrázek 9: Princip generování polohy koncového efekoru paralelního manipuláoru Paramery manipuláoru: ξ P = [ ] T M P = [ ] T.7325 CG P =.8 Maice servačnosi ramen v ěžiších (pro simulační model v SimMechanicsu): I P1 = , I P 3 = , I P e =

25 2 1 s v a 1.5 X[1] X[2] X[3] Obrázek 1: Polohy, rychlosi a zrychlení zobecněných souřadnic X P 25

26 osa 1 pohyb osa 3 pohyb max. [V,A] = [.54794,.85533][m/s,m/s 2 ] s v a max. [V,A] = [.47633,.87984][m/s,m/s 2 ] osa 2 pohyb max. [V,A] = [.44987,.664][m/s,m/s 2 ] Obrázek 11: Polohy, rychlosi a zrychlení kloubových souřadnic Θ P 26

27 6 max. F = [N] 6 max. F = [N] osa 1 F[N] 4 2 osa 2 F[N] required forces 2saic required 2 forces (graviy) max. F = [N] 4 2 osa 3 F[N] Obrázek 12: Požadované síly kloubových souřadnic F i, saická i dynamická složka požadovaných sil ze simulačního modelu v inverzní dynamice (modře), saické kompenzační síly-kompenzace graviačních sil (černě) 4 Závěr Přeso, že uvedená zpráva se zabývá jedním specifickým řešením sério-paralelního manipuláoru, uvedené algorimy lze snadno použí pro všechny neredundanní sériové manipuláory a bez omezení aké pro neredundanní paralelní manipuláory, jejichž koncový efekor je připojen ke kinemaickým řeězcům relací neomezující vzájemnou orienaci připojených komponen, zn. (kloub ypu R pro planární a kloub ypu S pro prosorové manipuláory). Z uvedených posupů je parné, že veškeré okamžié a saické kinemaické závislosi je možné sanovi analyicky, pokud jsou známy polohové závislosi mezi zobecněnými a kloubovými souřadnicemi manipuláoru (PKÚ, IKÚ). Poznamenejme, že POKÚ pro paralelní manipuláory není v exu zahrnua, nicméně může bý snadno odvozena inverzí IOKÚ, kerá vede na řešení sousavy lineárních rovnic. 27

28 Reference [1] L. Bláha. Groebnerova báze a eorie řízení. Práce ke sání dokorské zkoušce, Kaedra kyberneiky, FAV, ZČU v Plzni, 211. [2] J. Denavi and R. S. Harenberg. A kinemaic noaion for lower-pair mechanisms based on marices. J. Appl. Mechanics, June 1955, 22: , [3] B. Siciliano L. Sciavicco. Modelling and Conrol of Robo Manipulaors. Springer, 2 ediion, 2. [4] M. Švejda. Kinemaický popis manipuláoru agebo. Technical repor, kaedra kyberneiky, FAV, ZČU Plzeň, 211. [5] Marin Švejda. Kinemaika roboických archiekur. PhD hesis, Západočeská univerzia v Plzni, Fakula aplikovaných věd, 211. [6] E Dombre W. Khalil. Modeling, Idenificaion and Conrol of Robos. Buerworh- Heinemann,