Page 1
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)
Page 2
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
Page 3
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)
Page 4
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
Page 5
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
[
] [
]
Page 6
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ě
Page 7
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.
Page 8
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)
Page 9
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)
Page 10
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.
Page 11
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).
Page 12
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.
Page 13
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í.
Page 14
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é.
Page 15
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)