Technická zpráva Katedra kybernetiky, Fakulta aplikovaných věd Západočeská univerzita v Plzni

Transkript

1 Technická zpráva Kaedra kyberneiky, Fakula aplikovaných věd Západočeská univerzia v Plzni Kinemaická a dynamická analýza roboických archiekur pro pořeby moderních ulrazvukových konrol svarových spojů komplexních porubních sysémů jaderných elekráren Marin Švejda msvejda@kky.zcu.cz

2 Obsah 1 Úvod 3 2 Sériový manipuláor - Variana Přímá (PKÚ) a inverzní (IKÚ) kinemaická úloha Závislosi rychlosí a zrychlení Sériový manipuláor - Variana Přímá (PKÚ) a inverzní (IKÚ) kinemaická úloha Závislosi rychlosi a zrychlení Univerzální sériový manipuláor 23 5 Závěr 24 2

3 1 Úvod Drivá věšina současných zařízení pro ulrazvukové (UZ) konroly porubních sysémů je realizována jako jednoúčelová zařízení s maximálně jedním supněm volnosi (DoF) koncového efekoru. Za účelem navýšení univerzálnosi akových zařízení je proo nuné se uchýli ke komplexnějším roboických archiekurám s více supni volnosi, keré výrazně zlepšují manévrovaelnos při polohování UZ sondy. Teno rend je bohužel vykoupen vyšší složiosí roboických archiekur, kde kinemaická a dynamická analýza hraje klíčovou roli při jejich návrhu, neboť inuiivně lze o kinemaických a dynamických vlasnosech jen sěží korekně rozhodnou. Předložená zpráva se podrobně zabývá dvěma navrhovanými roboickým archiekurami pro pořeby moderních ulrazvukových konrol svarových spojů komplexních porubních sysémů jaderných elekráren. Jedná se o sériové manipuláory se 4 DoF (3 ranslační a 1 roační) ypu 4R a RRPR. Navržené archiekury se zdají bý dobrým kompromisem mezi složiosí modelu a manévrovaelnosí koncového efekoru. Třeí varianou je univerzální sériový manipuláor se 6 DoF (3 roační a 3 ranslační) ypu 6R. Tao variana je pouze zmíněna a její podrobná analýza bude pravděpodobně prezenována v navazujících echnických zprávách. Hlavní náplní zprávy je kinemaická a dynamická analýza předložených archiekur, kerá voří nedílnou součás synézy ěcho roboických zařízení s ohledem na mechanickou konsrukci, dimenzování pohonů a převodovek, osazení vhodných čidel, ad. Viruální simulační modely byly vyvořeny v prosředí Malab/Simulink/SimMechanics. 2 Sériový manipuláor - Variana 1 Sériový manipuláor v první varianě je vořen čyřmi roačními akuáory (manipuláor ypu 4R). První kloub manipuláoru reprezenuje pojezd po porubí kruhového průřezu. Koncový efekor manipuláoru umožňuje posunuí UZ sondy libovolně v prosoru (3 ranslační DoF) a její orienaci vzhledem k svislé ose pojezdu (1 roační DoF). Obrázek 2 znázorňuje schémaické uspořádání manipuláoru včeně zavedených souřadných sysémů (s.s.). Základní kinemaický popis manipuláoru je realizován prosřednicvím Denavi-Harenbergovy (D-H) úmluvy. D-H úmluva umožňuje sysemaické zavedení souřadných sysémů ramen manipuláorů akovým způsobem, aby vzájemné polohové vzahy mezi jednolivými rameny bylo možné popsa maximálně čveřicí kinemaických paramerů (D-H paramery), viz Pozn. 1 Poznámka 1 (Denavi-Harenbergova úmluva) Dnes snad nejznámější úmluva pro eleganní popis geomerie sériových manipuláorů. Předpokládejme dvě ramena manipuláoru Link i 1 a Link i, kerá jsou spojena kloubem Join i s jedním supněm volnosi, viz Obrázek 1. 3

4 Obrázek 1: D-H úmluva Definice s.s. F i = {O i x i y i z i } za předpokladu znalosi s.s. F i 1 = {O i 1 x i 1 y i 1 z i 1 } dle D-H úmluvy je vyjádřena následovně: Zvol osu z i podél osy roace, resp. ranslace kloubu Join i + 1 a osu z i resp. ranslace kloubu Join i podél osy roace, Umísi počáek O i s.s. F i do průsečíku osy z i a normály 1 os z i 1 a z i. Umísi počáek O i s.s. F i = {O i x i y i z i } do průsečíku osy z i 1 a éže normály. Zvol osu x i a x i Zvol osu y i a y i podél normály ve směru od kloubu Join i do kloubu Join i + 1. ak, aby výsledné s.s. byly pravoočivé. Lze snadno ukáza, že D-H úmluva nedefinuje jednoznačně umísění s.s. v následujících případech. Pro s.s. F = {O x y z } je určena jednoznačně pouze osa z (podle osy roace, resp. ranslace prvního kloubu manipuláoru Join 1). Osu x a počáek O lze proo voli libovolně. Osa y je pak určena ak, aby výsledný sysém byl opě pravoočivým. Pro s.s. F n = {O n x n y n z n }, kde n je poče kloubů s jedním supněm volnosi uvažovaného manipuláoru není jednoznačně určena osa z n, neboť kloub Join n + 1 již neexisuje. Osa x n však musí zůsa kolmá k ose z n 1. Pokud jsou dvě po sobě jdoucí osy kloubů (z i 1 a z i ) paralelní, jejich normála není jednoznačně definována (může bý libovolně posunua ve směru os kloubů). Pokud se dvě po sobě jdoucí osy kloubů (z i 1 a z i ) proínají (normála je nulové délky), osa x i bude volena ak, aby byla kolmá k rovině definované osami z i 1 a z i. Její kladný směr však může bý volen libovolně. Nyní může bý vzájemná poloha s.s. F i 1 a F i popsána pouze pomocí čyř D-H paramerů: 1 normála os x a y je spojnice ěcho os s minimální vzdálenosí svírající s osami pravý úhel 4

5 a i... vzdálenos mezi počáky O i a O i d i... vzdálenos mezi počáky O i 1 a O i α i... úhel mezi osami z i 1 a z i daný pooočením s.s. F i podél osy x i θ i... úhel mezi osami x i 1 a x i daný pooočením s.s. F i podél osy z i 1 Je zřejmé, že pro základní ypy kloubů s jedním supněm volnosi plaí: kloub Join i je ypu P proměnná definující pohyb kloubu je d i, proměnné a i, α i, θ i jsou konsany definující geomerické uspořádání ramene Link i kloub Join i je ypu R proměnná definující pohyb kloubu je θ i, proměnné a i, d i, α i jsou konsany definující geomerické uspořádání ramene Link i Transformační vzah, v našem případě homogenní ransformační maice, mezi s.s. F i 1 a F i je dán následujícím způsobem. Vyber s.s. F i 1 Posuň eno sysém podél osy z i 1 o vzdálenos d i a ooč jej okolo osy z i 1 o úhel θ i dosáváme s.s. F i. Maice přechodu2 : 1 c θi s θi c θi s θi T i 1 i = Trans(z, d i ) Ro(z, θ i ) = 1 s θi c θi 1 d i 1 s θi c θi 1 d i (1) Posuň s.s. F i podél osy x i o vzdálenos a i a ooč jej okolo osy x i o úhel α i dosáváme s.s. F i. Maice přechodu: 1 a i 1 1 a i T i i = Trans(x, a i ) Ro(x, α i ) = Výsledná maice přechodu ze s.s. F i 1 do s.s. F i je dána: T i 1 i 1 c αi s αi 1 s αi c αi c αi s αi s αi c αi (2) = T i 1 i T i i = Trans(z, d i ) Ro(z, θ i ) Trans(x, a i ) Ro(x, α i ) = c θi s θi c αi s θi s αi a i c θi = s θi c θi c αi c θi s αi a i s θi s αi c αi d i (3) 1 Připomeňme, že maice přechodu maice (3) je funkcí pouze kloubových souřadnic θ i (pro roační klouby R) a d i (pro ranslační klouby P). 2 Zkraka c θi, resp. s θi označuje cos θ i, resp. sin θ i. 5

6 D-H paramery manipuláoru pro vzájemnou polohu zavedených s.s. na Obrázku 2 lze sanovi následovně: i d i θ i a i α i 1 θ 1 a 1 π 2 2 θ 2 a 2 3 θ 3 a 3 4 θ 4 a 4 Tabulka 1: D-H paramery manipuláoru - Variana 1 Link 3 Link 2 Link 1 Link 4 Obrázek 2: Sériový manipuláor - Variana 1 Polohy roačních kloubů manipuláoru jsou určeny kloubovými souřadnicemi Q jako: Q = [ ] T θ 1 θ 2 θ 3 θ 4 (4) Návrhové paramery manipuláoru ξ předsavují délky jednolivých ramen: ξ = [ ] T a 1 a 2 a 3 a 4 (5) 6

7 kde a 1 = O O 1, a 2 = O 1 O 2, a 3 = O 2 O 3, a 4 = O 3 O 4. Poloha koncového efekoru lze popsa zobecněnými souřadnicemi X: X = [ x y z φ ] T (6) kde x, y, z jsou souřadnice bodu O 4 vzhledem k s.s. F a φ je vzájemné naočení s.s. F 4 a F 1 okolo osy z, edy maice roace R 1 4 = ro(z, φ) Přímá (PKÚ) a inverzní (IKÚ) kinemaická úloha Kinemaickými úlohami rozumíme závislos zobecněných souřadnic manipuláoru na souřadnicích kloubových a naopak, edy relace: X = G(Q) (PKÚ) (7) Q = G 1 (X) (IKÚ) (8) Zaímco PKÚ je pro sériové manipuláory řešielná vždy analyicky a má jednoznačné řešení, pro IKÚ oo obecně neplaí (nemusí exisova řešení v uzavřeném varu, exisence více řešení). Avšak v případě námi uvažovaných archiekur manipuláorů lze, s pomocí vhodné dekompozice, naléz řešení IKÚ analyicky. PKÚ: PKÚ manipuláoru lze přímo vyjádři posupným vynásobením homogenních ransformačních maic, odpovídajícím D-H paramerům z Tabulky 1. Pro pozici bodu koncového efekoru plaí: T 4(Q, ξ) = T 1(θ 1 ) T 1 2(θ 2 ) T 2 3(θ 3 ) T 3 4(θ 4 ) (9) [ x y z ] T = O 4 = T (Q, ξ) 4[1 : 3, 4] Pro orienaci koncového efekoru pak plaí: T 1 4(Q, ξ) = T 1 2(θ 2 ) T 2 3(θ 3 ) T 1 4(θ 4 ) (1) cos φ sin φ R 1 4 = T 1 4(Q, ξ)[1 : 3, 1 : 3] = sin φ cos φ 1 φ = aan2(r 1 4[2, 1], R 1 4[1, 1]) IKÚ: IKÚ lze sanovi pomocí dekompozice manipuláoru na jednodušší 2 DoF archiekuru - planární manipuláor ypu 2R vořený rameny Link 2, Link 3 a umísěný vzhledem k s.s. F 1. První kloubovou souřadnice lze sanovi přímo z polohy bodu O 4 jako: θ 1 = aan2(o 4[2], O 4[1]) (11) 3 označení ro(z, φ) = Ro(z, φ)[1 : 3, 1 : 3], viz Pozn. 1 a označení T [c : d] označuje a-ý až b-ý řádek a c-ý až d-ý sloupec maice T 7

8 Poloha O 3 koncového efekoru 2R manipuláoru vzhledem k s.s. F 1 lze urči posunuím polohy bodu O 4 ve směru osy x s.s. F 4 : O 1 3 = O 1 4 R 1 4[:, 1] a 4 (12) kde cos φ sin φ R 1 4 = sin φ cos φ 1 O 1 4 = (T 1(θ 1 )) 1 O 4 kde T 1(θ 1 ) je již známá ransformační maice. Je zřejmé, že souřadnice bodu O 3 musí odpovída svým varem součinu příslušejících ransformačních maic, edy porovnáním: kde T 1 3(θ 2, θ 3 ) = T 1 2(θ 2 ) T 2 3(θ 3 ). Dosáváme sousavu dvou rovnic pro neznámé 4 θ 2, θ 3. Umocněním a sečením (14), (15) dosáváme: O 1 3 = [ w x w y ] T! = T 1 3(θ 2, θ 3 ) (13) w x = a 3 cos θ 2,3 + a 2 cos θ 2 (14) w y = a 3 sin θ 2,3 + a 2 sin θ 2 (15) w 2 x + w 2 y = a a a 2 a 3 cos θ 3 cos θ 3 = w2 x + w 2 y a 2 2 a2 3 2a 2 a 3 (16) sin 2 θ 3 + cos 2 θ 3 = 1 sin θ 3 = ± 1 cos 2 θ 3 (17) Tedy; θ 3 = aan2(sin θ 3, cos θ 3 ) (18) Řešením sousavy rovnic: pro neznámé sin θ 2, cos θ 2 dosáváme: w x = a 3 cos θ 2,3 + a 2 cos θ 2 (19) w y = a 3 sin θ 2,3 + a 2 sin θ 2 (2) sin θ 2 = a 3 sin θ 3 w x + (a 2 + a 3 cos θ 3 )w y w 2 x + w 2 y cos θ 2 = (a 2 + a 3 cos θ 3 )w x + a 3 sin θ 3 w y w 2 x + w 2 y (21) (22) Tedy; θ 2 = aan2(sin θ 2, cos θ 2 ) (23) Poslední kloubová souřadnice lze sanovi jednoduše rozdílem: θ 4 = φ θ 2 θ 3 (24) Z rovnice (17) edy vyplývají dvě řešení IKÚ. 4 sin θ 2,3 = sin (θ 2 + θ 3) 8

9 2.2 Závislosi rychlosí a zrychlení Aby bylo možné sanovi korekní dynamický model manipuláoru, je nezbyně nuné zná kromě polohových závislosí aké závislosi rychlosí a zrychlení. Závislos rychlosi Q a zrychlení Q kloubových souřadnic na poloze X, rychlosi Ẋ a zrychlení Ẍ zobecněných souřadnic nazýváme inverzní okamžiou kinemaickou úlohou (IOKÚ) a opačně přímou okamžiou kinemaickou úlohou (POKÚ). Inuiivně je parné, že IOKÚ by bylo možné odvodi od IKÚ přímou časovou derivací rovnice (8) poažmo rovnic v z Kapioly 2.1. Přeso, že eno posup je možný (zejména využiím různých sofwarových násrojů pro symbolické derivování - Maple, Mahemaica, ad.), věšinou vede na složié, mnohačlenné vzahy, keré jsou pro implemenaci do řídících algorimů nevhodné. Alernaivním přísupem je sysemaická geomerická meoda, podrobně odvozená v [4]. Meoda je sručně shrnua v Poznámce 2. Poznámka 2 (Výpoče rychlosí v kloubech manipuláoru - geomerický přísup) Translační rychlos Ȯ i a úhlovou rychlos ω i i-ého s.s. F i vzhledem k s.s. F lze psá jako: q 1 ] q 2 [ [Ȯ i j p 1 j p 2... j p j... j p ] i ω =. j o i 1 j o 2... j o j... j o i }{{} q j (25) J i (Q). q i Tedy: Ȯ i = j p 1 q 1 + j p 2 q j p j q j + + j p i q i (26) ω i = j o 1 q 1 + j o 2 q j o j q j + + j o i q i [ j p] j kde j = 1... i a sloupcové subvekory, j p j R3 respekive j o j R 3 kinemaického jakobiánu j o j J i (Q), zprosředkovávají příspěvek j-ého kloubu q j do celkové ranslační respekive úhlové rychlosi s.s. F i vzhledem k s.s. F. Lze snadno ukáza, že příspěvek ranslační rychlosi v j,i a úhlové rychlosi ω j,i do s.s. F i (vyjádřenou vzhledem k s.s. F ) způsobenou pohybem j-ého kloubu, lze pro konkréní ypy kloubů a zavedení popisu s.s. dle D-H úmluvy vyjádři jako: Join j je ypu P (q j = d j ): Join j je ypu R (q j = θ j ): ω j,i = (27) v j,i = d j z j 1 ω j,i = θ j z j 1 (28) v j,i = ω j,i r j 1,i = θ j z j 1 r j 1,i kde z j je osa z s.s. F j a r j,i = O i O j je vzájemná ranslační poloha s.s. F j a F i. Při znalosi maic přechodu T j je zřejmé, že: z j = T j[1 : 3, 3] a r j,i = T i [1 : 3, 4] T j[1 : 3, 4] (29) Sloupcové subvekory j p j a jo j lze edy s ohledem na (26) psá jako: 9

10 Join j je ypu P (q j = d j ): Join j je ypu R (q j = θ j ): [ j p] = j j o j [ ] z j 1 [ j p] [ z = j 1 r ] j 1,i j j o j z j 1 (3) (31) Výsledný kinemaický jakobián J i (Q) je edy přímo dán rovnicemi (3), (31), (29) a lze urči pomocí dílčích maic přechodu T i 1 i s.s. manipuláoru. Pro efekivní výpoče ranslační rychlosi Ȯ i a úhlové rychlosi ω i (edy POKÚ pro libovolný s.s. v kinemaickém řeězci manipuláoru) lze nasíněný posup modifikova na následující zobecněný rekurzivní algorimus pro libovolný yp kloubů. Definujme pomocnou proměnnou σ i, pro kerou plaí: σ j = pokud Join j je ypu R (q j = θ j ) σ j = 1 pokud Join j je ypu P (q j = d j ) σ j = 1 σ j Z rovnic (26), (3) a (31) vyplývá následující rekurzivní schéma vzhledem k s.s. F : ω j = ω j 1 + z j 1 σ j q j (32) Ȯ j = Ȯ j 1 + ω j r j 1,j + z j 1σ j q j (33) Z uvedeného posupu výpoču rychlosi kloubových souřadnic v Poznámce 2 vyplývá, že pro danou polohu manipuláoru vždy exisuje lineární závislos mezi rychlosmi kloubových a zobecněných souřadnic, kerá je dána kinemaickým jakobiánem J n (Q), kde n označuje poče akuáorů manipuláoru. V případě námi uvažovaného manipuláoru lze ranslační rychlosi koncového efekoru Ȯ 4 vyjádři prosřednicvím kinemaického jakobiánu J4 (Q) (horní index označuje vzažný s.s.) a úhlovou rychlos ω 1 4 = [ ] T φ prosřednicvím kinemaického jakobiánu J 1 4(θ 2, θ 3, θ 4 ) následovně: [ O 4 ω 4 ] = J 4(Q) Q, [ O 1 4 ω 1 4 ] θ 2 = J 1 4(θ 2, θ 3, θ 4 ) θ 3 (34) θ 4 Kde kinemaické jakobiány J 4 a J 1 4 jsou vypočíány posupem z Poznámky 2. Výsledný kinemaický jakobián celého manipuláoru J 4 (Q) a řešení POKÚ a IOKÚ pro rychlosi lze souhrně psá jako: [ ] [ O Ẋ = 4 J ] = 4(Q)[1 : 3, :] φ J 1 4(θ 2, θ 3, θ 4 )[6, :] }{{} (POKÚ) (35) J 4 (Q) Q = (J 4 (Q)) 1 Ẋ (IOKÚ) (36) Chceme-li dále počía POKÚ a IOKÚ pro zrychlení, je zřejmé, že pořebujeme zná i časovou derivaci kinemaického jakobiánu. Poznámka 3 předkládá sysemaický geomerický přísup pro akový výpoče, jehož deailní odvození lze opě naléz v [4]. 1

11 Poznámka 3 (Výpoče zrychlení v kloubech manipuláoru - geomerický přísup) Translační zrychlení Ö i a úhlové zrychlení ω i s.s. F i vzhledem k s.s. F lze sanovi přímou časovou derivací z rovnice (25) jako: q 1 q 1 ] q 2 q 2 [Ö i ω = J i (Θ). i q j + J i (Θ). q j (37).. q i q i Tedy: Ö i = j p 1 q 1 + j p 2 q j p j q j + + j p i q i + j p 1 q 1 + j p 2 q j p j q j + + j p i q i (38) ω i = j o 1 q 1 + j o 2 q j o j q j + + j o i q i + j o 1 q 1 + j o 2 q j o j q j + + j o i q i Časová derivace kinemaického jakobiánu J i (Θ) je dána časovými derivacemi jeho dílčích prvků, edy jeho sloupců, viz rovnice (3),(31): Join j je ypu P (q j = d j ): [ ] jp j j o = j [ż ] j 1 (39) Join j je ypu R (q j = θ j ): [ ] [ż jp j j o = j 1 r j 1,i + z j 1 ] ṙ j 1,i j ż j 1 (4) kde ṙ j,i = Ȯ i O j a z i = T i [1 : 3, 4]. Translační rychlosi Ȯ i a úhlovou rychlos ω i lze vypočía jako v rovnici (25) či pomocí rekurzivního algorimu (32), (33). Časovou derivaci osy ż i lze se znalosí ω i urči jako: ż i = ω i z i (41) Analogicky jako při výpoču kinemaického jakobiánu kompleního manipuláoru, viz rovnice (35) lze podle algorimu v Poznámce 3 sanovi derivaci kompleního kinemaického jakobiánu manipuláoru J 4 (Q, Q): [ J 4(Q, Q) J = 4(Q, Q)[1 ] : 3, :] 1 J 4(θ 2, θ 3, θ 4, θ 2, θ 3, θ (42) 4 )[6, :] Kde derivace kinemaických jakobiánů J 4 a J 1 4 jsou vypočíány posupem z Poznámky 3. 11

12 Řešení POKÚ a IOKÚ pro zrychlení lze pak psá ve varu: ] [Ö Ẍ = 4 = φ J 4 (Q, Q) Q + J 4 (Q) Q (POKÚ) (43) Q = (J 4 (Q)) 1 (Ẍ J 4 (Q, Q) Q ) (IOKÚ) (44) Výsledný dynamický simulační model byl realizován v prosředí Malab/Simulink/SimMechanics [2]. Požadované polohy, rychlosi a zrychlení kloubových souřadnic jsou generovány výše uvedenými kinemaickými modely (IKÚ,IOKÚ) z požadované rajekorie pohybu koncového efekoru manipuláoru. Požadovaná rajekorie koncového efekoru manipuláoru byla složena ze dvou primiivních rajekorií - přímky a kružnice. Výsledný esovací pohyb manipuláoru lze charakerizova následovně: Přibližovací fáze Posun polohy koncového efekoru X z bodu A do bodu B po přímce: A = [ π ] T [m, m, m, rad] B = [ R L π ] T [m, m, m, rad] Tesovací fáze Posun pozice (ranslace) koncového efekoru X z bodu B do oožného bodu po kružnici kolmé k ose z s.s. F s poloměrem R =.5 [m] v kolmé vzdálenosi L =.4 [m] od počáku s.s. F. Úhel naočení φ zůsává konsanní φ = π. Oddalovací fáze Posun polohy koncového efekoru X z bodu B zpě do bodu A po přímce. Omezení na rychlos v max a zrychlení a max v zobecněných souřadnicích X (časově opimální rajekorie - přepínání mezi a max a a max, viz např. [3]): a max = 1 [ m s 2 ], v max = 1 [ m s ] směr působení graviace je volen v záporném směru osy x s.s. F Paramery manipuláoru byly voleny jako: Kinemaické paramery: Dynamické paramery: ξ = [ ] T [m, m, m, m] Link 1 (pojezd - hmoný bod): hmonos: m 1 = 5 [kg], poloha ěžišě (vzhledem k s.s. F 1 ): cg 1 = [.1 ] T [m] Link 2 (rameno - enká plná yč poloměru.1 m): hmonos: m 2 =.74 [kg], poloha ěžišě (vzhledem k s.s. F 2 ): cg 2 = [.15 ] T [m], maice servačnosi I 2 = diag( , , ) [kg m 2 ] Link 3 (rameno - enká plná yč poloměru.1 m): hmonos: m 3 =.49 [kg], poloha ěžišě (vzhledem k s.s. F 3 ): cg 3 = [.1 ] T [m], maice servačnosi I 3 = diag( , , ) [kg m 2 ] 12

13 Link 4 (rameno - enká plná yč poloměru.1 m): hmonos: m 4 =.25 [kg], poloha ěžišě (vzhledem k s.s. F 4 ): cg 4 = [.5 ] T [m], maice servačnosi I 4 = diag( , , ) [kg m 2 ] břemeno (UZ sonda - hmoný bod): hmonos: m 5 =.3 [kg], umísěn v počáku s.s. F 4 Obrázek 3 znázorňuje simulační schéma v prosředí Simulink/SimMechanics a Obrázek 4 zjednodušenou vizualizaci manipuláoru. Vzhledem k faku, že známe polohy, rychlosi i zrychlení kloubových souřadnic manipuláoru, viz Obrázek 5, umožňuje SimMechanics simulova model v režimu zv. inverzní dynamiky. Právě eno režim simulace hraje klíčovou roli v analýze a návrhu komponen manipuláoru (ramena, akuáory, převodovky, ad.) a plánování požadované rajekorie, neboť nám poskyuje požadované síly a silové momeny ve všech kloubech manipuláoru, viz Obrázek 6, keré jsou zapořebí pro generování požadovaného pohybu. 13

14 B F Weld1 Revolue4 Revolue3 Revolue2 Revolue1 ou_hea4 To Workspace3 Thea_h4 [Links(1).hea_h(4);;] ou_hea3 To Workspace2 Thea_h3 [Links(1).hea_h(3);;] ou_hea2 To Workspace1 Thea_h2 [Links(1).hea_h(2);;] ou_hea1 To Workspace Thea_h1 [Links(1).hea_h(1);;] To Workspace4 ou_hea1_moion To Workspace5 ou_hea2_moion To Workspace6 ou_hea3_moion To Workspace7 ou_hea4_moion Selecor U(E) U Selecor1 U(E) U Selecor2 U(E) U Selecor3 U(E) U Scope4 Selecor7 Trajecory Generaor X dx ddx B F B F B F B F Env Machine Environmen Link4 Join Sensor Join Acuaor3 Link3 Join Sensor1 Join Acuaor2 Link2 Join Sensor2 Join Acuaor1 Link1 Join Sensor3 Join Acuaor Ground CS1 CS2 Link5 From4 [hea4_moion] From2 [hea3_moion] From1 [hea2_moion] From [hea1_moion] Body Sensor Goo [hea1_moion] Goo1 [hea2_moion] Goo2 [hea3_moion] Goo3 [hea4_moion] InverseKinemaics MATLAB Funcion CS1 CS2 CS1 CS2 CS1 CS2 CS1 CS2 CS3 Obrázek 3: Simulační model v SimMechanicsu - Variana 1 14

15 osa porubí Link 4 Link 3 Link 1 Link 2 Obrázek 4: Simulační model v SimMechanicsu - Variana 1 15

16 osa 1 pohyb max. [V,A] = [2, 2][rad/s,rad/s 2 ] P V A osa 2 pohyb max. [V,A] = [.78518,.9676][rad/s,rad/s 2 ] max. [V,A] = [.75455, ][rad/s,rad/s 2 ] max. [V,A] = [.92833, ][rad/s,rad/s 2 ] 1 osa 3 pohyb 2 osa 4 pohyb Obrázek 5: Polohy Q, rychlosi Q a zrychlení Q kloubových souřadnic - Variana 1 4 max. M = 3.832[Nm] 5 max. M = 5.53[Nm] osa 1 M[Nm] 2 2 osa 2 M[Nm] max. M = [Nm] max. M =.3898[Nm].6 osa 3 M[Nm] 1 1 osa 4 M[Nm] Obrázek 6: Požadované momeny v kloubech manipuláoru - Variana 1 16

17 3 Sériový manipuláor - Variana 2 Sériový manipuláor v druhé varianě je vořen řemi roačními akuáory a jedním akuáorem prizmaickým (manipuláor ypu RRPR). První kloub manipuláoru opě reprezenuje pojezd po porubí kruhového průřezu. Koncový efekor manipuláoru opě umožňuje posunuí UZ sondy libovolně v prosoru (3 ranslační DoF) a její orienaci vzhledem k svislé ose pojezdu (1 roační DoF). Obrázek 7 znázorňuje schémaické uspořádání manipuláoru včeně zavedených souřadných sysémů dle D-H úmluvy. D-H paramery manipuláoru pro vzájemnou polohu zavedených s.s. na Obrázku 7 lze sanovi následovně: i d i θ i a i α i 1 θ 1 a 1 π 2 π 2 θ d 3 π 2 4 θ 4 a 4 Tabulka 2: D-H paramery manipuláoru - Variana 2 Link 3 Link 2 Link 4 Link 1 Obrázek 7: Sériový manipuláor - Variana 2 Polohy kloubů manipuláoru jsou určeny kloubovými souřadnicemi Q jako: Q = [ ] T θ 1 θ 2 d 3 θ 4 (45) Návrhové paramery manipuláoru ξ předsavují délky jednolivých ramen: ξ = [ ] T a 1 a 4 (46) 17

18 kde a 1 = O O 1, a 4 = O 3 O 4. Poloha koncového efekoru lze popsa zobecněnými souřadnicemi X: X = [ x y z φ ] T (47) kde x, y, z jsou souřadnice bodu O 4 vzhledem k s.s. F a φ je vzájemné naočení s.s. F 4 a F 1 okolo osy z, edy maice roace R 1 4 = ro(z, φ). 3.1 Přímá (PKÚ) a inverzní (IKÚ) kinemaická úloha PKÚ: PKÚ manipuláoru lze opě přímo vyjádři posupným vynásobením homogenních ransformačních maic, odpovídajícím D-H paramerům z Tabulky 2. Pro pozici bodu koncového efekoru plaí: T 4(Q, ξ) = T 1(θ 1 ) T 1 2(θ 2 ) T 2 3(d 3 ) T 3 4(θ 4 ) (48) [ x y z ] T = O 4 = T (Q, ξ) 4[1 : 3, 4] Pro orienaci koncového efekoru pak plaí: T 1 4(Q, ξ) = T 1 2(θ 2 ) T 2 3(d 3 ) T 3 4(θ 4 ) (49) cos φ sin φ R 1 4 = T 1 4(Q, ξ)[1 : 3, 1 : 3] = sin φ cos φ 1 φ = aan2(r 1 4[2, 1], R 1 4[1, 1]) IKÚ: První kloubovou souřadnici lze sanovi opě přímo z polohy bodu O 4 jako: θ 1 = aan2(o 4[2], O 4[1]) (5) Poloha O 3 lze aké urči posunuím polohy bodu O 4 ve směru osy x s.s. F 4 : O 1 3 = O 1 4 R 1 4[:, 1] a 4 (51) kde cos φ sin φ R 1 4 = sin φ cos φ 1 O 1 4 = (T 1(θ 1 )) 1 O 4 kde T 1(θ 1 ) je již známá ransformační maice. Je zřejmé, že souřadnice bodu O 3 musí odpovída svým varem součinu příslušejících ransformačních maic, edy porovnáním: O 1 3 = [ w x w y ] T! = T 1 3(θ 2, d 3 ) (52) 18

19 kde T 1 3(θ 2, θ 3 ) = T 1 2(θ 2 ) T 2 3(d 3 ). Dosáváme sousavu dvou rovnic pro neznámé θ 2, d 3. w x = d 3 sin θ 2 (53) w y = d 3 cos θ 2 (54) Umocněním a sečením (53), (54) dosáváme: d 3 = ± wx 2 + wy 2 (55) A zároveň: θ 2 = aan2(w x, w y ) pro: d 3 > (56) θ 2 = aan2( w x, w y ) pro: d 3 (57) Poslední kloubová souřadnice lze sanovi jednoduše rozdílem: θ 4 = φ θ 2 (58) Z rovnice (55) edy vyplývají dvě řešení IKÚ. 3.2 Závislosi rychlosi a zrychlení Závislosi rychlosí a zrychlení lze sanovi analogicky jako v Kapiole 2.1. Výsledný simulační model byl opě vyvořen v prosředí SimMechanics a byla provedena simulace v režimu inverzní dynamiky pro pohyb po idenické požadované rajekorii koncového efekoru manipuláoru. Paramery manipuláoru byly voleny následovně: Kinemaické paramery: ξ = [.5.1 ] T [m, m] Dynamické paramery: Link 1 (pojezd - hmoný bod): hmonos: m 1 = 5 [kg], poloha ěžišě (vzhledem k s.s. F 1 ): cg 1 = [.1 ] T [m] Link 2 (rameno - enká plná yč poloměru.1 m): hmonos: m 2 =.37 [kg], poloha ěžišě (vzhledem k s.s. F 2 ): cg 2 = [.75 ] T [m], maice servačnosi I 2 = diag(7 1 4, 7 1 4, ) [kg m 2 ] Link 3 (rameno - enká plná yč poloměru.1 m): hmonos: m 3 =.37 [kg], poloha ěžišě (vzhledem k s.s. F 3 ): cg 3 = [.75 ] T [m], maice servačnosi diag(7 1 4, 7 1 4, ) [kg m 2 ] Link 4 (rameno - enká plná yč poloměru.1 m): hmonos: m 4 =.25 [kg], poloha ěžišě (vzhledem k s.s. F 4 ): cg 4 = [.5 ] T [m], maice servačnosi I 4 = diag( , , ) [kg m 2 ] břemeno (UZ sonda - hmoný bod): hmonos: m 5 =.3 [kg], umísěn v počáku s.s. F 4 19

20 B F Weld1 Revolue2 Prismaic1 Revolue1 Revolue3 ou_q4 To Workspace3 Thea_h4 (Links(1).q_h(4) ) ou_q3 To Workspace2 Thea_h3 (Links(1).q_h(3) ) ou_q2 To Workspace1 Thea_h2 (Links(1).q_h(2) ) ou_q1 To Workspace Thea_h1 (Links(1).q_h(1) ) Selecor7 Scope To Workspace8 Body Sensor ou_q1_moion Generaor Selecor MATLAB Fcn Goo4 X To Workspace9 U(E) U ou_q2_moion Selecor1 U(E) U To Workspace1 ou_q3_moion Selecor2 U(E) U To Workspace11 ou_q4_moion Selecor3 U(E) U B F B F B F B F Env Machine Environmen Link4 Join Sensor Join Acuaor3 Link3 Join Sensor1 Join Acuaor2 Link2 Join Sensor2 Join Acuaor1 Link1 Join Sensor3 Join Acuaor Ground CS1 CS2 Link5 From4 [q4_moion] From2 [q3_moion] From1 [q2_moion] From [q1_moion] [q1_moion] MATLAB Funcion dx ddx Goo5 [q2_moion] Goo6 [q3_moion] Goo7 [q4_moion] CS1 CS2 CS1 CS2 CS1 CS2 CS1 CS2 CS3 Obrázek 8: Simulační model v SimMechanicsu - Variana 2 2

21 Link 4 osa porubí Link 2 Link 3 Link 1 Obrázek 9: Simulační model v SimMechanicsu - Variana 2 21

22 osa 1 pohyb max. [V,A] = [2, 2][rad/s,rad/s 2 ] P V A osa 2 pohyb max. [V,A] = [.6765,.88886][rad/s,rad/s 2 ] max. [V,A] = [.1339,.21][m/s,m/s 2 ] max. [V,A] = [.3857,.54813][rad/s,rad/s 2 ] 4 osa 3 pohyb.4.2 osa 4 pohyb Obrázek 1: Polohy Q, rychlosi Q a zrychlení Q kloubových souřadnic - Variana 2 4 max. M = 3.832[Nm] 5 max. M = [Nm] osa 1 M[Nm] 2 2 osa 2 M[Nm] max. F = [N] max. M =.3898[Nm].6 osa 3 F[N] 5 osa 4 M[Nm] Obrázek 11: Požadované momeny v kloubech manipuláoru - Variana 2 22

23 4 Univerzální sériový manipuláor V Kapiolách 2 a 3 byly předsaveny sériové manipuláory se 4 DoF (3 ranslační a 1 roační). UZ sonda ak může bý vzhledem k povrchu porubí polohována do libovolné pozice a její orienace může bý libovolně měněna pouze úhlem (zobecněná souřadnice φ), kerý svírá kolmá osa porubí (osa x s.s. F 1 ) s podélnou osou UZ sondy (osa x s.s. F 4 ). V případě nunosi řízení obecné libovolné orienace UZ sondy je nezbyně nuné přisoupi k univerzálnějšímu řešení archiekury manipuláoru. Jedna z možných varian manipuláoru se 6 DoF (3 ranslační a 3 roační) je znázorněna na Obrázku 12. Tao předkládaná variana nadále zůsává předměem analýzy a vhodnos éo konsrukce bude nadále diskuována s ohledem na pořeby univerzálnosi versus echnologické složiosi. V současnosi vyvsávají následující faka a oázky ýkající se navržené archiekury: PKÚ lze řeši analyicky (plaí obecně pro sériové manipuláory). Analyické řešení IKÚ je známo pro sériové manipuláory ypu 6R jen za předpokladu, že lze manipuláor dekomponova na zv. sférické zápěsí (3 roační osy se společným průsečíkem) a zbývající čás manipuláoru plnící roli ranslačního pohybu. Uvedená archiekura oo nesplňuje v současnosi nám nejsou známy efekivní algorimy pro řešení IKÚ (vyjma algorimů čisě numerických). Nunos osazova 6 nezávislých roačních akuáorů. Je o skuečně nuné, nesačilo by např. poslední dva klouby realizova jako pasivní? (Zajišění ečného konaku s porubím.) Obrázek 12: Univerzální sériový manipuláor 23

24 5 Závěr Náplní zprávy bylo vyvoření viruálních kinemaicko-dynamických modelů předložených archiekur manipuláorů. Při analýze polohových závislosí mezi kloubovými a zobecněnými souřadnicemi manipuláorů byly nalezeny analyické vzahy pro řešení přímé i inverzní kinemaické úlohy. Závislosi rychlosí a zrychlení kloubových a zobecněných souřadnic byly odvozeny sysemaickou geomerickou meodou bez nunosi symbolického derivování polohových závislosí. Všechny výpočy byly relaizovány ve výpočením prosředí Malab [2] a Maple [1]. Viruální simulační modely byly sesaveny s pomocí blockseu SimMechanics v nadsavbě Simulink v Malabu. Požadované síly a silové momeny v jednolivých kloubech manipuláoru, keré jsou zapořebí pro generování požadované rajekorie koncového efekoru byly získány simulacemi v režimu inverzní dynamiky v SimMechanicsu. 24

25 Reference [1] MapleSof, Maple. hp:// [2] The MahWorks, Malab. hp:// [3] Bláha, L.: Groebnerova báze a eorie řízení. Práce ke sání dokorské zkoušce, Kaedra kyberneiky, FAV, ZČU v Plzni, 211. [4] Švejda, M.: Kinemaika roboických archiekur. Práce ke sání dokorské zkoušce, Kaedra kyberneiky, FAV, ZČU v Plzni,