Popis polohy tělesa. Robotika. Popis polohy tělesa. Vladimír Smutný. Centrum strojového vnímání

Transkript

1 opis poloh tělesa Robotika opis poloh tělesa Vladimír Smutný Centrum strojového vnímání Český institut informatik, robotik a kbernetik (CIIRC) České vsoké učení technické v rae ROBOTICS: Vladimír Smutný Slide, age

2 Bod, vektor, geometrie, algebra O a A b v B Bod a vektor v geometrii a algebře ojem bod v geometrii považuji a dostatečně námý a protože jeho definice není matematick snadná, nebudu ho definovat. Jen chci připomenout, že je dobré ho vnímat jako místo v prostoru, které eistuje neávisle na nějakých číslech. ojem vektor v geometrii můžeme definovat jako uspořádanou dvojici bodů, počáteční a koncový bod. Značíme AB. V geometrii pak můžeme používat obrat jako bod A je průsečíkem kružnice k se středem v bodě B, procháející bodem C a přímk procháející bod D a E. Naše problém v robotice jsou svou podstatou geometrické problém (přinejmenším v kinematice). Velmi často potřebujeme repreentovat geometrii v počítači, který ale s geometrickými pojm přímo pracovat neumí. Matematická teorie lineárních prostorů nám umožňuje pracovat s matematickými objekt tpu uspořádaná n-tice čísel. Le ukáat, že pro geometrické a lineární prostor konečné dimene eistuje vájemně jednonačné obraení (isomorfismus) mei geometrickými a algebraickými objekt. Abchom mohli avést tento isomorfismus, musíme v geometrii avést souřadnicovou soustavu. Geometrický bod v prostoru pak můžeme totožnit s uspořádanou trojicí (vektorem, de algebraický pojem) reálných čísel, kde uvedená čísla popisují souřadnice geometrického bodu v dané souřadnicové soustavě. Takovému vektoru v geometrii říkáme polohový vektor bodu, jeho počáteční bod je počátek souřadnicového sstému, jeho koncový bod je pak repreentovaný geometrický bod. Lineární prostor tak tvoří univerální model geometrického prostoru a jeho pomocí můžeme repreentovat všechn vlastnosti geometrického prostoru a naopak. To, že geometrické pojm můžeme považovat a ákladní, ospravedlňuje například to, že geometrie bla matematik úspěšně pěstována přibližně let be potřeb avádění souřadnic. To, že eistuje isomorfismus, ale ároveň ukauje, že oba popis jsou v principu ekvivalentní a můžeme libovolně přecháet jednoho do druhého. Mei bod a vektor v geometrii můžeme avést řadu operací: dva bod definují vektor v = AB, bod a vektor definují bod A + v = B vektor le sčítat a odčítat b = a + v Tto operace platí be avedení souřadnicového sstému. Stejné operace můžeme avést v algebře mei uspořádanými trojicemi (n-ticemi) repreentující bod a vektor. Zápis vorců ástává stejný, jen operátor namenají něco jiného. Algebraické objekt v rovnicích správně repreentují geometrické objekt, pokud jsou všechn vjádřen ve stejné souřadnicové soustavě. Na volbě souřadnicové soustav přitom neáleží. Formálně budou popis geometrie algebraickými rovnicemi vžd stejné, číselně se liší v ávislosti na volbě souřadnicové soustav. K situaci, kd máme růné souřadnicové sstém, se dostaneme dále. ROBOTICS: Vladimír Smutný Slide 9, age

3 Technická ponámka O A v α B u C β α γ Běžný přístup pro určení parametrů trojúhelníku je používat kosinovou nebo sinovou větu, thagorovu větu a podobně. Tto vět a podobné vorce, jako například součet úhlů v trojúhelníku, je v analtické geometrii obtížné používat. roblém je v tom, že naleené řešení musíme interpretovat do příslušných kvadrantů, uměle vrábět další řešení, či naleená řešení testovat na splnění vstupních podmínek. To může být pracné a drojem řad chb, které se mohou projevit až při provou aříení. Niže uvádím některá doporučení, jak se chbám vhnout: Snažte se počítat souřadnice rohů trojúhelníků místo délek stran či velikostí úhlů v trojúhelníku. oužívejte pro určení úhlů vorec φ = atan(, ) vžd, kdž je to možné. Vhýbejte se použití funkce arccos a podobně. Kdž počítáte úhl a souřadnice, načte je do obráků a interpretujte je vžd orientovaně. Takto spočítané úhl a souřadnice mohou být pak snadno sčítán a odčítán be nutnosti analý příslušné situace. okud pracujete s analtickým tvarem přímk, používejte tvar a + b + c =, který na rodíl od směrnicového tvaru = k + q bechbně pracuje ve všech kvadrantech. Výpočet úhlu mei osou a vektorem AB vnačeným na obráku. Orientovaný, čtřkvadrantový úhel α le spočítat α = atan(b A, B A ). Nejrchlejší a nejbepečnější cesta, jak spočítat úhel β Srovnej běžný prací prášek. mei vektor v a u je následující: α = atan(b A, B A ), () γ = atan(c B, C B ), () β = γ α. (3) V prostoru je situace složitější. Úhel mei dvěma vektor nemá sám o sobě orientaci. Takový úhel φ můžeme snadno spočítat a pomoci skalárního součinu a tak ho můžeme spočítat v prostoru libovolné dimene. Abchom v tříroměrném prostoru mohli definovat orientovaný úhel, musíme si předem určit směr, který budeme považovat a kladný. Ve 3 D prostoru tak mohou dva vektor definovat rovinu a volbou normálového vektoru n k rovině tak volíme orientaci. Výsledný orientovaný úhel θ spočítáme takto: φ = arccos v v v v, (4) θ = arccos v v v v pro n ( v v ) (5) θ = arccos v v v v pro n ( v v ) < (6) Funkci sgn nele použít, protože pro vektor v, v opačné je jejich skalární součin nula, naménko také nula a výsledek je také nula, atímco správně je π. Výše uvedený postup má ještě další problém: normálový vektor n nesmí ležet v rovině určené vektor v, v a žádný vektorů nesmí mít nulovou délku. V těchto případech se ale singularit v uvedeném vorci krjí se singularitami ve skutečném světě, takže je apotřebí se jim vhnout především v realitě. ROBOTICS: Vladimír Smutný Slide, age 3

4 Technická ponámka II O A w α δ B v C β α u γ Necht máme dán souřadnice bodů A a C a délk vektorů v a u. Nejbepečnější cesta, jak určit souřadnice bodu B a příslušné úhl je místo použití kosinové vět a boje s úhlem δ nalét bod B jako průsečík dvou kružnic se střed v bodech A a C a příslušnými poloměr. Tím dostaneme pro bod B dvě řešení. Úhl pak spočítáme pomocí výše uvedených rovnic. ROBOTICS: Vladimír Smutný Slide, age 4

5 Těleso v souřadnicovém sstému Těleso v rovině má 3 DOF. Těleso v prostoru má 6 DOF. Kontrolní oták: Kolik stupňů volnosti má skládací metr na stole? Kolik stupňů volnosti má gumička? Tuhé těleso v našem výkladu uvažujeme jen tuhá tělesa. S tuhým tělesem můžeme sváat souřadnicový sstém a polohu jednotlivých bodů tělesa v tomto souřadnicovém sstému pak náme, např. výkresu předmětu, kterým manipulujeme. Aktuální poloha tělesa v čase le ji popsat polohou souřadnicového sstému s tělesem sváaným v jiném, nepohblivém, světovém souřadnicovém sstému. Jak konkrétně popsat polohu tělesa bude ukááno dále. ohb tělesa v čase můžeme popsat jako funkci aktuální poloh tělesa v ávislosti na čase. Vájemná poloha dvou souřadnicových soustav le vžd roložit na posun a otočení. Zvolíme souřadnicový sstém rámu O. S tělesem svážeme souřadnicový sstém O b b b. opis souřadnicového sstému O b b b v souřadnicovém sstému rámu je: OO = o = o o o (n, t, b). Utvořme matici R = (n, t, b), n, t, b jsou jednotkové a ortogonální vektor, matice R je ortonormální, ted R = R T. ROBOTICS: Vladimír Smutný Slide, age 5

6 opis poloh tělesa Bod v 3D prostoru popsán třemi souřadnicemi. Tuhé těleso v 3D prostoru popsáno šesti souřadnicemi: 3 souřadnice referenčního bodu t =, orientace může být popsána jedním e působů: souřadnicemi vektorů spojených s tělesem (n, t, b), Eulerovými úhl (φ, θ, ψ), rotační maticí R, osou úhlem, kvaternionem, rotačním vektorem. Souřadnice referenčního bodu a rotační matice mohou být kombinován do transformační matice. ro rotační matici srovnej heslo Eric W. Weisstein. Rotation Matri. From MathWorld A Wolfram Web Resource. řevodní vtah jsou přehledně na stránce /rotations/conversions/inde.htm Je třeba dávat poor na použité definice, ab se nesmíchali vorce používající růnou notaci. ROBOTICS: Vladimír Smutný Slide 3, age 6

7 Osa úhel, kvaternion a rotační vektor 4/ S p r s θ r Eulerova věta o rotaci říká, že každá rotace ve 3 D le repreentovat jako rotace okolo určité os s o určitý úhel θ. Tuto dvojici (s, θ) naýváme osa úhel. Kvaternion popisují rotaci pomocí poloh os rotace s a úhlu otočení θ takto: q = (cos(θ/), sin(θ/)s T ) = O (cos(θ/), sin(θ/)s, sin(θ/)s, sin(θ/)s ) Rotační vektor vužívá skutečnosti, že směrový vektor s definující osu rotace je jednotkový a má ted jen dva stupně volnosti. Rotaci ted můžeme vjádřit vektorem délk tři: v = (θs). Kvaternion jsou ajímavý matematický nástroj. Mei jejich důležité vlastnosti patří všechn 4 souřadnice mají ekvivalentní postavení, q a q popisují stejnou rotaci, v kvaternionech je relativně jednoduché interpolovat rotaci. Úlohu interpolovat rotaci (např. pro manipulaci v robotice nebo pro viualiaci v počítačové grafice) le snadno řešit pomocí kvaternionů. Metoda se naývá Spherical linear interpolation (SLER). Interpolujeme q do q a dostáváme q, interpolační parametr je t, α repreentuje celkový úhel, o který rotujeme (úhel mei kvaternion je polovina tohoto úhlu, absolutní hodnota skalárního součinu nám garantuje kratší rotaci): q = (q q )t q, (7) q = sin(( t)α) q + sin(tα) sin(α) sin(α) q, (8) cos(α/) = q q, (9) t <, >. () Srovnej Eric W. Weisstein. Quaternion. From MathWorld A Wolfram Web Resource. Interpolace v soustavě osa-úhel je rotace okolo dané os, přičemž úhel se mění lineárně od do θ. Rotační vektor je neredundantní a má dobrou topologii takže je hodně používán například v počítačovém vidění při odhadování rotace. Dobrá topologie namená, že blíké vektor (jejich rodíl je malý) repreentují podobné rotace. Rotační vektor spojitě repreentuje rotaci i v okolí nul, atímco repreentaci malých rotací odpovídají silně nespojité Eulerov úhl. ROBOTICS: Vladimír Smutný Slide 4, age 7

8 Rodriguesův vorec pro rotaci 5/ S p s Rodriguesův vorec pro rotaci vektoru: θ r = r cos θ + (s r ) sin θ + s(s r )( cos θ) r r Rotační matice repreentace osa-úhel: R = I cos θ + [s] sin θ + ss T ( cos θ) O kde [s] je skew smmetric (antismetrická) matice: [s] = s s s s s s Rodriguesův vorec umožňuje vpočítat otočený vektor r nebo jeho koncový bod při nalosti repreentace osa-úhel. Upravený vorec pak dává návod na výpočet rotační matice repreentace osa-úhel. Opačnou transformace je: ( ) trace(r) θ = arccos () r 3, r,3 s = r,3 r 3, () sin θ r, r, ROBOTICS: Vladimír Smutný Slide 5, age 8

9 Definice Eulerových úhlů 6/ precese nutace 3 rotace Eulerov úhl Matice R má devět koeficientů, ale má hodnost poue tři. Je ted redundantní repreentací, omeující podmínk jsou právě jednotkovost a kolmost vektorů n, t, b: n T t = t T b = b T n = n = t = b = Matici R le snadno konstruovat pomocí Eulerových úhlů. Otočme souřadnicový sstém O okolo os o úhel φ. Dostaneme O. R (φ) = R (θ) = R (ψ) = cos φ sin φ sin φ cos φ cos θ sin θ sin θ cos θ cos ψ sin ψ sin ψ cos ψ (3) (4) (5). Otočme souřadnicový sstém O okolo os o úhel θ. Dostaneme O. 3. Otočme souřadnicový sstém O okolo os o úhel ψ. Dostaneme O b b b. R = R (φ)r (θ)r (ψ) R = cos φ cos ψ cos θ sin φ sin ψ cos θ cos ψ sin φ cos φ sin ψ sin φ sin θ cos ψ sin φ + cos φ cos θ sin ψ cos φ cos θ cos ψ sin φ sin ψ cos φ sin θ sin θ sin ψ cos ψ sin θ cos θ (6) Trojice Eulerových úhlů dává jednonačně otočení v prostoru, poloha v prostoru nedává jednonačně trojici úhlů. Eistují další podobné definice, které mají podobné vlastnosti, ale jiné rovnice. Jestliže je dána matice R, le Eulerov úhl vpočítat porovnání prvků r 33, r 3, r 3. ROBOTICS: Vladimír Smutný Slide 6, age 9

10 Rotation Matri Resulting from Euler Angles Eulerov úhl podle definice v těchto přednáškách (Asada, Slotine): cos ϕ cos ψ cos ϑ sin ϕ sin ψ cos ϑ cos ψ sin ϕ cos ϕ sin ψ sin ϑ sin ϕ cos ψ sin ϕ + cos ϑ cos ϕ sin ψ cos ϑ cos ϕ cos ψ sin ϕ sin ψ cos ϕ sin ϑ sin ϑ sin ψ cos ψ sin ϑ cos ϑ Rotační matice definována úhl Yaw, itch, Roll použitými například v robotu CRS, ted rotujeme postupně okolo,, : cos α cos β cos α sin β sin γ cos γ sin α cos α cos γ sin β + sin α sin γ cos β sin α cos α cos γ + sin α sin β sin γ cos γ sin α sin β cos α sin γ sin β cos β sin γ cos β cos γ Jak vpočítat e námé rotační matice Eulerov úhl Mějme námou matici R (3 3) a smbolickou matici, která vnikla složením tří rotací definovaných třemi úhl. Máme najít tto tři úhl. Smbolická matice, která vnikla rotacemi okolo kolmých os má vláštní tvar, podobný maticím na výše uvedeném slidu. Máme ted řešit rovnici podobnou (ne nutně stejnou) jako rovnice (6) se třemi nenámými φ, θ, ψ. Nejdříve je nutné najít v smbolické matici prvek, který je funkcí jen jedné proměnné (v našem případe je to monom). Tento prvek (v příkladu na třetím řádku ve třetím sloupci) je ve tvaru bud ± cos nebo ± sin. Tento prvek může být přímo použit k naleení hodnot první nenámé. onamenejme, že obecně jsou v každém intervalu délk π dvě řešení. Kdž je určen první úhel, další mohou být určen pomocí funkce atan porovnáním elementů v řádku a sloupci, v kterém se nacháí pivot. Je nutné ošetřit situaci, kd pivot v konstantní matici má hodnotu blíkou ±. Jedná se o degenerovaný případ, kd v každém intervalu délk π máme jen jedno řešení. Dalším problémem v této situaci je, že prvk ve sloupci a řádku pivota nele použít pro určení dalších nenámých, protože konstantní matice obsahuje nul. Soustředíme se ted na blou podmatici a dosadíme již vpočtenou proměnnou. Zjistíme, že daná podmatice je funkcí součtu nebo rodílu nenámých a tak dostáváme obecně jednodimenionální množinu řešení. ři smbolickém řešení můžeme vjádřit tuto množinu. okud úlohu řešíme numerick, můžeme například jeden úhel afiovat (např. ). Obecně bchom měli vužít další omeení daná úlohou. V robotice tato situace tpick indikuje singulární řešení, např. požadavek kontinuit trajektorie nám napovídá, že máme jistit polohu ramene před daným bodem a požadovaný pohb po průchodu daným bodem. Jinou situací, kterou je nutné ošetřit, jsou problém spojené s nepřesností měření nebo výpočtů. T mohou například působit, že prvk v konstantní matici budou větší než a podobně. ROBOTICS: Vladimír Smutný Slide 7, age

11 Jiné sstém popisu orientace třemi úhl ROBOTICS: Vladimír Smutný Slide 8, age

12 Srovnání popisů rotace Sstém Smbol Ekvivalent ar. odmínk Matice rotace R 9 orthonormální Vektor os n, t, b R 9 vektor jednotkové, kolmé Eulerov úhl φ, θ, ψ aw, pitch, roll,... 3 Osa, úhel s, θ 4 jednotkový vektor Kvaternion q osa, úhel 4 jednotkový vektor Vektor rotace v osa, úhel 3 Sstém Výhod Nevýhod Užíván R snadné výpočt poloh redundantní Matlab toolbo n, t, b sroumitelný redundantní φ, θ, ψ neredundantní složitá topologie Mitsubishi, Staubli, CRS s, θ sroumitelný redundantní snadná interpolace q snadná interpolace redundantní ABB v dobrá topologie odhadování rotace neredundantní ROBOTICS: Vladimír Smutný Slide 9, age

13 Transformace souřadnic O p t b n p O u A t w v B Známe souřadnice (polohu) bodu v souřadnicovém sstému O : = p = u v w a hledáme polohu v souřadnicovém sstému O : = p = ravý dolní inde se většinou vtahuje k souřadnicovému sstému, ke kterému daný element patří. ravý horní inde říká, v kterém souřadnicovém sstému jsou souřadnice elementu vjádřen. Například počátek souřadnicového sstému (bod) O má souřadnice v souřadnicovém sstému : O = (,, ) T, ale v souřadnicovém sstému má souřadnice. O = O + t. Geometrick: Algebraick: řepsáno: O = O O + O A + AB + B. = p = t + un + vt + wb. Obrácená transformace: p = t + R p. p = R T t + R T p. ROBOTICS: Vladimír Smutný Slide, age 3

14 Homogenní souřadnice e h Homogenní souřadnice Zaved me homogenní souřadnice takto: Euklidovské (metrické) Homogenní - projektivní = = = /w /w /w neeistuje (nevlastní bod) = w = w Le snadno ukáat, že v homogenních souřadnicích: kde A je matice 44: Inverní matice: A = A = = A b, [ R t ] [ R T R T t ] ROBOTICS: Vladimír Smutný Slide, age 4

15 osloupnost transformací souřadnic O t p p p O t O Transformace přes více souřadnicových sstémů v euklidovských souřadnicích a v homogenních souřadnicích: = p = A A p = A ba b = p = t + R p = t + R (t + R p ) = A A A 3A... A n n n. (7) ROBOTICS: Vladimír Smutný Slide, age 5

16 asivní versus aktivní transformace asivní b b a a γ a b a = R a b (γ) b b a asivní transformace repreentuje měnu souřadnic. ROBOTICS: Vladimír Smutný Slide 6, age 6

17 asivní versus aktivní transformace - Aktivní a b γ a a b b = R a b ( γ) a = R a b (γ) a = R b a(γ) a b Aktivní transformace repreentuje pohb tuhého tělesa v nepohblivém souřadnicovém sstému. ROBOTICS: Vladimír Smutný Slide 3, age 7

18 asivní versus aktivní transformace - Srovnání b b a a γ asivní Aktivní. a b b b a a = R a b (γ) b b = R a b ( γ) a = R a b (γ) a = R b a(γ) a a γ a a b b Srovnejte pasivní a aktivní transformaci. ovšimněte si orientace úhlu γ. V robotice budeme nadále používat pasivní transformaci. ovšimněte si indeů u rotační matice. ROBOTICS: Vladimír Smutný Slide, age 8