S13. Kinematika laboratorního robotu EPSON Stránka 1

Základy robotiky (BROB)

2014

Projekt: S13. Kinematika laboratorního robotu EPSON

Vedoucí projektu: prof. Ing. František Šolc, CSc.

Vypracovali: Jakub Krejčí (151650)

Marcel Štoudek (154890)

Petr Kříž (154781)

S13. Kinematika laboratorního robotu EPSON Stránka 2

Zadání

V laboratoři ÚAMT je k dispozici moderní laboratorní robot EPSON PROSIX C3. Vyřešte pro něj

přímou, případně i inverzní, úlohu kinematiky a nalezněte jeho možné singulární polohy. Ověřte

chování robotu při průchodu singulární polohou.

Teoretické zpracování

Základní úlohou kinematiky je zkoumání vzájemného pohybu členů kinematického řetězce,

hlavně pak pohybu koncového členu vzhledem k rámu, v závislosti na kloubových

proměnných (q1, q2, q3). Jelikož je robot EPOSN C3 manipulátor, je zde nejdůležitější znát

vzájemné vztahy mezi polohou koncového bodu a orientací kloubových proměnných

(musíme vědět, jak mají být jednotlivé klouby natočené, abychom dosáhli požadovaného

bodu). K tomuto využijeme homogenní transformaci.

Obrázek 1 - Schéma robotu EPSON C3 s vyznačenými souřadnicovými systémy v jednotlivých kloubech

S13. Kinematika laboratorního robotu EPSON Stránka 3

Matice homogenní transformace (MHT), používaná pro výpočet homogenní transformace,

v sobě obsahuje matici rotace R0, která udává, jak se bude měnit orientace systému 1

vzhledem k systému 0. MHT v sobě dále obsahuje vektor posunutí d0, který nám dává

informaci o tom, jak se mění poloha. Poslední řádek MHT obsahuje řádkový vektor 3 nul a

jedničky.

[

]

Přímá úloha kinematiky

Pro náš konkrétní manipulátor (EPSON C3) jsme vypočítali MHT pro všechny po sobě jdoucí

souřadnicové systémy, umístěné v kloubech manipulátoru, kde proměnné uhel1 až uhel6

udávají natočení příslušných kloubů a proměnné l1 až l5 (l4 a l6 jsou nulové, neobjevují se

proto v maticích) popisují délku jednotlivých kinematických členů manipulátoru (délky

ramen):

MHT pro „tělo“ manipulátoru (klouby 1-3 netvoří zápěstí)

kinematický systém 0 - rotace kolem osy x

[

]

Kde je velikost posunutí v ose x (320mm), je délka ramene 1 promítnutá do osy z

(100mm) a je posunutí počátku souřadného systému od osy kloubu 1 v ose z (90mm)

kinematický systém 1 - rotace kolem osy y

[

]

Kde je délka ramene 2 (250mm)

kinematický systém 2- rotace kolem osy y

[

]

Kde je délka ramene 3 (250mm)

S13. Kinematika laboratorního robotu EPSON Stránka 4

MHT pro zápěstí manipulátoru

kinematický systém 3- rotace kolem osy z

[

]

kinematický systém 4- rotace kolem osy y

[

]

Kde je délka ramene 5 (65mm)

kinematický systém 5 – rotace kolem osy z

[

]

Pokud známe kloubové souřadnice manipulátoru q (úhly natočení jednotlivých kloubů),

můžeme vypočítat polohu koncového bodu manipulátoru. Tento výpočet se nazývá přímá

úloha kinematiky a řeší se tak, že nalezneme MHT mezi počátečním souřadnicovým

systémem (rám manipulátoru) a jeho koncovým souřadnicovým systémem. Řešení získáme

tak, že mezi sebou vynásobíme jednotlivé MHT mezi všemi systémy:

H06 = H01*H12*H23*H34*H45*H56

Jelikož je výsledný tvar MHT mezi počátkem a koncovým bodem manipulátoru H06 příliš

dlouhý, je zobrazen pouze v přiloženém m-file.

Výsledné souřadnice koncového bodu manipulátoru pak najdeme v MHT H06 v jejím

posledním sloupci (první řádek x, druhý řádek y, třetí řádek z).

Inverzní úloha kinematiky a chování manipulátoru v okolí singulární polohy

Chceme-li nalézt kloubové souřadnice q, musíme znát polohu koncového bodu

manipulátoru. To v praxi znamená, že chceme vypočítat natočení jednotlivých kloubů

manipulátoru v závislosti na tom, jaké má v 3D prostoru souřadnice x, y a z koncový bod

manipulátoru. Tento problém řeší právě inverzní úloha kinematiky (inverzní k přímé úloze,

kde se hledaly koncové souřadnice v závislosti na natočení jednotlivých kloubů). Inverzní

úlohu můžeme také vypočítat, pokud známe MHT mezi rámem manipulátoru a koncovým

S13. Kinematika laboratorního robotu EPSON Stránka 5

bodem. Obecně má tato úloha tvar řešení q = f-1 ( Hrk), kde Hrk představuje MHT mezi rámem

a koncovým bodem, nebo q = f-1 (P), kde P je vektor pozice koncového členu manipulátoru,

P= *x; y; z; α; β; γ+.

V našem případě máme manipulátor, který má rameno se 3 stupni volnosti a zápěstí také se

3 stupni volnosti. Změna kloubových proměnných zápěstí nijak neovlivňuje polohu

koncového bodu ramene.

Vycházíme z předpokladu, že souřadný systém umístěný v počátku manipulátoru je shodně

orientovaný se souřadným systémem koncového bodu manipulátoru. Výsledná rovnice se

pak dá zapsat ve tvaru [

] [

] [

]

Hodnoty px, py a pz jsou funkce kloubových proměnných q1, q2 a q3. Úlohu si můžeme

rozložit na 2 části:

1) výpočet prvních 3 kloubových souřadnic (úhly natočení ramene) ze znalosti polohy

kloubu č. 5

– z výše uvedené maticové rovnice tak vyplývá tato rovnice

[

] [

]

S13. Kinematika laboratorního robotu EPSON Stránka 6

Vyřešením této rovnice nalezneme kloubové proměnné q1, q2 a q3, neboli úhel natočení

kloubů ramene.

Označíme si vzdálenosti mezi klouby podle tohoto schématu

kde r = sqrt(z^2+y^2) – vzdálenost kloubu 5 od osy kloubu 1

c = x-320+196.5 – výška bodu od kloubu 2 (vertikální vzdálenost)

d = r-100 – délka od kloubu 2 (horizontální vzdálenost)

ll = sqrt(c^2+d^2)

Aby tyto vztahy platily, musíme nejprve upravit souřadnice do polohy kloubu 1 na tyto

hodnoty:

x = x – 196,5 - souřadnici x je potřeba zmenšit o výšku základny robota

z = z + 90 - souřadnici z je potřeba zvětšit o vzdálenost okraje robota od kloubu 1

Rovnice pro jednotlivé kloubové proměnné:

q1 = atan2(-y,z)

q1 = q1 *180/pi - převedení velikosti úhlu na stupně

q2 = acos ((ll^2+l2^2-l3^2)/(2*ll*l2))-atan2(d,c)

q2 = q2 *180/pi - převedení velikosti úhlu na stupně

q3 = acos((l2^2+l3^2-ll^2)/(2*l2*l3))-pi/2

q3 = q3 *180/pi - převedení velikosti úhlu na stupně

S13. Kinematika laboratorního robotu EPSON Stránka 7

Nyní můžeme pomocí vyřešení přímé úlohy kinematiky pro rameno vypočítat MHT pro první

3 klouby (rameno manipulátoru):

[

]

[

]

[

]

H03 = H01*H12*H23;

Matici Rr3 získáme jako submatici MHT H03 ( Rr3 = H03(1:3,1:3) )

Tímto výpočtem jsme zjistili orientaci souřadného systému v prostoru a můžeme díky ní

vypočítat zbývající 3 kloubové proměnné (pro zápěstí). Výslednou matici rotace pro zápěstí

R36 (je to rotace MHT H36) dostaneme tak, že takto vzniklou maticí rotace systému H03

(rameno) vynásobíme požadovanou matici rotace systému H06. Vzhledem k tomu, že

chceme stejnou orientaci koncového systému a počátečního systému, má H06 tento tvar:

[

]

Matici R získáme jako submatici matice H06 ( R=H06(1:3,1:3) ).

Pokud vynásobíme invertovanou matici rotace Rr3 s maticí R, získáme tak matici rotace

Eulerova zápěstí R36.

[

] [

]

kde ϕ = q4; ϑ = q5 ; ψ = q6;

Z této matice pak můžeme odečíst zbývající kloubové proměnné q4, q5 a q6. Tímto

problémem se budeme dále zabývat v druhé části řešení inverzní úlohy kinematiky.

S13. Kinematika laboratorního robotu EPSON Stránka 8

2) získání kloubových proměnných z matice rotace R36

Kloubové proměnné získáme tak, že porovnáme jednotlivé prvky ve 2 tvarech matice R36.

Výsledné rovnice pro jednotlivé kloubové proměnné však mohou mít více řešení.

Vzhledem k tomu, že inverzní úlohu jsme řešili hlavně proto, abychom mohli zkoumat

chování manipulátoru v okolí singulární polohy (poloha, ve které dojde ke ztotožnění os

z souřadných systémů v kloubech 3 a 5), musíme si nyní zvolit situace, ve kterých se bude

manipulátor pohybovat. Od toho se pak budou odvíjet jednotlivé tvary řešení hodnot

kloubových proměnných.

a) pohyb po přímce nad singulární polohou

q5 = atan2(-sqrt(1-az^2),az)

q4 = atan2(ay,-ax)

q6 = atan2(oz,nz)

S13. Kinematika laboratorního robotu EPSON Stránka 9

Grafy průběhů jednotlivých kloubových proměnných

- 100mm ve směru osy y nad singulární polohou (pohyb, rychlost)

Pohyby ramene

Jak můžeme vidět z tohoto grafu, kloub 1 (rotace kolem osy x) se otáčí v kladném smyslu a

zprostředkovává pohyb ramene v ose y. Klouby 1 a 2 (které rotují kolem osy y) slouží

k nastavení požadované výšky x koncového bodu. Zde pro pohyb nad singulární polohou

musí být úhel q2 v záporných hodnotách (tzn. rameno je mírně vykloněno) a úhel q3 je

naopak v hodnotách kladných (tzn. rameno je zvednuto)

S13. Kinematika laboratorního robotu EPSON Stránka 10

Pohyby zápěstí

Z grafu je patrné, že kloub 4 se otáčí z kladných hodnot do záporných – tedy z pohledu

ramene 3 se otáčí zprava doleva (proti směru hodinových ručiček). Tedy na začátku pohybu,

kdy je celý systém ramen 1-3 otočený doleva (z pohledu v ose z), je rameno 4 otočeno po

směru hodinových ručiček a umožňuje ramenu 5 se „vyklonit“ a to je pak rovnoběžně

orientované s osou z. Aby tomu tak bylo, musí mít úhel q5 zápornou hodnotu. Úhel q6 má

opačnou velikost než úhel 4, tedy „kompenzuje“ natočení ramene 4 v ose z tak, aby byl

koncový bod stejně orientován jako počáteční souřadnicový systém. Při vykonávání pohybu

se rameno 4 stále otáčí jedním směrem, rameno 5 se vyklopí do určité polohy a když je

souřadnice y kladná, začíná se vyklápět zpět (průběh q5 je tedy symetrický). Rameno q6 stále

rotuje jedním směrem podobně jako rameno 4 (ale směrem opačným).

Rychlost ramene

Průběhy všech rychlostí jsou symetrické, nejrychleji se pohybuje rameno 1 – nejdříve mírně

zrychluje a od poloviny pohybu mírně brzdí. Rameno 2 do poloviny pohybu brzdí, kde se

zastaví, a od druhé poloviny se pohybuje opačným směrem. Podobně se pohybuje i rameno

3.

S13. Kinematika laboratorního robotu EPSON Stránka 11

Rychlost zápěstí

Kloub 5 neustále konstantně zpomaluje, v polovině pohybu se zastaví a začne zrychlovat

opačným směrem. Klouby 4 a 6 sice každý rotují opačným směrem, ale v polovině pohybu

(nad singulární polohou) jsou jejich rychlosti nejvyšší – tedy do této polohy zrychlují, od ní

naopak zpomalují.

- 50mm ve směru osy y nad singulární polohou (pohyb, rychlost)

Pohyb ramene

Pohyby všech kloubů mají tvarově stejný průběh jako ve 100mm, vidíme ale, že kloub 2

(rotace kolem osy y) se více blíží nulové hodnotě (tedy je méně vykloněný), stejně tak jako se

kloub 3 (rotace kolem osy y) více blíží k nule (také je méně vykloněný, je vidět, že se blížíme

singulární poloze).

S13. Kinematika laboratorního robotu EPSON Stránka 12

Pohyb zápěstí

Průběhy jsou opět totožné se 100mm, jenom s tím rozdílem, že každý kloub se natáčí do

větších úhlů.

Rychlost ramene

Z grafu vyplývá, že se všechny klouby pohybují stejnou rychlostí, jako se pohybovaly ve

100mm.

S13. Kinematika laboratorního robotu EPSON Stránka 13

Rychlost zápěstí

Vidíme, že kloub 5 (rotace kolem osy y) se pohybuje mírně rychleji než ve 100mm, klouby 4 a

6 (rotace kolem osy z) se pohybují znatelně rychleji, než ve 100mm. Zde se totiž pohybují

rychlostí až 80°/s, zatímco ve vzdálenosti 100mm se pohybovaly rychlostí poloviční. Z toho

vyplývá, že čím blíže singulární poloze se pohybujeme, tím větší nároky jsou kladeny na

rychlosti jednotlivých kloubových proměnných, což není možné zaručit pro pohyby velmi

blízké singulární poloze – rychlosti kloubů mají svá omezení.

S13. Kinematika laboratorního robotu EPSON Stránka 14

b) pohyb po přímce pod singulární polohou

- kvůli fyzikální realizovatelnosti tohoto pohybu jsme zvolili tuto variantu řešení

q5 = atan2(sqrt(1-az^2),az)

q4 = atan2(-ay,ax)

q6 = atan2(-oz,-nz);

Grafy průběhů jsou podobné jako u pohybu nad singulární polohou (viz grafy přiložené v el.

podobě)

c) pohyb po přímce ve výšce singulární polohy (předpověď optimálního průchodu)

- tento pohyb je podle teoretického průběhu snáze realizovatelnější než průchod poblíž

singulární polohy, nutný je však předpoklad, že se pohybujeme tímto bodem po předem

známé trajektorii a „požadujeme“ jistý průběh kloubových proměnných. To je však teorie,

která platí pouze pro jedinou trajektorii – pohyb v ose y (předpoklad našeho řešení).

1) kladné hodnoty y

q5 = atan2(-sqrt(1-az^2),az);

q4 = atan2(ay,-ax);

q6 = atan2(oz,nz);

2) záporné hodnoty y

q5 = atan2(sqrt(1-az^2),az);

q4 = atan2(-ay,ax);

q6 = atan2(-oz,-nz);

d) koncový bod se nachází v singulární poloze

- pokud se koncový bod manipulátoru nachází v singulární poloze, dochází ke křížení os,

výsledné kloubové proměnné pak mají tyto tvary

q4 = π/2;

q6 = atan2(ny,nx)- π/2;

q5 = 0;

zde je pouze jisté, že q5 = 0, jinak můžeme určit pouze q4+q6 pro az=1 nebo rozdíl q4-q6 pro

az=-1 –tedy tato úloha má nekonečně mnoho řešení. My jsme zvolili q4 =90 kvůli

optimálnímu průchodu singulární polohou v ose y (viz graf přiložený v el. podobě), ale proto,

že jsme to chtěli dopředu. Kdyby se robot pohyboval v ose x, byla by optimální poloha q4=0,

proto by naše řešení bylo zde nepoužitelné.

S13. Kinematika laboratorního robotu EPSON Stránka 15

Závěr

Pro manipulátor EPSON C3 jsme vyřešili přímou úlohu kinematiky, díky čemuž bychom mohli

v praxi programovat tento manipulátor zadáváním kloubových proměnných – pomocí

programovacího software bychom zadali, jak má být který úhel natočený a v reálném čase

bychom mohli vidět, jaké jsou souřadnice koncového bodu manipulátoru. Tento princip by

jistě našel využití v nějaké simulační aplikaci pro tento manipulátor. Další a asi největší

využití přímé úlohy kinematiky je při výpočtech inverzní úlohy kinematiky a také například

pro kontrolu, zda je inverzní úloha správně vypočítaná (zpětným dosazením).

Abychom mohli ověřit chování manipulátoru poblíž singulárních poloh, museli jsme vypočítat

například inverzní úlohu kinematiky. Řešení této úlohy je podstatně složitější, než řešení

přímé úlohy, protože počítáme se soustavou nelineárních rovnic. V praxi má ale inverzní

úloha větší využití, než úloha přímá. Pomocí ní můžeme z polohy koncového bodu

manipulátoru určit, jak mají být natočeny jednotlivé klouby. Tohoto by se dalo v reálných

aplikacích využít například při přímém programování robotu. Operátor by buď ručně nebo

pomocí ovládacího panelu navedl koncový bod manipulátoru (nějaké zařízení na konci

zápěstí, například svářecí hlavici) do přesné polohy a výpočtem inverzní úlohy bychom mohli

zjistit, jak mají být jednotlivé klouby natočeny. Další využití by se dalo využít v simulačních

programech, kde bychom mohli nasimulovat pohyb manipulátoru a sledovat, jak se budou

měnit úhly natočení jednotlivých kloubů a případně pohyb optimalizovat.

Zjištění, jak se robot chová při průchodu blízko singulární polohy nebo dokonce při průchodu

skrz tuto polohu, může být velice důležité. Poblíž těchto poloh se totiž robot začíná chovat

nepředvídatelně, začne například zvyšovat rychlosti pohybu, není schopen se hýbat v určité

ose nebo se blíží k hranicím svého vymezeného pracovního prostoru. Pokud by nebyly

ošetřeny průchodu poblíž nebo skrze singulární polohu, mohlo by při nejhorším dojít i

k poškození robotu. Výpočet chování robotu v blízkosti singulární polohy (ve které dojde ke

ztotožnění osy z v souřadných systémech v kloubech 3 a 5) jsme vyřešili pomocí inverzní

úlohy kinematiky. Vypočtené průběhy jsme pak znázornili v grafech pro různé situace: pokud

se manipulátor pohybuje po přímce ve větší vzdálenosti nad singulární polohou, postupně se

přibližuje k singulární poloze, pohybuje se skrze singulární polohu a pro pohyb pod singulární

polohou. Grafy přehledně ukazují, jak se mění úhly natočení jednotlivých kloubů jak pro

rameno manipulátoru (úhly 1 – 3), tak pro zápěstí (úhly 4 – 6). V grafech jsou dále zobrazeny

změny úhlových rychlostí pro jednotlivé klouby. Z grafů se pak dá snadno vyčíst, jak se

manipulátor chová, když se blíží k singulární poloze, když se nachází v singulární poloze (resp.

nad nebo pod singulární polohou) a když se vzdaluje od singulární polohy.

Zdroje: Solc F., Zalud L., Robotika, Vysoke uceni technicke v Brne, 2006, Brno (cs)