Obsah
1 Úvod 7 1.1 Základní předpoklady a motivace 7 1.2 Postup výkladu látky . . . . . . . 8 1.3 Motivace: Řízení nelineární soustavy 9
2 Kinematika prostorových mechanismů 19 2.1 Rotační matice a její derivace . . 20 2.2 Homogenní transformace . . . . . . . 26 2.3 Popis orientace tělesa v prostoru . . . 32 2.4 Denavit - Hartenbergovy parametry . 36 2.5 Přímá úloha kinematiky 40 2.6 Shrnutí . . . . . . 42 2.7 Kontrolní otázky . . . 43
3 Diferenciální kinematika 45 3.1 Diferenciální kinematika a jakobián 46 3.2 Inverzní úloha kinematiky . . 48 3.3 Singulární stavy . . . . . . . . 52 3.4 Statika s využitím jakobiánu . 52 3.5 Manipulovatelnost 53 3.6 Shrnutí . . . . . . 56 3.7 Kontrolní otázky 57
4 Kinematika - příklady 59 4.1 Příklad: Kinematika rovinného manipulátoru RR 60 4.2 Příklad: Kinematika rovinného manipulátoru RT . 70 4.3 Příklad: Rovinný manipulátor RRR . . . . . . . . 72 4.4 Příklad: Prostorový manipulátor RRR (antropomorfní ruka) 76 4.5 Příklad: Prostorový manipulátor RR 79 4.6 Shrnutí . . . . . . 81 4.7 Kontrolní otázky . . . . . . . . . . . 82
4
5 Kinematika soustav s uzavřenou topologií 5.1 Metody řešení . . . . . . . . . . . 5.2 Příklad: Čtyřčlenný mechanismus 5.3 Shrnutí . . . . . . 5.4 Kontrolní otázky . . . . . . . . .
6 Dynamika prostorových mechanismů 6.1 Úvod . . . . . . . . . . . . . . . . . . 6.2 Základní metody a tvary rovnic . . . 6.3 Kinetostatika (inverzní úloha dynamiky) 6.4 Maticová metoda vycházející z LR2 6.5 Shrnutí . . . . . . 6.6 Kontrolní otázky . . . . . . . . . .
1 Dynamika - příklady 7.1 Příklad: Dynamika fyzikálního kyvadla . . . . . . . 7.2 Příklad: Dynamika rovinného manipulátoru RR . . 7.3 Příklad: Dynamika prostorového RR manipulátoru
8 Kinematika a dynamika neholonomních soustav 8.1 Holonomní a neholonomní vazby . . 8.2 Neholonomní soustavy . . . . . . . 8.3 Kinematika neholonomních soustav 8.4 Dynamika neholonomních soustav 8.5 Shrnutí . . . . . . 8.6 Kontrolní otázky . . . . . . . . .
9 Plánování trajektorie manipulátorů 9.1 Dráha a trajektorie (path and trajectory) . 9.2 . Pohyb v kloubových souřadnicích 9.3 Kontrolní otázky . . . . . . . . . . . . . .
10 Přílohy 10.1 Softwarové nástroje 10.2 Matematika . . . . 10.3 Označování veličin
10 Rejstřík
12 Použitá literatura
83 84 87 90 90
91 92 93 95
101 104 105
101 107 113 120
123 123 125 125 133 136 136
131 137 138 143
145 145 152 155
156
159
Předmluva
Skriptum "Kinematika a d ynamika mec hatronickýc h s ys-témů " je ur čeno především studentům stejnoj menné ho před -O mětu specializace Mec hatronika na Ústavu mec hanik y těles, mec hatronik y a biomec hanik y FSI v Brně, dále také st udentům předmětu "Mec hanika manipula čníc h zařízení " na Ústavu výrobníc h st rojů , s ystémů a robotik y a případně dalším zájemcům.
5
V textu jsou rozvíjen y znalosti základníc h před mětů Statika , Kinematika a Dynamika směrem k složitějším prostorovým mec hanismům a řešení problémů na po číta či . Předpokládá se dobrá znalost uvedenýc h předmětů a také lineární algebr y a základů programování.
Důležitou sou částí výkladu proble matik y jsou cvi čení prováděná na po číta či. Pro s ymbolické úprav y výrazů používáme Maple, výsledné rovnice pak numerick y řešíme v Matlabu. Z hlediska praktickýc h inženýrskýc h problémů jsou podstatné program y, které automatizují proces sestavení matematic ké ho modelu (rovnic) . V textu zmiňujeme použití Robotic Toolboxu pro Matlab [4] (kinematika a d ynamika otevřenýc h řetězců) a Matlab jSimulink jSimMec hanics [7 J.
V ypracování skript b y neb ylo možné bez kvalitní za hrani ční i české literatur y uvedené na konci textu, kterou lze zároveň doporu čit k dalšímu doplnění znalostí. Především jsem v yc házel ze skript [9, 21] a kni h [10, 16 , 18] .
Poznatk y z prostorové kine matik y a d ynam ik y uvedené v to mto textu můžeme považovat za výc hozí pro další studium řízení robotů a manipulátorů. Další zajímavou obl astí je modelování d ynamik y soustav s uzavřenou topologií a složitějšími
kinematickými dvojicemi ([15, 20] ) , která je v těc hto skriptec h pouze nazna čena v kap. 5. Cílem těc hto skript je také př ipravit student y na kvalifikovanou práci se so ftwarem t ypu SimMec hanics nebo Ada ms.
Jakékoli komentáře či připo mínk y k textu jsou vítán y.
V Brně , prosinec 2007, autor rtL/JrA--I1\l . J Email: grepl<Ofme.vutbr.cz � V / Web: http://www . umt . fme. vutbr . czrrgrepl/ .I
6
Kapitola 1
,
Uvod
Obsah kapitoly
1.1 Základní předpoklady a motivace .
1 .2 Postup výkladu látky . . . . . . . .
1.3 Motivace: Řízení nelineární soustavy . .
1 .3 . 1 Zadání úlohy . . . .
1 .3.2 Inverzní k inemat ika
1 .3.3 Polohové řízení
1 .3.4 Shrnu tí . . . .
7
8
9
9
1 1
1 1
1 8
V této úvodní kapitole popíšeme kontext a možné motivace studia prostorové kinematiky a dynamiky. Použití základních úloh dále vysvětlených v textu ukážeme na příkladu řízení konkrétnfho rovinného manipulátoru.
1.1 Základní předpoklady a motivace
Jedním z hlavních rysů mechatronického přístupu k návrhu pokročilých výrobků je komplexní pojetí technického objektu a jeho počítačové modelování . V simulačním modelu integrujeme mechanický, elektrický a řídicí subsystém a získáváme tak i informace o jejich vzájemných interakcích. Modelujeme-li mechanické a elektrické prvky jako systémy se soustředěnými parametry, pak lze jednoznačně říci, že sestavení modelu mechanické části je nejobtížnější.
Základní znalosti studentů z předmětů Statika, Kinematika a Dynamika je potřebné rozšířit a prohloubit a to především směrem k složitým prostorovým soustavám a jej ich modelování na počítači.
8 KAP. 1: ÚVOD
V praxi se pro řešení prostorových úloh kinematiky a dynamiky používají nástroje s implementovaným kinematickým a dynamickým formalismem, které tedy automaticky sestaví a vyřeší matematický model problému (např. programy MatlabjSimMechanics, MSC.Adams, ProMechanica a další) . Role uživatele spočívá v definici vlastností soustavy a v zadání vnějšího působení a počátečních podmínek. Efektivní práce a korektní zadání vstupů a vyhodnocení výsledků je možné pouze s dobrými znalostmi teorie kinematiky a dynamiky. Při práci v prostředí SimulinkjSimMechanics např. uživatel musí umět pracovat s rotační maticí a dobře rozumět vlastnostem vzájemně pootočených souřadnicových systémů.
Mimo používání uvedeného softwaru existuje řada úloh a situací , kdy je potřebné sestavit vlastní algoritmus řešení . Příkladem mohou být kinematické modely použité pro řízení implementované např. v mikrokontroleru nebo dynamický model soustavy, který je nutno řešit v reálném čase při řízení silně nelineární soustavy.
1.2 Postup výkladu látky
Základním předpokladem pro porozumění textu je mimo znalostí z mechaniky také zvládnutí maticového počtu a základů práce s programy Maple a Matlab. Vysvětlovanou látku je nutno prakticky procvičit na počítači.
Prvním krokem výkladu musí být otázka orientace tělesa v prostoru. Rotační matice a Eulerovy úhly by měly být známé již ze základního studia, v tomto textu tuto oblast tedy zopakujeme a rozšíříme.
Z pedagogického hlediska je poměrně snadno přístupná teorie přímé kinematiky otevřeného řetězce s využitím maticového počtu (homogenních transformací) . V příkladech upozorníme na možnost analytického nebo numerického řešení (násobení matic) .
Inverzní kinematika otevřeného kinematického řetězce je již obtížnější, uplatníme zde úvahy z diferenciální kinematiky. Ukážeme, že se jedná o problém řešení soustavy nelineárních algebraických rovnic a upozorníme na některé problémy (singularity, omezený pracovní prostor, dvojznačnost řešení apod.) . Analytické řešení je možné ve speciálních případech, důraz bude kladen na numerické řešení iterační metodou s využitím jakobiánu soustavy.
Řešení úloh kinematiky uzavřeného řetězce převedeme na problém otevřeného řetězce rozpojením smyčky ve vybrané vazbě. Ukážeme, že přímá i inverzní úloha zde mají podobný charakter , řeší se numericky iterační metodou s použitím odlišných jakobiánů.
1 .3 : MOTIVACE: ŘÍZENÍ NELINEÁRNÍ SOUSTAVY 9
Dále se budeme věnovat dynamice otevřených řetězců. Nezbytným vstupem ze strany studenta zde jsou znalosti Z Dynamiky, a to především metoda uvolňování a Lagrangeovy rovnice druhého druhu.
Vysvětlíme princip kinetostatiky, kterou lze použít pro řešení inverzních úloh dynamiky. Dále vyložíme maticovou metodu založenou na Lagrangeových rovnicích druhého druhu, která vede na soustavu ODE a umožňuje tak řešení přímé i inverzní úlohy dynamiky.
V další kapitole ukážeme na příkladech možnosti modelování kinematiky a dynamiky kolových vozidel a prostředků (neholonomní soustavy). Jde o vysoce praktickou oblast (automobilový průmysl) , ale i zajímavé a aktuální vědecké téma (řízení a modelování mobilních kolových robotů) .
Na závěr se zmíníme. o otázce plánování trajektorie v kloubových souřadnicích. Výsledky a důsledky výkladu použijeme při modelování a řízení široké škály praktických problémů.
Důležitou součástí výkladu jsou př{klady k jednotlivým oblastem. Doporučujeme studentům jejich porovnání s příslušnými teoretickými pasážemi a následně samostatné řešení na počítači.
1.3 Motivace: Řízení nelineární soustavy
V této motivační případové studii návrhu řízení nelineární mechanické soustavy se pokusíme ukázat roli statiky, kinematiky a dynamiky v kontextu implementace řídidho algoritmu a simulace systému na počítači. Při popisu řešení úlohy budeme odkazovat na jednotlivé kapitoly textu a pffklady. Otázka výkladu problematiky řízení a speciálně řízení s využitím inverze dynamiky je mimo rámec tohoto předmětu, př-tklad je však volen tak, aby byl maximálně pochopitelný.
1.3 .1 Zadání úlohy
Uvažujme rovinný manipulátor se dvěma stupni volnosti podle obr. Ll. Parametry soustavy jsou v tab. 1.1, uvažujeme působení tíhové síly.
Naším cílem bude navrhnout algoritmus řízení polohy koncového efektoru E v celém rozsahu pracovního prostoru (kružnice s poloměrem r) .
Budeme přitom předpokládat implementaci algoritmu v relativně málo výkonném zařízení, např. mikrokontroleru nebo DSP.
Z důvodů maximální jednoduchosti výkladu nebudeme modelovat elektrické pohony a předpokládáme, že působíme momentem. Vstupem do soustavy (řídicí
10 KAP. 1 : ÚVOD
hramce pracovního prostoru (L, - LJ
Obr. 1.1: Rovinný man ipulá tor RR (kr užn icí je naznačen výsek po žadovaného pracovního pros tor u)
veličinou) jsou tedy dva momenty v pohonech 'T. Dále uvažujme, že je soustava vybavena sensory polohy i rychlosti v pohonech. Dodejme ještě, že ramena manipulátoru považujeme za dokonale tuhá tělesa.
Základní specifikace úlohy řízení může znít takto:
• Na počátku je manipulátor v klidu, v poloze dané kloubovými souřadnicemi qo a odpovídajícími kartézskými souřadnicemi r� .
• Požadujeme přesun koncového efektoru do nové polohy r� v rámci pracovního prostoru (vymezeného mezikružím) . Na trajektorii přesunu ne-
Tab. 1.1: Parametry rovinného manipulátoru RR Parametr II Hodnota I Jednotka I délka Ll 0,750 m délka L2 0,500 m
hmotnost ml 2,3 kg hmotnost m2 1,8 kg
moment setrvačnosti ITl mlLI kg m2 12 moment setrvačnosti IT2
m2L� m 12 tlumení v kloubech b 1,5 Nm / S-I
1 . 3 : MOTIVACE: ŘÍZENÍ NELINEÁRNÍ SOUSTAVY 1 1
klademe žádné další požadavky.
Zpřesnění můžeme definovat takto:
• Požadujeme přesun a ustálení v čase tF = 2s s maximálním překmitem hodnoty iJi < 0.1 rad .
• Po ustálení musí být přesnost např. iJ'i < 0.02 rad.
1.3.2 Inverzní kinematika
Úkolem inverzní kinematiky pro polohy je při zadané poloze koncového efektoru E vektorem rE = [x, y]T vypočítat kloubové souřadnice pohonů q = [iJI, iJ2]T.
V příkladu 4.1 je podrobně popsán postup sestavení rovnic, dozvíme se zde, že v tomto konkrétním případě získáme snadno i analytický inverzní model v této podobě:
Přitom předpokládáme
a aby měla úloha řešení , musíme testovat, zda:
(1.1)
(1. 2)
(1 .3) (1 .4)
(1 .5)
Nyní tedy známe c{lové kloubové souřadnice, víme tedy kam se máme přesunout . Zbývá ještě otázka jak to provést .
1.3.3 Polohové řízení
1.3.3.1 Kinematické řízení
Nejjednodušším přístupem k řešení problému je zanedbání vlivu dynamiky manipulátoru. Použitelný je, pokud je požadovaný pohyb pomalý (dlouhý čas přesunu) a pohony dostatečně předimenzované.
12 KAP. 1 : ÚVOD
Předpokládejme na chvíli , že máme k dispozici kompletní řešení řízení pohonů včetně regulátorů (časté v průmyslové praxi) . Řídicí veličinou tedy nebude moment, ale natočení v kloubové souřadnici1 . Při přesunu do nové cílové polohy máme dvě možnosti :
1. Přímé zadání požadované hodnoty qF do regulátoru - samotný přesun plně necháme na pohonu a vestavěném regulátoru.
2. Naplánování hladké trajektorie - podrobně popsáno v kap. 9.2. Použijeme např. polynom 5. řádu, díky spojitému časovému průběhu zrychlení ))zvýšíme pravděpodobnost" , že vestavěné (a pro nás neznámé) řízení úlohu zvládne. Průmyslová řídící jednotka ale musí být schopna realizovat definovanou trajektorii.
Je zřejmé, že kinematické řízení bude fungovat jen pro "pomalé a předimenzované" soustavy. Při vyšší požadované rychlosti přesunu již nemusí vestavěný regulátor soustavu uřídit . Ekvivalentním efektem je ztráta kroku (a říditelnosti) krokového motoru.
Na druhou stranu, přístup je implementačně velmi jednoduchý, jedná se v podstatě o řízení v otevřené smyčce, je možná implementace i v relativně málo výkonném zařízení, např. mikrokontroleru.
1 .3.3.2 Nezávislé řízení jednotlivých pohonů (2xSISO)
Z pohledu dynamiky je manipulátor RR soustavou se dvěma stupni volnosti, jejichž dynamika je vzájemně spřažená. Matematicky je to vyjádřeno maticí B v rovnici 7.72 závislou na proměnné 'lJ2 (lineárně implicitní soustava ODE) . Jednoznačně z toho plyne, že chování v kloubu 2 má vliv na setrvačné síly v kloubu 1. Soustavu tedy musíme z hlediska řízení chápat jako MIM02 .
Nejjednodušším způsobem zpětnovazebního řízení manipulátoru je použití nezávislého PID regulátoru pro každý pohon. Vzájemný vliv jedné osy na druhu zanedbáme a považujeme jej za poruchu, se kterou se řízení musí vyrovnat. MIMO řízení tak převedeme na 2 x SISO.
Tento přístup bude mít dvě nevýhody:
• Pokud jsou vlivy spřažení podstatné, bude obtížné navrhnout regulátor kloubu 1, který bude dobře fungovat v celém rozsahu V2 .
lJednoduchým příkladem takovéto situace je použití krokového motoru. Tento je řízen nastavením jednotlivých kroků, tedy polohy. Předpokládáme přitom, že pohon je schopen předepsaný počet kroků provést. Pokud krokový motor přetížíme, dojde ke "ztrátě kroku" a pohon není říditelný.
2MIMO, angl. mu lti input - mu lti output systém, srovnej s S180 (single input - single output).
1.3: MOTIVACE: ŘÍZENÍ NELINEÁRNÍ SOUSTAVY 13
• Regulátor navržený pro jeden bod pracovního prostoru může fungovat velmi špatně v jiné části prostoru.
Druhá nevýhoda je jasně demonstrována na obr. 1 . 2 . PID byl nejprve nastaven (viz tab. 1 .2) pro počáteční polohu blízkou qo = [O, OlT. Na obr. nahoře je vidět uspokojivá odezva na skok .6.q = [0. 5,0. 5JT. Pokud však zadáme poč. polohu qo = [2.89, O.lJT a shodný skok, regulátor má již podstatně horší (neuspokojivé) chování znázorněné na obr. dole. Je zřejmé, že se zde jedná o vliv tíhové síly, která působí opačným směrem než v prvním případě.
Tab. 1 .2 : Parametry řízení rov inného manipulátoru RR po mocí dvou nezávislých P I D re gulátorů
Kp 350 I 180 D 38
Tmax 50 [NmJ Regulátor pohonu 1
Kp 350 I 1 00 D 20
Tmax 50 [NmJ Regulátor pohonu 2
Pokud požadujeme kvalitnější řízení, můžeme použít :
• Statickou kompenzaci vlivu tíhové síly - manipulátor bude v každé poloze ve statické rovnováze, kompenzace se ale musí v reálném čase v řídící smyčce počítat (nebo např. volit z tabulky) .
• Volbu různých hodnot PID podle polohy manipulátoru - této metodě se v angl. lit. říká gai n scheduling a také vyžaduje výpočty (tabulky) V reálném čase.
• Použít MIMO řízení - v dalším textu ukážeme možnost řízení pomocí inverze dynamiky, v lit . označované jako computed torque.
Poznamenejme ještě, že v případě, že chceme ověřit chování řízení při použití 2xSISO, nezbývá než provést pro dané podmínky simulaci. A abychom mohli simulovat, potřebujeme dynamický model (např. ve formě rovnice 7.72) .
1.3.3.3 Centralizované řízení (MIMO) - inverzní dynamika
Další cestou jak vylepšit vlastnosti řízené soustavy je použití metod pro MIMO systémy. V tomto příkladu použijeme metodu inverzní dynamiky, která patří do oblasti nelineárního řízení mezi metody globální linearizace. Na rozdíl od běžné linearizace, kdy nelineární soustavu nahrazujeme lineárním modelem platným v blízkém okolí pracovního bodu, zde provedeme pomocí určité transformace linearizaci v celém 'rOzsahu pmcovm1w prostoru.
14 KAP. 1 : ÚVOD
0.6
0.5
0.4
, 0.3 I ]. 0.2 ' -5 'o � 0.1 '" '3
--- &1 .".-----------------1 ___ &2 �
/7
o Vl -5 '�
ov o �-0.1 L---�--�----J---�----�---L----L----L--�--� � O 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 > t[s] 'a
I �:� r--_-�-_ ..,...�-:��_:_:_::_ :::::::::::::::::::::::1 :::::-::::: :1 0.4 I
I 0.3 � 0.2 0.1
O
_0.1 �--�--�----J---�----�---L----L---�--�--� O 0.5 1 1.5 2 2.5
t[s] 3 3.5 4 4.5
Obr. 1 .2 : Časo vý průběh nato čení v pohonech při řízení pomocí d vou nezá vislých P I D regulátorů ( vle vo: regulátor nasta vený na daný praco vní bod; vpra vo: odez va mimo nasta vený praco vní bod)
5
Vyjdeme z rovnice dynamiky manipulátoru 7.72 maticově zapsané jako 6.45:
B(q)q + C(q, q)q + g(q) = T (1.6)
1 .3 : MOTIVACE: ŘíZENÍ NELINEÁRNÍ SOUSTAVY 15
Podstata metody spočívá v definici vstupního vektoru momentu T takto:
T = B(q)r + C(q, q)q + g(q) ( 1 .7)
kde r je nový vstup do soustavy. Dosadíme-li rov. 1 .7 do rov. 1 .6, dostáváme po úpravě
q = r (1 .8)
Tento výsledek je velice důležitý. Původně nelineární systém s vzájemně spřaženou dynamikou jsme transformovali na lineární systém (!) druhého řádu s vzájemně nezávislou dynamikou (decoupled). Systém tedy lze chápat jako dva nezávislé řetězce dvou integrátorů.
Z hlediska řízení je podstatné, že nyní můžeme na systém aplikovat veškeré metody a nástroje dostupné pro lineární systémy.
Obvyklým postupem je volba
r - qd _ Koq-Klq
q _ q _ qd (1. 9)
( 1 . 10)
kde qd je požadovaný a q skutečný vektor kloubových souřadnic. Po úpravě dostáváme
( 1 . 1 1 )
Připomeňme, že matice Kl a Ko jsou diagonálními maticemi a rovnici 1 . 1 1 tedy můžeme chápat jako soustavu dvou nezávislých harmonických oscilátorů, jejichž vlastnosti můžeme libovolně volit.
Nastavíme-li Ko tak, aby rychlost regulace odpovídala našim požadavkům (odpovídá hodnotě n2 mechanického oscilátoru) a dále při
Kl = [2� O 1 O 2ýko; ( 1 . 12)
dosáhneme kritického tlumení (odpovídá hodnotě 8 = rl u mechanického oscilátoru) .
Na obr. 1 .3 vidíme výsledné schéma řízenÍ. Je zřejmé, že algoritmus inverzní dynamiky podle rov. 1.7 je nutné počítat v reálném čase, což klade vysoké nároky na použitý hardware.
Odezva obou pohonů při libovolných zadaných počátečních podmínkách (v celém rozsahu pracovního prostoru) a požadované relativní hodnotě 6q = [0.5,0.5]T odpovídá chování kriticky tlumeného harmonického oscilátoru a je znázorněna na obr. 1 .4.
1 6
Obr. 1 .3: Schéma ř ízen í
0.5 ,----..--.---::::;;r;;;;a----........ -...------.....,
N "
."
0.4
] 0.3 8. � -8 0.2 'f .� ;:1 Ol
0.5 1.5 2.5 t[sJ
3.5 4 4.5
KAP. 1 : ÚVOD
Obr. 1 .4: Č asový průběh natočení v pohonech př i řízení pomoc í inve rzní dynam iky
Výsledný model v prostředí Simulink s využitím SimMechanics pro mechanický subsystém je znázorněn na obr. 1 .5. Mimo výše popsaných bloků je v modelu v souladu s realitou použito omezení maximálního absolutního momentu. Pro větší přehlednost jsou ve schématu použity bloky Fro m a Go to, které nahrazují přímé propojení .
1 .3 : MOTIVACE: ŘÍZENÍ NELINEÁRNÍ SOUSTAVY
Regulátor
Control!er '------------( Idq)
From dq
Iq)
Fromq
,'j"'j\}---l---+I tau 1
Torque. í
q 1
Revolute B [O 01)1
tau
L-.-----l--el lo joint
Fromq 1
rorque.
� v;s:�����:�n9 \ �IL-_____ ______ �, __________________ � Viscous damping
and Sansors 1
1 7
Output q
!lKr;> _ 43 c:n � -= c:::z> � <=I � ... �� Q;;IlO C;EI --------=------==-� ou:a_IOD� __ a.,,<�� .-..o""""'lt"':S>�'i
Mechanical Branching Ba'
ICI-----l JoInt hitial CondiIIon
• 0._-.:11 __
Subsystém: - působení momentem v kloubu - viskózní tlumení v kloubu - sensor polohy a rychlosti pro řízení
Obr. 1 . 5: S imulační model s výpočtem inverzní d ynam ik y: d ynam ika man ipulátoru je řešena pomo cí SimMe chan ics , inverzní řízení použ ívá anal yt ický model ( rovn ice) d ynam ik y
18 KAP. 1 : ÚVOD
1 .3.4 Shrnutí
Při řešení úlohy polohového řízení manipulátoru RR jsme ukázali význam a použití následujících úloh:
• Inverzní kinematika - je potřebná vždy, když definujeme požadované chování v kartézských souř. a řídíme polohové.
• Přímá dynamika - je potřebná pro simulaci vlastností soustavy, např. chceme-li testovat nastavení regulátoru. Často postačuje automaticky3 sestavený model, tedy např. použití Matlab jSimMechanics.
� .... OE) ' : . . . . .. ' '- ,
• Inverzní dynamika - využijeme při pokročilejších metodách nzení (např. výše uvedená globální linearizace pomocí inverze dynamiky) , při implementaci již potřebujeme analyticky vyjádřené pohybové rovnice (kinetostatika, pokročilejší metody) .
• Plánování trajektorie kloubových souřadnic - použijeme při kinematickém, ale i vyšších přístupech pro definici hladké trajektorie manipulátoru bez skoků ve zrychlení (v tomto příkladu se tím detailně nezabýváme) .
Převážná část následujícího textu těchto skript je zaměřena právě na řešení těchto nejdůležitějších úloh a to pro prostorové mechanismy. Podobně jako jsme v tomto příkladu ukázali možnost řízení rovinného mechanismu, budeme po prostudování následující látky schopni pracovat s komplikovanými prostorovými soustavami s mnoha stupni volnosti (např. reálné průmyslové roboty) .
3Model je vytvořen pomocí softwaru na základě zadané topologie a geometrie soustavy. Uživatel sice nemá k dispozici pohybové rovnice, doba návrhu je však podstatně kratší. Prakticky se také odstraní možnost lidské chyby při odvozování analytického modelu.
Kapitola 2
Kinematika prostorových
mechanismů
Obsah kapitoly
2.1 R otační matice a její derivace
2 .1 . 1 Rotačn í mat ice v rov ině . . 2 .1 .2 Rotačn í mat ice v p rosto ru . 2. 1.3 De riva ce rotačn í mat ice 2 .1 .4 Dvě postupné rota ce . .
20
20 21 22
25
2.2 Homogenní transformace . . . . . . . . . . . . . . . 26
2.2 .1 Tran sfo rma ce v rov ině . . 2 .2 .2 Trans fo rma ce v p rosto ru . 2.2.3 De riva ce t rans fo rmačn í mat ice 2 .2.4 Použ it í d ife ren ciáln ích ope ráto rů
26 27
28
30
2.3 Popis orientace tělesa v prostoru . . . . . . . . . . . 32
2.3.1 Eule rov y úhl y. . . . . . . 32 2.3.2 Jednotkový vekto r a úhel 35
2 .3.3 Jednotkový kvate rn ion . . 36
2.4 Denavit - Hartenbergovy parametry . . . . . . . . 36
2.4.1 U rčen í pa ramet rů . . . . . . . . 37
2.4.2 Trans fo rmačn í mat ice . . . . . 2.4.3 De riva ce t rans fo rmačn í mat ice
2.5 Přímá úloha kinematiky
2.6 Shrnutí . . . . . . .
2.7 Kontrolní otázky .
38 39
40
42
43
20 KAP. 2 : KINEMATIKA PROSTOROVÝCH MECHANISMŮ
2.1 Rotační matice a její derivace
Rotační matice1je základním "nástrojem" prostorové kinematiky. Umožňuje přepočet souřadnic vektorů ve dvou vzájemně pootočených souřadnicových systémech. Dále je součástí homogenní transformace. Bez pochopení podstaty a vlastností rotační matice si lze také obtížně představit práci softwarovými nástroji.
2.1 .1 Rotační matice v rovině
Pro větší názornost a jednodušší pochopení výkladu provedeme nejprve odvození rotační matice R v rovině.
y
Obr. 2. 1 : Rotace dvou souřadnicových systémů kolem totožné o sy z (rotace v rovině)
Uvažujme vzájemné natočení dvou souřadnicových systémů v rovme (obr. 2. 1 ) . Známe-li souřadnice bodu M v systému b, můžeme jeho souřadnice v a snadno určit takto:
Xa = cos tpXb - sin tpYb Ya = sin ZPXb + cos tpYb
Tyto dvě rovnice přepíšeme do maticového tvaru
rM = [Xa] = [C?S tp - sin zpl [Xb] = R rM a Ya sm tp cos tp Yb ba b
(2. 1 )
(2 . 2)
(2.3)
kde r� a r� jsou polohové vektory bodu M v souřadnicích systému a resp. b, Rba je rotační matice2 ze systému b do a.
lTaké matice směrových kosinů nebo směrová matice, angl. rotational matrix . 2V české literatuře bývá často označována Cba.
2 . 1 : ROTAČNÍ MATICE A JEJÍ DERIVACE 2 1
2.1.2 Rotační matice v prostoru
Problematika orientace (natočení) tělesa v prostoru je podstatně složitější než v rovině, jak uvidíme v následujícím textu. Připomeňme, že orientace tělesa v rovině je dána jedním úhlem, zatímco v prostoru třemi.
N a rovinném příkladě jsme si názorně ukázali ortonormální transformaci3souřadnic vektoru rM ze souřadnicového systému b do systému a. Formálně stejný vztah platí i v prostoru:
kde
Rba = [Xba Yba ZbaJ ra = [xa Ya ZaJT,
(2. 4)
(2. 5)
(2.6)
Sloupcové vektory Xba,Yba, Zba E ]R3Xl mají význam os souřadnicového systému b viděného v systému a. Vektory jsou ortonormální , platí tedy pro ně např.:
XbaT Yba = 1 XbaT Xba = O
Důsledkem je významná vlastnost rotační matice:
RbaR�a = I R-1 RT ba = ba
kde I je jednotková matice.
2.1 .2 .1 Speciální tvary rotační matice
(2. 7)
(2.8)
(2. 9)
(2. 10)
Uveďme ještě speciální tvary rotační matice pro otáčení kolem jednotlivých souřadnicových os (např. matice Rx představuje rotaci systému b vzhledem k systému a okolo x):
O cos <p sin <p
- s�n <p] cos <p
(2. 1 1 )
30rtonormální transformace - báze obou souřadnicových systémů jsou kolmé a měřítko je zachováno (délka vektoru je v obou systémech stejná).
22 KAP. 2 : KINEMATIKA PROSTOROVÝCH MECHANISMŮ
(2. 1 2)
(2. 13)
Vidíme, že některé prvky jsou nulové a matice Rz je zobecněným tvarem matice v rov. 2.3. Pro úplnost dodejme ještě tvar matice při nulovém pootočení souřad. systémů vůči sobě (identita) :
R= [H �] (2. 1 4)
V případě obecné rotace kolem všech tří os jsou všechny prvky matice R nenulové.
2.1 .2.2 Opačná transformace
Matice Rba reprezentuje transformaci ze systému b do systému a. Opačnou transformaci Rab provedeme takto:
Vidíme tedy, že
ra = Rbarb R-1 R-IR ba ra = ba barb
Rb":ra = R�ara = Rab = rb
2 .1 .3 Derivace rotační matice
(2. 1 5) (2. 1 6)
(2. 1 7)
(2. 18)
Výše popsaný postup umožňuje pomocí matice R transformovat polohu bodu mezi dvěma vzájemně natočenými souřadnicovými systémy.
Derivací matice R dále získáme výpočetní vztahy pro rychlost a zrychlení.
2 . 1 : ROTAČNÍ MATICE A JEJÍ DERIVACE
2.1 .3.1 Rychlost bodu v rovině
23
Z pedagogických důvodů se vrátíme k rovinnému případu podle rov. 2 .3, kterou zderivujeme.
(2. 19)
(2.20)
Vzhledem k tomu, že systém b je pevně spojen s tělesem, je vektor r� v čase konstantní a tedy f� = O.
Poznamenejme hned, že
(2.21 )
kde vektor v� je rychlost bodu M vyjádřená v souřadnicích systému b (obecně je nenulová) . Mezi rychlostmi v obou systémech platí zřejmý vztah
Dostáme pak
kde
Rba = [-�s<p -�c<p] = [c<p -s<p] [� -cp]
+<pc<p -<ps<p s<p c<p <p O Rba = Rba!1ba !1 = [� -cp] = [ O -wz] ba <p O Wz O
2.1.3.2 Rychlost bodu v prostoru
(2.22 )
(2.23)
(2 .24)
(2 .25)
(2.26)
V prostoru bude výsledný vztah formálně stejný i když je situace komplikovanější. Vyjdeme nejprve z vektorového zápisu rychlosti bodu při rotačním pohybu
(2 .27)
kde wa = (wx, wy, wz)a je vektor okamžité úhlové rychlosti tělesa a indexy a značí, že všechny veličiny jsou vyjádřeny v systému a. Rovnici 2 .27 zapíšeme
24 KAP. 2: KINEMATIKA PROSTOROVÝCH MECHANISMŮ
maticově pomocí antisymetrické matice takto:
(2. 28)
Podobně bychom mohli rov. 2. 27 zapsat v souřadnicích systému b.
(2. 29)
Vzájemným srovnáním a použitím vztahů v předchozím textu dostaneme
(2.30)
Podstatné je, že pro základní rotace podle rov. 2.11-2.13 (které budeme používat především) platí
a můžeme tedy psát výsledný vztah
2.1.3.3 Zrychlení bodu v prostoru
(2.31)
(2.32)
Vztah pro zrychlení bodu při prostorovém pohybu získáme podobně jako jeho rychlost.
kde f = (E:n Ey, Ez ) je vektor okamžitého úhlového zrychlení tělesa.
(2.33)
(2.34)
(2.35)
(2.36)
(2.37)
(2.38)
2 . 1 : ROTAČNÍ MATICE A JEJí DERIVACE 25
2 .1.4 Dvě postupné rotace
Uvažujme ještě dvě po sobě jdoucí rotace tělesa. Budeme je chápat jako vzájemné transformace mezi třemi souřadnicovými systémy.
Mějme tedy systém c (pevně spojen s tělesem) , který je natočen vůči b a ten je natočen vůči a (nehybný systém, rám) . Analogicky předchozím úvahám můžeme psát:
ra = Rbarb rb = Rcbrc r a = Rbarb = RbaRcbr c = Rcar c
(2 .39) (2 .40) (2 .41)
Dostáváme tedy důležitý vztah pro rotaci mezi systémy a a c, zdůrazňujeme, že záleží na pořadí násobení rotačních matz"c.
(2 .42)
Vidíme, že celkovou rotaci lze vyjádřit pomocí dílčích rotací. Derivací rov. 2 .42 získáme vztahy podstatné pro výpočet (úhlové) rychlosti
a zrychlení dvou systémů c, a pomocí dílčích matic.
Rca = RbaRcb + RbaRcb Řca = ŘbaRcb + 2ŘbaRcb + RbaŘcb
Analogicky vztahům pro jednu rotaci platí:
Rca = Rcanc;u " 2 Rca = Rca(nca + Eca) a pro výpočet celkové úhlové rychlosti a zrychlení můžeme tedy psát:
T' nca = Rca Rca T" 2 Eca = Rca Rca - nea
(2.43)
(2.44)
(2.45)
(2.46)
(2.47)
(2.48)
Kombinací rov. 2 .42, 2 .43 a 2 .44 jsme tedy dosáhli podstatného výsledku: celkovou úhlovou rychlost a úhlové zrychlení umíme vyjádřit pomocí dílčích rotačních matic a jej ich derivací.
Úvahu rozvineme dále a vyjádříme celkovou úhlovou rychlost systému c vůči b, pro větší srozumitelnost znovu přidáme indexy definující ve kterém systému je daná rychlost vyjádřena.
T' neac = Rca Rca = = RCb T RbaT(RbanbabRcb + RbaRcbncbJ = = RCb TnbubRcb + nebe
(2.49)
(2 .50)
(2 .51)
26 KAP. 2 : KINEMATIKA PROSTOROVÝCH MECHANISMŮ
S použitím rov. 2.1.3.2 můžeme psát
Podobný postup provedeme pro zrychlení:
T" 2 Eeac = Rca Rca - ncac =
= RcbT RbaT(Rba(n�ab + Ebab)Rcb + 2 RbanbabRcbOcbc+ + RbaRcb(n�c + EebJ) - n�ac =
= n;ac + 2 nbuc nebe + Ebue + ECbe - n�ac
a dostáváme podobnou rovnici
(2.52)
(2.53)
(2.54)
Uvedené úvahy lze zobecnit na n postupných rotací, což má velký praktický význam při řešení úloh na počítači.
2.2 Homogenní transformace
Rotační matice definuje natočení tělesa v prostoru. Transformační matice definuje současně rotaci i translaci tělesa v prostoru.
2 .2 .1 Transformace v rovině
Nejprve se opět soustředíme na názorný rovinný případ. Uvažujme dva souřadnicové systémy podle obr. 2.2. Naším cílem bude vyjádřit polohu bodu M v systému a známe-li jeho souřadnice v systému b. Oba systémy jsou vůči sobě pootočeny a posunuty. Poznamenejme, že ačkoli se jedná o rovinnou úlohu, budeme j iž všechny vektory a matice psát v prostorovém tvaru (tedy všechna z = O).
Se znalostí rotační matice (rov. 2.4, 2.13) můžeme snadno psát:
kdy význam jednotlivých matic je tento:
-srp crp O
(2.55)
(2.56)
2 . 2 : HOMOGENNÍ TRANSFORMACE
Y\:::;:7'� \
x,
Obr. 2 .2 : Trans forma ce (posun a natočení) dvou souřadni cový ch systémů v rovině
Tuto rovnici lze maticově zapsat také tímto způsobem:
nebo názorněji takto:
-sep cep O O
2.2 .2 Transformace v prostoru
27
(2. 57)
(2 .58 )
Úvahy provedené v předchozím rovinném příkladu lze snadno zobecnit a definovat homogenní souřadnicí E ]R4Xl
r =
[�l a transformační matici E ]R4x4
T [Rba r�bl ba= O O O 1
(2 .59)
(2 . 60 )
kde r�b E ]R3Xl je souřadnice středu systému b vzhledem k systému a v systému a.
28 KAP. 2 : KINEMATIKA PROSTOROVÝCH MECHANISMŮ
Polohu bodu M ve vzájemně transformovaných (posunutých a natočených) systémech pak zapíšeme takto:
(2.61)
V předcházejících rovnicích splývá označení kartézské (IR3Xl) a homogenní (IR4X 1) souřadnice. Toto ne zcela korektní označování je přípustné, protože obě souřadnice se liší pouze "jedničkou" a jejich konkrétní význam lze snadno určit z kontextu. Další rozlišování by zbytečně komplikovalo matematický zápis.
2.2.2.1 Opačná transformace Tab Nyní vyjádříme opačnou transformaci ze systému a do b
což lze maticově v homogenních souřadnicích zapsat takto:
Vidíme tedy, že
ra = Tbarb, Tba = [o �a O
r;b]
[RT -RT rOb] rb = Tabra, Tab = O�a �a a
=* Tab = Tj;"; =I- T�a
Dokázali jsme tedy, že transformační matice není ortogonální.
2.2 .3 Derivace transformační matice
(2.62) (2.63) (2.64) (2.65) (2.66)
(2.67)
(2.68)
(2.69)
(2.70)
Podobně jako v případě rotační matice budeme derivovat transformační matici Tba a získáme vztahy pro rychlost a zrychlení vzájemně se otáčejících a posouvajících systémů a a b.
2 . 2 : HOMOGENNÍ TRANSFORMACE
2.2.3.1 Rychlost
Derivací vztahu pro polohu 2.61 získáme rychlost
Definujeme rychlostní matici E ]R4X4
a můžeme psát
Důkaz můžeme provést srovnáním 2.71, 2.22 a 2.23.
29
(2.71)
(2.72)
(2.73) (2.74)
(2.75)
Jako výsledek tedy dostáváme vztah pro rychlost bodu M pomocí homogenních souřadnic
2.2.3.2 Zrychlení
Zrychlení získáme derivací vztahu pro rychlost 2.76.
M .. M aa = Tbarb .. . • 2 •
Tba = TbaVba + TbaVba = TbaVba + TbaVba
(2.76)
(2.77) (2.78)
(2.79) (2.80)
(2.81)
(2.82)
30 KAP. 2 : KINEMATIKA PROSTOROVÝCH MECHANISMŮ
2 . Vba + Vba =
-
-
[ 2 . nba + nba OT
[n�a + Oba OT [2 . nba + nba OT
[n�a + Eba OT
n Ob n RT Ob + RT . Ob] ba V b - ba ;a V a ba V a n RT Ob n RT Ob + RT . Ob] ba ba V a - bO ba V a ba V a RT VOb] ba a
O RT aOb] ba a
O Definujeme matici zrychlení E }R4X4
A V2 V· [n�a + Eba R�aa�b] ba = ba + ba = OT O
a získáme výsledné vztahy pro výpočet zrychlení bodu M.
Tba = TbaAba
2.2 .4 Použití diferenciálních operátorů
(2.83)
(2.84)
(2.85)
(2.86)
(2.87)
(2.88)
(2.89)
(2.90)
(2.91)
(2.92)
(2.93)
Matice V a A lze výhodně určovat pomocí matice Dba, tzv. maticového diferenciálm1w operátoru. Konkrétní tvar matice závisí na typu vazby mezi dvěma tělesy. Typ vazby vymezuje možné vzájemné pohyby, které jsou popsány příslušnými kloubovými souřadnicemi (rotace nebo translace) . '
V technické praxi se často setkáme s konstrukčními uzly s jedním stupněm volnosti realizovaným jednou rotační nebo jednou translační vazbou. Při vhodném umístění souřadnicového systému pak můžeme použít tvary pro základní typy vazeb v tab. 2.1 .
2 . 2 : HOMOGENNÍ TRANSFORMACE
Tab. 2.1: Maticový diferenciální operátor pro základní pohyby
I základní pohyb II matice T matice D 1 O O x O O O 1
posuv ve směru osy x O 1 O O O O O O O O 1 O O O O O O O O 1 O O O O
[�
O O
�] [�
O O
�]
posuv ve směru osy y 1 O O O O 1 O O O O O O
[�
O O
�] [�
O O
!]
posuv ve směru osy z 1 O O O O 1 O O O O O O
[�
O O
�] [�
O O
�]
rotace okolo x c<.p -s<.p O -1 s<.p c<.p 1 O O O O O
[ cw O s<.p
�] [�1
O 1 �l rotace okolo y
+ 1 O O O O c<.p O O
�J O O O O
c<.p -s<.p O O O -1 O O rotace okolo z
s<.p c<.p O O 1 O O O O O 1 O O O O O O O O 1 O O O O
3 1
32 KAP. 2 : KINEMATIKA PROSTOROVÝCH MECHANISMŮ
Matice rychlosti a zrychlení pro vazbu s jedním stupněm volnosti pak s pomocí diferenciálního operátoru určíme podle následujícího vztahu
Vba = qbDba Aba = V ba 2 + éibDba
(2 .94) (2.95)
kde qb je časová derivace příslušné kloubové souřadnice a Dba je příslušný diferenciální operátor.
Vidíme tedy důležitý výsledek: derivaci matice T podle 2 . 73 jsme převedli na násobení této matice maticí D.
Konkrétní použití diferenciálních operátorů je zřejmé z příkladů 4. 1 a 4.2.
2.3 Popis orientace tělesa v prostoru
V předchozím textu jsme se seznámili s rotační maticí R E ]R.3X3 , která slouží k popisu orientace tělesa v prostoru a obecně má všechny prvky nenulové. Těleso má v prostoru tři rotační stupně volnosti, je tedy zřejmé, že devět prvků matice je vzájemně závislých (což ostatně vyplývá i z ortogonality matice) .
V následujícím textu se seznámíme s několika přístupy, které používají tři (minimální reprezentace) nebo čtyři proměnné a usnadňují tak popis orientace tělesa v prostoru. Nejdůležitější pro nás budou Eulerovy úhly.
2.3. 1 Eulerovy úhly
Uvažujme dva souřadnicové systémy a a b, které jsou na počátku totožné.
• nejprve systém b otočíme okolo osy Zb = Za O úhel 'l/J
• pak otočíme systém b okolo nové osy Xb o úhel -ď • a nakonec otočíme systém b okolo nové osy Zb o úhel cp
Souřadný systém b takto můžeme libovolně natočit (orientovat) v prostoru a daná orientace je popsána třemi Eulerovými úhly 'l/J , -ď , cp.
Maticově zapíšeme jednotlivé dílčí rotace kolem os z, x a Z takto:
(2 .96)
2 .3 : POPIS ORIENTACE TĚLESA V PROSTORU [1 O O ] I4 = O d) -srJ
O srJ crJ [C</J -s</J O] Rrj> = s</J c</J O
O O 1
Výsledná rotační matice vznikne vynásobením dílčích rotací
-c'!jJ s</J - s'!jJ crJ c</J -s'!jJ s</J + C1p crJ c</J
srJ c<j; Poznamenejme, že:
• záleží na pořadí dz1čích rotací
33
(2 .97)
(2.98)
s'!jJ srJ 1 -c'!jJ srJ (2 .99)
crJ
• v literatuře lze najít různé varianty Eulerových úhlů, které používají např. místo osy x osu y
• někdy se Eulerovými úhly rozumí obecně popis orientace pomocí tří po sobě jdoucích dílčích rotací s použitím libovolných os (celkem 12 různých možností)
2.3.1.1 RPY
V letectví se často používá varianta Eulerových úhlů nazývaná klonění, klopení a bočení, angl. Rol l , P itch , Yaw, zkratkou RPY.
Postupně otáčíme o úhly '!jJ, rJ , </J kolem os XYZ pevného souřadnicového systému. Jednoduše lze dokázat, že otáčení v pořadí XYZ kolem pevných os je ekvivalentní pořadí ZYX kolem okamžitých os a proto můžeme psát:
(2 . 100)
(2 . 101)
(2. 102)
34 KAP. 2 : KINEMATIKA PROSTOROVÝCH MECHANISMŮ
-s<J; c'lj; + c<J; s{} s'lj; c<J; c'lj; + s<J; sť} s'lj;
cť} s'lj; scjJ s'lj; + ccjJ sť} c'lj; 1
-ccjJ s'lj; + scjJ sť} c'IjJ cťJ c'IjJ
(2. 103)
2.3.1.2 Cardanovy úhly
Další varianta, kdy otáčíme postupně kolem os XYZ okamžitého souřadnicového systému. Angl. také Tait-Bryan angles.
2.3.1 .3 I nverzní úloha
V předchozích definicích jsme pracovali s danými třemi úhly a pomocí smluvené konvence získali výslednou rotační matici. Inverzní úlohou je pak při dané obecné matici
(2. 104)
a konvenci nalézt tři úhly. Použijeme-li konvenci RPY, pak lze inverzní úlohu pro ť} E (-7r /2; 7r /2)
vyřešit s pomocí rov. 2. 103 takto:
<J; = atan2(T21 , Tll) ť} = atan2( -r31 , v'-r�-2
-+-
r�-3)
'Ij; = atan2(r32 , r33)
(2. 105)
(2. 106 )
(2. 107)
Pro ostatní konvence (ZXZ, XYZ) můžeme řešení snadno odvodit podobným způsobem.
N a příkladu konvence RPY si nyní ukážeme hlavní nevýhodu definice orientace pomocí Eulerových úhlů. Pro hodnotu cťJ = O má matice 2. 103 tvar
RRPY = [ � -1
-scjJ c'lj; + ccjJ s'lj; scjJ s'lj; + ccjJ c'Ij; 1 [ O s('Ij; - <J;) c('Ij; + cjJ) ] c<J; c'lj; + scjJ s'lj; -ccjJ s'lj; + scjJ c'Ij; = O c('Ij; + cjJ) -s('Ij; - <J;)
O O -1 O O (2. 108 )
Je vidět , že je možné vypočítat pouze součet nebo rozdíl úhlů 'Ij; a cjJ a nelze jednoznačně určit hodnotu každého z nich.
Jinak řečeno, inverzní úloha má nekonečně mnoho řešení a tento stav označujeme jako singulární.
2 . 3 : POPIS ORIENTACE TĚLESA V PROSTORU 35
Příklad: Trojice úhlů [1/1 = 0.3, i} = 7r /2, cP = -0.4] a [1/1 = 0 .7, i} = 7r /2, cP = O] vede na shodnou matici R a při inverzní úloze nelze jednoznačně určit hodnoty jednotlivých úhlů.
Poznamenejme ještě, že nelze nalézt žádnou tří parametrickou reprezentaci orientace v prostoru, která by netrpěla singularitou. Klasické Eulerovy úhly ZXZ jsou např. singulární při si} = O.
2.3.2 Jednotkový vektor a úhel
V předchozím textu jsme ukázali, že tříparametrická reprezentace natočení vykazuje singularitu, kterou nelze odstranit . Pro některé úlohy, při kterých se těleso pohybuje jen v určitém intervalu úhlů lze použít např. konvenci RPY (-7r /2 < i) < 7r /2) , obecně je ale vhodné hledat j inou nesingulární reprezentaci.
Snadno lze dokázat, že libovolné natočení tělesa v prostoru můžeme vyjádřit jako otočení okolo pevného vektoru o určitý úhel.
Uvažujme jednotkový vektor w a úhel e. Rotační matici lze pak vypočítat pomocí maticové exponenciály takto:
00 (fW)k 1 1 R = exp(rW) = L -k'- = I + oe + - (00)2 + - (00)3 + . . . . 2 6 k=O
kde O je antisymetrická matice vektoru w a I je jednotková matice.
(2. 109)
(2 .110)
(2 . 1 1 1 )
Při inverzní úloze máme opět zadánu matici R podle rov. 2 . 104 a hledáme vektor w a úhel O.
kde
je stopa matice.
e (trace(R) - 1
) = acos 2
trace(R) = r11 + 7'22 + 7'33
(2 . 1 12)
(2. 1 13)
(2. 1 14)
36 KAP. 2: KINEMATIKA PROSTOROVÝCH MECHANISMŮ
Vidíme, že inverzní úloha nemá řešení při sin (j = O, kdy nedochází k rotaci a vektor w lze volit libovolně. Tento singulární stav odpovídá rotační matici podle rov. 2. 14.
2.3 .3 Jednotkový kvaternion
Nevýhody předchozí reprezentace řeší jednotkový kvaternion Q = {'TJ, E}, který vychází z vektoru a úhlu. Definujme
B 'TJ = cos 2
. B E = sm 2w
(2 . 1 15)
(2 .1 16)
kde 'TJ je skalární část a E = [Ex , Ey , EzJT je vektorová část kvaternionu. Přitom platí
'TJ2 + E; + E; + E; = 1 (2. 1 17)
Důležité je, že natočení dané dvojicí [w , Bl a [-w , -Bl vede na stejný kvaternion (což u reprezentace vektor/úhel neplaU) .
Známe-li prvky kvaternionu Q = {'TJ, E} můžeme určit odpovídající rotační matici takto: ['TJ2 + E; - E� - E; 2ExEy - 2'TJEz 2'TJEy + 2ExEz ]
R = 271Ez + 2ExEy 'TJ2 - E; + E; - E; 2EyEz - 271Ex
2ExEz - 2'TJEy 271Ex + 2EyEz 'TJ2 - E; - E� + E;
(2 . 1 18)
Při inverzní úloze pro zadanou matici R podle rov. 2 . 104 hledáme prvky kvaternionu.
1 'TJ = 2 vtrace(R) + 1
1 [Sgn(/32 - /23) /11 - /22 - /33 + 1 E = 2 sgn(/13 - /31 h/T22 - /33 - 1'11 + 1
sgn(/21 - '12h/r33 - /11 - /22 + 1
(2. 1 19)
(2 .120)
Vidíme, že výsledné řešení již na rozdíl od předchozích způsobů reprezentace netrpí singularitou.
2.4 Denavit - Hartenbergovy parametry
Volba polohy a orientace souřadnicových systémů soustavy a tím i tvar transformačních matic nejsou jednoznačné. Pro algoritmizaci úloh řešených na počítači potřebujeme obecnou systematickou metodu pro popis vzájemné polohy
2 .4 : DENAVIT - HARTENBERGOVY PARAMETRY 37
a orientace kloubů, pokud možno s využitím co nejmenšího počtu parametrů. V robotice se standardně používá pro popis otevřených kinematických řetězců (manipulátorů) Denavit-Hartenbergova konvence, která využívá čtyř parametrů.
2.4.1 Určení parametrů
Na obr. 2.3 jsou znázorněna dvě tělesa i a i - 1 propojená rotačními vazbami. Předpokládejme, že osy rotace kloubů jsou obecně mimoběžné.
�_--.::kIoub i+ I i
kloub i- I
Obr. 2.3: Denavit-Hartenbe rgov y pa ramet ry
Nejprve zjistíme polohu a orientaci souř. syst. i takto:
• najdeme osy rotace Zi a Zi-l a jejich společnou normálu4
• tím jsou dány body Oi a O�
• osu Xi zvolíme jako prodloužení společné normály a ve směru od kloubu i do kloubu i + 1
• osu Yi doplníme do pravotočivé souřadnicové soustavy
Nyní již můžeme zjistit čtyři DH parametry:
• ai - vzdálenost mezi body Oi a O�
• di - vzdálenost mezi body O� a Oi-l
• ai - úhel mezi osami Zi-l a Zi okolo osy Xi (kladný směr určen osou Xi)
• f)i - úhel mezi osami Xi-l a Xi okolo Zi-l
4Spojnice dvou nejbližších bodů přímek, také ozn. jako osa mimoběžek.
38 KAP. 2 : KINEMATIKA PROSTOROVÝCH MECHANISMŮ
Parametry ai a ai jsou vždy konstantní a závisí na konstrukci manipulátoru. DR parametry byly vytvořeny pro sériové manipulátory s rotačními nebo translačními vazbami, podle typu vazby je pak proměnnou
• iJi v případě rotační vazby a
• di - v případě translační vazby.
Můžeme tedy konstatovat, že tři ze čtyř DR parametrů jsou konstanty závisející na konstrukci soustavy a čtvrtý je proměnnou závisející na konkrétní poloze soustavy.
Z uvedeného plyne důležitý praktický závěr: DR parametry neumožní libovolnou definici počáteční polohy mechanismu. Důsledkem může být obtížnost definice parametrů pro zadaný mechanismus, zvláště jsou-li některé po sobě jdoucí osy rovnoběžné či se protínají. Názorně je tento problém demonstrován v příkladu 4. 5.
2.4.2 Transformační matice
Pomocí výše uvedených definicí můžeme zapsat transformační matice využívající DR parametrů.
[C" -SťJi O
�l Ti-1 i' = Sili Cili O
, O O 1 O O O
(2. 121)
T" " =
[t
O O
�l
Cai -Sai Sai Cai O O
(2. 122)
Jejich Vynásobením dostaneme celkovou transformační matici mezi kloubem í - 1 a í
(2. 123)
Poznamenejme ještě, že definice DR parametrů nemusí být jednoznačná a to v těchto případech:
• souř . syst . n popisuje koncový efektor (není zde kloub) a volba jeho natočení je proto libovolná
2 .4 : DENAVIT - HARTENBERGOVY PARAMETRY 39
• pokud jsou dvě po sobě jdoucí osy rovnoběžné, není definice společné normály jednoznačná
• pokud se dvě po sobě jdoucí osy protínají , volba Xi je libovolná.
2.4.3 Derivace transformační matice
Uvažujme ještě otázku derivace transformační matice Ti-I,i . Pro větší přehlednost výkladu zavedeme zjednodušené označení obou transformačních matic v rov. 2 . 123 . S uvážením, že matice TK = Ti',i (ai , ai) je vždy konstantní a matice Tq = Ti-I,i' závislá na proměnné q, můžeme psát :
i\-I,i = Tq TK + Tq � o
Ti-U = Tq TK Ti-I,i = Tq TK
(2 . 124)
(2. 125)
(2 .126)
Zjistili jsme tedy, že při výpočtu matice rychlostí a zrychlení derivujeme pouze T i-I,i' (7J i , di) ' která má jednoduchý tvar a pro kterou snadno nadefinujeme diferenciální operátor:
o -1 O O 1 O O O
pro rotační vazbu , O O O O
Dq = O O O O O O O O O O O O
pro translační vazbu. O O O 1 O O O O
Výpočet derivace transformační matice pak provedeme takto:
Ti-1 ,i = Tq TK = Tq Dq q TK •• •• 2 Ti-1,i = Tq TK = Tq Aq TK = Tq (Vq + q Dq) TK
(2 .127)
(2 . 128)
(2. 129)
40 KAP. 2 : KINEMATIKA PROSTOROVÝCH MECHANISMŮ
2.5 Přímá úloha kinematiky
Uvažujme prostorový otevřený kinematický řetězec podle obr. 2.4 , který se skládá z n těles, n prostorových vazeb a je popsán (n + 1 ) souřadnicovými systémy.
Obr. 2.4 : Obecný prostorový otevřený kinematický řetězec s n stupni volnost i
Předpokládejme, že všechny klouby (zobrazeny kolečky) jsou realizovány obecně prostorovou rotační nebo translační vazbou s jedním stupněm volnosti. Posunutí nebo natočení v i-té vazbě je popsáno souřadnicí qi . V kloubech jsou definovány souřadnicové systémy XiYiZi , přičemž poloha a orientace těchto systémů je jednoznačně dána kloubovými souřadnicemi ql , . . . , qi .
Přímou úlohu kinematiky pro polohy5nyní můžeme definovat takto:
• známe kloubové souřadnice q = [ql , . . . , qn]T E ]Rn a rozměry mechanismu
• hledáme kartézské souřadnice x E ]Rm (polohu a orientaci souřadnicového systému xnYnzn) koncového efektoru manipulátoru.
S využitím vztahu 2 .61 pro homogenní transformaci mezi dvěma souř. systémy můžeme snadno vytvořit transformační matici mezi systémem n a pevným rámem O:
n TnO (ql , " " qn) = TlO (ql )T21 (q2) . . . Tn,n-l (qn) = II Ti,i-l (2 . 1 30)
i=l
Matici T nO nazýváme transformační matice manipulátoru. Nyní již můžeme určit polohu bodu E
(2 . 131 ) 5Někdy říkáme dopředná úloha kinematiky, případně přímý kinematický model, angl. často
forward kinematics, d irect kinematics nebo forward kinematic model ( FKM) .
2 . 5 : PŘÍMÁ ÚLOHA KINEMATIKY 41
a orientaci systému n (koncového efektoru) danou rotační maticí, která je submaticí Tno podle rov. 2.60
R = Tno (1..3, 1..3) (2.132)
Přímou kinematickou úlohu pro polohy tedy můžeme formálně zapsat
x = f(q) (2.133)
Poznamenejme ještě, že:
• přímou úlohu pro otevřený řetězec je možné vždy vyřešit v uzavřeném tvaru
• konkrétní úlohu pak můžeme řešit numericky (rovnici 2.130 realizujeme maticovým násobením) nebo symbolicky (maticové násobení se provede symbolicky a teprve do výsledného tvaru dosadíme konkrétní hodnoty kloubových souřadnic)
• přímou úlohu pro rychlosti a zrychlení vyřešíme podobně jako pro polohy (matice VnO a Ano podle rov. 2.76 a 2.93).
42 KAP. 2 : KINEMATIKA PROSTOROVÝCH MECHANISMŮ
2.6 Shrnutí
• Zavedli jsme rotační matici Rba E }R3X3, která umožňuje provádět ortogonální transformace obecně různých vektorových veličin. Tzn., že můžeme vektorové veličiny (poloha, rychlost, síla, moment) viděné v jednom souřadnicovém systému b přepočítat do jiného systému a.
• Příklad: cyklista jede do kopce o sklonu <po Rychlost kola je v jeho lokálním souř. syst. rovna Vb = [v , 0, oV. Vnější pozorovatel (globální systém) však vidí , že se pohybuje rychlostí Va = Rba Vb = [V cos <p, 0, V sin <p]T (osa z svislá) .
• Rotační matice je ortogonální a má tedy některé zajímavé vlastnosti (viz také 10.2 .4) .
• Definovali jsme transformační matici Tba E }R4X4 . Umožňuje vyjádření rotace i translace dvou souřadnicových systémů. Obsahuje rotační matici a využívá homogenní souřadnice. Pro jednoduché typy kloubů (posuv, rotace) jsme definovali diferenciální operátory, které usnadňují výpočet časových derivací transformační matice. Pro složitější vazby lze diferenciální operátory odvodit nebo najít v literatuře.
• Rotační matice popisuje natočení souř. syst. v prostoru pomocí 9-ti závislých parametrů (redundantní) . Uvedli jsme několik dalších možností, které totéž provádí s menším počtem parametrů. Nejdůležitější jsou varianty Eulerových úhlů (tři parametry) , např. notace RPY používaná při popisu pohybu automobilů a letadel. V simulaci MBS, řízení manipulátorů a také např. počítačové grafice jsou podstatné nesingulární čtyřparametrícké popisy (kvaterniony).
• Denavit-Hartenbergovy parametry jsou standardně používány v robotice pro popis sériového (otevřeného) manipulátoru. Mimo jiné je využívá i Robotíc Toolbox pro Matlab (viz 10. 1 .4) .
• Ukázali jsme, že pomocí násobení dílčích transformačních matíc můžeme snadno vyřešit úlohy přímé kinematiky pro polohy libovolného sériového manipulátoru. Výpočet můžeme provést numericky nebo analyticky, přičemž analytické řešení existuje vždy a je snadné jej získat. Podobně pro rychlosti a zrychlení , zde pak využíváme diferenciálních operátorů a následně matic V nO a Ano .
2 .7 : KONTROLNí OTÁZKY
2.7 Kontrolní otázky
1 . Vysvětlete strukturu a nejdůležitější vlastnosti rotační matice. K čemu slouží rotační matice? Zapište tvar rotační matice pro samostatnou rotaci kolem os x, y a z.
2 . Uvažujte dvě po sobě jdoucí rotace souřadnicového systému. Jak vypočteme celkovou rotaci?
43
3. Co to je homogenní souřadnice? Jaký je vztah mezi trans for- '----.:,. mační maticí a rotační maticí? Zapište konkrétní tvar transformační matice pro daný jednoduchý vzájemný posun a ro-taci dvou souř. syst.
4. K čemu sloužÍ derivace transformační matice, jaké matice v té souvislosti používáme a jak a proč pracujeme s diferenciálními operátory?
5. Charakterizujte Eulerovy úhly a jejich souvislost s rotační maticí. Jak se liší jednotlivé varianty Eulerových úhlů? Popište problém se singularitou Eulerových úhlů. Charakterizujte reprezentaci "vektor a úhel" a "jednotkový kvaternion" .
6 . K čemu používáme Denavit-Hartenbergovy parametry? Jaký je jejich význam a hlavní výhody a nevýhody.
7. Popište řešení přímé úlohy kinematiky pro polohy pro otevřený kinematický řetězec. Diskutujte možnosti a vlastnosti numerického a analytického postupu výpočtu.
44 KAP. 2 : KINEMATIKA PROSTOROVÝCH MECHANISMŮ
Kapitola 3
Diferenciální kinematika
Obsah kapitoly
3.1
3.2
Difer enc iál ní k inem atik a a jak obián , .
3 . 1 . 1 Analyt ický jakob ián .
3. 1 .2 Geometrický jakobián
3. 1 .3 Numerický výpočet jakob iánu .
I nver zní úl oha k inem atiky
3.2 .1 Analyt ické řešen í . . . . . . . . . . . . . . .
3 .2 .2 Numerické řešen í pomoc í inverze jakob iánu
3.3 Singul ár ní stavy . . . . . . . .
3.4 Statik a s vy už itím jak obiánu .
3.5 Manipul ovatel nost . .
3.5. 1 Rychlostn í el ipso id
3.5.2 S ilový el ipso id
3.6 Shr nutí . . . . . . .
3.7 Kontr ol ní otázky .
46
46
46
47
48
49
49
52
52
53
53
55
56
57
V této kapitole popíšeme především numerické řešení inverzní úlohy kinematiky. Základním nástrojem je jakobián. S jeho pomocí můžeme také vyřešit úlohu statiky či posoudit manipulovatelnost a singularitu.
46 KAP. 3 : DIFERENCIÁLNÍ KINEMATIKA
3.1 Diferenciální kinematika a jakobián
3.1 .1 Analytický jakobián
Obecný vztah pro přímou kinematiku pro polohy 2 . 133 můžeme přepsat takto:
Provedeme-li derivaci rovnice podle času, dostáváme
d af dql ar dq2 af dqn -x = - - + - - + - -
dt aql dt aq2 dt . . . aqn dt
a výsledek přepíšeme
(3. 1 )
(3.2)
(3.3)
Dostali jsme vztah, kterým na základě znalosti rychlostí kloubových souřadnic určíme rychlosti kartézských souřadnic. Matici J E ]Rmxn
?!ll ?!ll ?!ll tR � oqn o 2 o 2 !1h J = aql aq2 aqn = [g� ax aX ] (3 .4) OQ2 . . . oqn atm atm atm aql aq2 aqn
nazýváme jakobián nebo Jacobiho matice a má, jak uvidíme dále, zcela zásadní význam při řešení inverzní úlohy a dalších problémů. Vidíme, že pro výpočet podle rov. 3 .4 musíme znát funkci f v uzavřeném tvaru a takto vypočtený J proto nazýváme analytický jakobián.
3 .1 .2 Geometrický jakobián
Pro vyjádření funkce f (q) v uzavřeném tvaru je potřebný symbolický formalismus, který je implementačně značně obtížnější než numerický. Proto byl nalezen jiný způsob výpočtu, který je lokální a vede na geometrický jakobián. Uvažujme těleso (angl. l i nk) podle obr. 3 . 1 , matici J rozdělme na subvektory
= [J pl =
[jPl jpn] J J . . .
R • • JR1 JRn (3.5)
3 . 1 : DIFERENCIÁLNÍ KINEMATIKA A JAKOBIÁN
o
Obr. 3 . 1 : Obecný pros torový o tevřený kinema tický ře tězec s n s tupni volnos ti
47
kde qdPi reprezentuje příspěvek kloubu i k translační rychlosti koncového efektoru a qJRi reprezentuje příspěvek kloubu i k úhlové rychlosti koncového efektoru.
Bez dalšího odvozování (lze najít v literatuře) uveďme způsob výpočtu itého sloupce geometrického jakobiánu:
[1::] -ZtO'-l] pro prizmatickou vazbu,
Zi-l x (r - ri- 1 )] pro rotační vazbu. Zi-l (3.6)
kde Zi-l je vektor osy rotace (i - l)-tého kloubu vyjádřený v souřadnicovém systému O, r = rn(ql , . . . , qn) je polohový vektor n-tého souř. systému (koncového efektoru) vyjádřený v souř. systému O a ri-l je polohový vektor i - I souř. syst. vyjádřený také v syst . O. Poznamenejme, že:
• problém parciálních derivací v rov. 3 .4 byl převeden na maticové násobení a je tedy možné použít numerický formalismus
• vektorové násobení realizujeme pomocí antisymetrické matice
• obecně se geometrický jakobián nemusí shodovat s analytickým.
Konkrétní odvození geometrického jakobiánu je ukázáno na příkladech.
3 .1 .3 N umerický výpočet jakobiánu
Pro úplnost zde zmíníme ještě implementačně nejjednodušší metodu numerického výpočtu jakobiánu. Vychází z velmi jednoduché myšlenky nahrazení
48 KAP. 3 : DIFERENCIÁLNÍ KINEMATIKA
parciální derivace diferencí.
(3 .7)
Je zřejmé, že metoda nevyužívá informace o charakteru soustavy a bude citlivá na volbu diference ó.qj '
3.2 I nverzní úloha kinematiky
Inverzní úlohu kinematiky pro polohy1pro otevřený kinematický řetězec definujeme takto:
• známe vektor kartézských souřadnic x E ]Rm (polohu a orientaci souřadnicového systému xnYnzn ) koncového efektoru manipulátoru a rozměry mechanismu a
• hledáme kloubové souřadnice q = [q1 , . . . , qn]T E }Rn .
(3.8)
Víme, že přímá úloha je vždy řešitelná v uzavřeném tvaru. Naproti tomu inverzní úloha je podstatně složitější. Jde o řešení soustavy silně nelineárních algebraických rovnic 2 . 133 a můžeme říci, že je ve většině případů není možné vyjádřit v uzavřeném tvaru (analyticky) , tj . nalézt funkci f-1 . Pak musíme použít iteračních numerických metod s využitím jakobiánu.
Matematické obtíže mají základ v těchto přirozených vlastnostech problému:
Omezený pracovní prostor - každý manipulátor má pouze omezený pracovní prostor, často obtížně popsatelného tvaru. Pokud požadujeme polohu a orientaci 'mimo tento pracovní prostor, řešení neexistuje.
Víceznačnost řešení - i velmi jednoduché manipulátory mohou zaujmout jednu definovanou polohu koncového efektoru dvěma či více způsoby (přitom se nejedná o redundantní manip.) . Příkladem je rovinný manipulátor se dvěma stupni volnosti v 4 . 1 .
lTaké nepřímá úloha kinematiky, případně inverzní kinematický model, angl. i nverse kinematies nebo i nverse kinematie model ( IKM) .
3 .2 : INVERZNÍ ÚLOHA KINEMATIKY 49
Redundantní manipulátor - pokud je délka n vektoru q větší než počet m požadovaných souřadnic x (což je v prostoru max. 6 ) , mluvíme o redundantním manipulátoru. Definovanou polohu a orientaci koncového efektoru je pak možno dosáhnout nekonečně mnoha způsoby.
Singulární stav manipulátoru - více v kap. 3 .3 .
3.2 .1 Analytické řešení
V některých jednodušších případech je možné inverzní kinematiku pro polohy vyjádřit analyticky. Takový stav je pochopitelně optimální. Příklad obtížnosti takového řešení spolu s nemožností řešení automatizovat ukážeme v úloze 4. 1 .
3.2 .2 Numerické řešení pomocí inverze jakobiánu
V případě složitějších soustav není možné nalézt analytické řešení. Totální diferenciál v rov. 3 .2 převedeme na diference a můžeme psát
�x = J�q �q = J-l�X
Inverzní úlohu pak realizujeme iteračním procesem
Xk = f(qk) Qk+1 = qk + J-l (X - Xk)
(3 .9) (3 .10)
. (3. 1 1) (3. 12)
kde x je požadovaná hodnota vektoru kartézských souřadnic a Xo je počáteční stav (odhad) vektoru kloubových souřadnic.
Příliš velké hodnoty rozdílu požadovaného x a skutečného x vektoru kartézských souřadnic mohou vést k oscilacím a špatné konvergenci řešenÍ. Metodu můžeme modifikovat například omezením maximální vzdálenosti požadované hodnoty ( cíle) od současné
Wk = X - Xk {Wk prol lwk I I � d, ek = d lI:kll jinde.
Qk+1 = Qk + J-1ek
(3 . 13)
(3. 14)
(3 . 15)
50 KAP. 3 : DIFERENCIÁLNÍ KINEMATIKA
kde d je maximální dovolená vzdálenost v metrice 1 1 1 1 , což je euklidovská norma2 vektoru.
m
I lwl l = L W; (3 . 16) i=l
Vidíme, že algoritmus inverzní kinematiky je použitelný lze-li spočítat inverzi jakobiánu, což je možné když:
• det(J) =1= O • matice J je čtvercová, tedy m = n.
Obě tato omezení lze překonat použitím pseudoinverze, příp. dalších metod, se kterými se seznámíme dále.
3.2.2.1 Pseudoinverze
Klasickou inverzi můžeme v algoritmu inverzní kinematiky nahradit pseudoinverzí3 , která je definována i pro obdélníkovou matici a umožňuje tak řešení úlohy pro redundantní manipulátor, kde n > m.
�q = JŤ�x JŤ = JT(JJT)-l
3.2.2.2 Transpozice
(3. 17)
(3 . 18)
Základní myšlenka je velice jednoduchá: použijeme transpozici4 jakobiánu místo jeho inverze v rovnici 3 . 10 . Rovnici doplníme o parametr a.
(3. 19)
Věnujme se nyní konvergenci algoritmu a volbě Q. Vzhledem k rov. 3 .9 můžeme psát
II = av = aJJTe (3.20)
kde v je vektor skutečného posunu manipulátoru v kartézských souřadnicích pokud bychom nepoužili měřítko a.
Z obr. 3 .2 je situace zřejmá a ilustruje volbu a tak, abychom posunuli ma-
2V Matlabu implementován příkaz normo 3Přesněji Moore-Penroseova pseudoinverze, někdy se používá termín zobecněná inverze. 4Z předchozího textu víme, že pro ortogonální matici platí A-I = A T, myšlenka nahrazení
inverze transpozicí proto není zcela bezdůvodná. Můžeme říci, že transpozice b ude fungovat tím lépe, "čím blíže b ude matice J k ortogonalitě" .
3 . 2 : INVERZNÍ ÚLOHA KINEMATIKY 51
Obr. 3.2 : S chéma volb y hodnot y a
nipulátor co nejblíže cílovému stavu Xd. Úhel mezi vektory e a v vypočteme známým vztahem a vektor u určíme
jako průmět vektoru e.
eTv cos <p =
I l e l l l lv l l v veTy
u = l Iu l l"M =W
Porovnáním s rov. 3 .20 dostaneme optimální hodnotu a
eTv eTJJTe a =
I Iv l 1 2 =
I IJJTel 1 2 Podmínku konvergence lze zapsat jako <p < 7r /2 , tedy
eTJJTe > O
(3 .21)
(3.22)
(3.23)
(3.24)
Víme, že aTa = I I a l 1 2 a pro nenulový vektor JTe je tedy konvergence zaručena.
3.2.2.3 Metoda tlumených nejmenších čtverců - DLS
Předchozí metody hledají takové Ď.q, které splňuje rovnici Ď.x = JĎ.q. Metoda Levenberg-Marquardtova5 hledá .6.q, které minimalizuje
(3.25)
kde A E IR je tlumící konstanta, která musí být dostatečně velká, aby vyřešila problémy v okolí singulárních stavů a zároveň dostatečně malá, aby byla zaručena rychlá konvergence.
Bez odvozování uvedeme výsledný vztah
(3.26)
5Také LMA (Levenberg-Marquardt algorithm) nebo damped least squares.
52 KAP. 3 : DIFERENCIÁLNÍ KINEMATIKA
3.3 Singulární stavy
Všechny výše popsané numerické metody vychází z rovnice
(3.27)
kde inverzi J-1 lze určit pouze pro regulární čtvercovou matici6 . V případě, že je matice J singulární, tedy
detJ = O (3.28)
mluvíme o kinematické singularitě manipulátoru. Praktickým důsledkem je snížení pohyblivosti manipulátoru a numerické obtíže při výpočtu. Malé rychlosti v kartézských souL mohou být zobrazeny jako velmi vysoké rychlosti v kloubových souřadnicích.
Poznámky:
• Singularity lze rozdělit na hraniční, které nastávají na hranicích pracovního prostoru (a nejsou tak problematické) a vnitřní, které se vyskytují uvnitř pracovního prostoru (a jsou vážným problémem např. při plánování trajektorie apod. ) .
• Uvedli jsme, že v singulární poloze nelze vypočítat inverzi jakobiánu a úloha není řešitelná. Z praktického hlediska je podstatné, že lze vypočítat pseudoinverzi (event. transpozici) a tím např. při výpočtu "uniknout" ze singulárního stavu. Vážné numerické obtíže ale zůstávají .
• V případě redundantm7w manipulátoru je matice J obdélníková a singularitu tedy nelze detekovat pomocí determinantu. Obecně můžeme říci, že: Kinematická singularita manipulátoru nastává tehdy, když dojde ke snížení hodnosti matice J.
3.4 Statika s využitím jakobiánu
Cílem statiky otevřeného kinematického řetězce je vyjádřit vztah mezi zobezněnými silami působícími na koncový efektor a zobecněnými silami v kloubech (síla pro prismatickou vazbu, moment pro rotační vazbu) .
Označme T E Rn vektor zobecněných sil v kloubech a 'Y E RT vektor sil působících na koncový efektor. Dimenze r :::; 6 v prostoru resp. :::; 3 v rovině.
60tvercová matice A rozměru n x n se nazývá regulární platí-li h(A) = n (hodnost matice, angl. ra n k) . V ostatních případech se je matice singulární. V Matlabu je implementován příkaz ranko
3 . 5 : MANIPULOVATELNOST 53
Nemusí být tedy shodná s počtem kartézských souřadnic použitých při řešení inverzní kinematiky, jak je dobře patrné z příkladu 4 . 1 .
Pro odvození statických vztahů použijeme princip virtuální práce. Elementární práci v kloubových souřadnicích vyjádříme jako
(3.29)
Práce vektoru vnějších zobecněných sil , = [f, Jl.lT, kde f jsou síly a Jl. momenty je
(3 .30)
kde J p a JR mají význam podle rov. 3 .5 . Z rovnosti obou prací vyplývá důležitý vztah
(3.31 )
3 . 5 Manipulovatelnost
3.5 .1 Rychlostní elipsoid
V kap. 3.3 jsme zjistili, že v singulární poloze může manipulátor ztratit pohyblivost v určitém směru, což může být nežádoucí . Z praktického hlediska je tedy jistě důležité najít způsob, kterým lze možnosti pohybu v kartézských souřadnicích měřit . Vhodným nástrojem je rychlostní elipsoid manipulovatelnosti7 .
Uvažujme rovnici
• T · 1 q q = (3 .32)
která popisuje jednotkové vektory rychlosti ležící na nadkouli8 . Při odvození budeme uvažovat obecnější případ redundantního manipulátoru kdy J E ]Rnxr , 'výsledky platí i pro neredundantní manipulátor popsaný čtvercovým jakobiá-, nem.
Uvažujme nyní jak se tato koule definovaná v kloubových souřadnicích zobrazí v prostoru kartézských souřadnic. Rovnici 3 . 17 lze přepsat takto
q = JŤx (3.33)
7Z angl. velocity manipu labi l ity el ipsoid. 8Nadkoule, angl. hypersphere, je zobecněním pojmu "koule" pro n rozměrů. Nadkoule pro
n = 2 je kružnice, pro n = 3 koule.
54 KAP. 3 : DIFERENCIÁLNÍ KINEMATIKA
a po dosazení do 3 .32 dostáváme
(3.34)
Uvážíme-li rov. 3 . 18 můžeme psát
(3.35)
což je kvadratická forma. Koule v souřadnicích q se bude zobrazovat do
q Tq = 1 :C(Ji)"'i = 1
Obr. 3 .3 : R ychlostní manipulovatelnost - transformace mezi kloubovým a kartézským souřadnicovým s ystémem
elipsoidu v souřadnicích v. Přitom víme, že:
• n rozměrů elipsoidu určíme jako odmocniny vlastních číse19 matice JJT (ne inverzní mat. )
• osy elipsoidu, který je obecně natočen v prostoru, určíme jako vlastní vektory matice J JT.
Příklad: elipsa (tj . elipsoid pro n - 2) se středem v počátku a rameny o délce a a b je popsána rovnicí
kterou můžeme maticově vyjádřít jako
[x y] [1/0a2 1Jb2] [�] = 1
xTAx = 1
(3.36)
(3.37)
(3 .38)
9 Vlastní čísla a vlastní vektory matice A vypočteme v Matlabu příkazem [V , D] =
eig(A) , kde V je matice vlastních vektorů a D je diagonální matice vlastních čísel.
3 . 5 : MANIPULOVATELNOST
Vlastní čísla matice A-I a odpovídající vlastní vektory pak jsou
Ul = [ l , O]T U2 = [O, l]T
55
(3.39) (3.40)
Vidíme, že odmocnina vlastních čísel odpovídá ramenům elipsy a vlastní vektory orientaci os .
Pokud chceme manipulovatelnost (pohyblivost) vyjádřit skalární hodnotou, použijeme Yoshikawovu míru, která souvisí s objemem elipsoidu
w(q) = Vdet(J(q)J(q)Y) (3 .41 ) a je w > ° mimo singulární polohy, kde w = O . V případě neredundantního manipulátoru (r = n) platí
w (q) = I detJ(q) I (3.42)
3.5.2 silový elipsoid
Stejně jako jsme uvažovali nad zobrazením rychlostí z prostoru ČJ. do v můžeme definovat manipulovatelnost pro say. Uvažujme tedy zobrazení sil v pohonech (kloubové souřadnice) r do ,.
rTr = 1 ,TJJT, = 1
(3 .43) (3 .44)
Tím jsme definovali silový elipsoid manipulovatelnosti, který charakterizuje schopnost manipulátoru vyvinout sílu v příslušných kartézských osách. Vlastnosti elipsoidu určíme z matice (J JTt 1 .
Srovnáním rovnic 3.44 a 3.35 vidíme, že rychlostní a silový elipsoid jsou dány inverzními maticemi. Z toho plyne důležitá vlastnost:
• osy obou elipsoidů jsou totožné
• dimenze odpovídajících os jsou v převráceném poměru.
Zjistili jsme tedy, že požadavky na rychlost pohybu a přenos síly jdou proti sobě. Pohybujeme-li manipulátorem v prostoru a zlepšuje se jeho možná rychlost, klesá schopnost přenést zatíženÍ.
56 KAP. 3: DIFERENCIÁLNÍ KINEMATIKA
3.6 Shrnutí
• Přímá úloha kinematiky pro polohy je popsána soustavou �'. c.i> nelineárních funkcí x = f (q) a pro otevřený řetězec je vždy analyticky řešitelná. Inverzní úloha kinematiky pro polohy je pak hledáním hodnot q pro zadané x a tedy řešením soustavy nelineárních algebraických rovnic. To není, jak známo, vždy možné provést analyticky (v uzavřeném tvaru) a musíme použít numerické metody.
• Pro numerické řešení soustavy nelineárních algebraických rovnic s výhodou použijeme jakobián. V textu jsme popsali postup vedoucí k získání analytického a geometrického jakobiánu. Numerické řešení podle rov. 3 . 10 je pak j iž snadné. Pro neredundantní manipulátor můžeme použít přímo inverzi matice, častěji však budeme pracovat s pseudoinverzí, která je použitelná i na obdélníkovou matici a tedy redundantní manipulátor a má lepší chování v okolí singulárních stavů.
• Inverzní úloha kinematiky pro rychlosti je dána rovnicí 3 .3 . Jedná se tedy o řešení soustavy lineárních algebraických rovnic. Úloha inverzní kinematiky pro rychlosti je tedy podstatně jednodušší než pro polohy.
• Odvodili jsme jednoduchý vztah pro statické řešení otevřeného manipulátoru pomocí jakobiánu. Na základě zadané síly působící na koncový efektor vypočteme statické zatížení všech pohonů.
• Dále jsme definovali rychlostní a silový elipsoid manipulovatelnosti a také skalární Yoshikawovu míru manipulovatelnosti. Tyto veličiny také charakterizují "vzdálenost" od singularity a proto se prakticky používají např. při plánování dráhy redundantních manipulátorů. Z více možných řešení vybíráme taková, která maximalizují manipulovatelnost .
3 .7 : KONTROLNÍ OTÁZKY
3 . 7 Kontrolní otázky
1 . Charakterizujte přímou a inverzní úlohu kinematiky pro polohy. O jaké jde matematické problémy? Jakým způsobem je můžeme řešit?
2. Popište algoritmus numerického řešení inverzní úlohy kinematiky pro polohy.
3. Jaká specifika má řešení inverzní úlohy pro redundantní ma- ,--..........: nipulátor?
57
4. Charakterizujte singulární stav manipulátoru, uveďte jednoduchý příklad a vysvětlete důvody singularity.
5. Jak lze formulovat úlohu statiky pro otevřený řetězec? Jakým způsobem ji vyřešíme?
6 . Definujte význam pojmu manipulovatelnost. Jak se liší elipsoid rychlostní a silové manipulovatelnosti? V jakém jsou vzájemném vztahu? Lze k vyjádření manipulovatelnosti použít skalární veličinu?
58 KAP . 3: DIFERENCIÁLNÍ KINEMATIKA
Kapitola 4
Kinematika - příklady
Obsah kapitoly
4.1 Příklad: Ki nemati ka rovi nné ho mani pulátoru RR . 60
4.1 . 1 Př ímá kinematika 60 4 . 1 .2 Inverzn í kinematika 63 4. 1 .3 Statika . . . . . . 67 4. 1 .4 Manipulovatelnost 68
4.2 Příklad: Ki nemati ka rovi nné ho mani pulátoru RT 70
4.2. 1 Př ímá kinematika 70 4.2.2 Inverzn í kinematika 71 4.2.3 Singulárn í poloha . . 72
4.3 Příklad: R ovi nný mani pulátor RRR . . . . . . . 72
4.3 .1 Př ímá kinematika 72 4.3.2 Anal yti cký jakobián 73 4.3.3 Manipulovatelnost 74
4.4 Příklad: Pros torový manip ulátor RRR ( antropo-morfní ruka) . . . . . . . . . . . . . . . . . . . . . . . 76
4.4.1 Př ímá kinematika pomo cí DR parametrů 76 4.4.2 Anal yti cký jakobián 77 4.4.3 Singulárn í poloha . . 77 4.4.4 Inverzn í kinematika 78
4.5 Příklad: Pros torový mani pulátor RR . . . . . . . . . 79
4.5.1 Př ímá kinematika pomo cí DR parametrů . . . . . 79 4.6 Shrnutí . . . . . . . 81
4.7 Kontrolní otáz ky . . 82
60 KAP. 4 : KINEMATIKA - PŘÍKLADY
4.1 Příklad : Kinematika rovinného manipulátoru RR
Cílem tohoto příkladu je ukázat několik různých přístupů k řešení problému přímé a inverzní kinematiky otevřeného řetězce.
Obr. 4. 1 : S chéma rovinného manipulátoru RR
4.1 .1 Přímá kinematika
4.1 .1 .1 Klasický přístup
S využitím základních trigonometrických funkcí snadno odvodíme následující přímý kinematický model pro polohy:
x = L2 Cl2 + Ll Cl Y = L2 Sl2 + Ll Sl
(4. 1) (4.2)
Derivací funkcí 4 . 1 a 4 .2 získáme přímý kinematický model pro rychlosti a zrychlení :
:i; = (-L2 812 - Ll Sl ) Jl - L2 Sl2 J2 (4.3)
Ý = (L2 Cl2 + Ll Cl ) Jl + L2 Cl2 J2 (4.4) .. . . 2 .. ' 2 X = (-L2 812 - Ll S l ) 191 - L2 Cl2 (191 + 192) - L2 812 192 - Ll C1 19l (4.5) .. . . 2 .. . 2 ii = (L2 C12 + Ll Cl ) 191 - L2 S12 ('!Jl + 192) + L2 C12 192 - Ll Sl 191 (4.6)
4 . 1 : PŘÍKLAD : KINEMATIKA ROVINNÉHO MANIPULÁTORU RR 61
4.1.1 .2 Maticová metoda - numericky
Nejprve definujeme potřebné matice. Jejich tvar vyplývá z typu vazby mezi tělesy.
[Cl -Sl O Ll Cl] T10 = Sl Cl O Ll Sl
O O 1 O O O O 1
[C2 -s2 O L2 C2] s2 c2 O L2 s2 T21 = � O 1 O
O O 1
V - [010 V�l 10 - OT O [ O -�1 O] 010 = �l O O
O O O
(4.7)
(4.8)
(4.9)
(4. 10)
kde v� je rychlost bodu B (počátek souř.s. 1) vyjádřená v souřadnicích systému 1 .
Alternativně můžeme derivaci transformační matice získat pomocí diferenciálního operátoru
VlO = �ID1O [O - 1 1 O DlO = O O O O
O O
I
O Ll O O O O
2 .. ÁlO = V 10 + t9l DlO Formální podobnost matic 4.7 a 4.8 bude platit i dále, proto:
V21 = �2D21 [O - 1 1 O D21 = O O O O
O O ] O L2 O O O O
2 .. Á21 = V 21 + '!92D21
(4. 11 )
(4.12)
( 4. 13)
(4. 14)
( 4 . 15)
(4.16)
62 KAP. 4 : KINEMATIKA - PŘÍKLADY
Podle rov. 2.73 pak platí
TlO = TlOVlO Ť21 = T21V21
a podobně pro zrychlení podle rov. 2.73 platí
ŤlO = TlOAlO Ť21 = T21A21
( 4 .17) ( 4. 18)
(4. 19)
(4.20)
Nyní můžeme snadno vypočítat polohu, rychlost a zrychlení koncového efektoru E vzhledem k pevnému souř.syst. O takto:
kde
r� = T20r� = TlOT21r� E . E Yo = T20r2 E .. E ao = T20r2
(4.21)
(4.22)
(4.23)
r� = [O O O l] T (4.24) . d . . . . T20 =
dt (TlOT21) = TlOT21 + TlOT21 = TlO191DlOT21 + TlOT21192D21 ( 4.25)
. . d · d . . .. . . . .
T20 = dt (T20) =
dt (TlOT21 + TlOT2d = TlOT21 + 2TlOT21 + TlOT21 (4.26)
Všechny potřebné matice máme vyjádřeny výše, můžeme tedy do nich dosadit konkrétní čísla a maticovým násobením získat numerické výsledky (např. v programu Matlab) . Je zřejmé, že uvedený postup lze snadno algoritmizovat pro libovolně složitý manipulátor. Postačuje pouze podle použitých typů vazeb definovat matice Ti,(i-l) a Di, (i-l) '
4.1.1.3 Maticová metoda - symbolicky
Dílčí matice relativních pohybů můžeme dále použít pro symbolické násobení (např. v programu Maple) .
T20 = TlOT21 = ( 4.27) [C12 -812 O L2 C12 + Ll Cl] 812 C12 O L2 812 + Ll 81 ( 4.28) - O O 1 O O O O 1
4 . 1 : PŘíKLAD : KINEMATIKA ROVINNÉHO MANIPULÁTORU RR 63
. . -'131 S12 - '132 S12
O O
-�l L2 S12 - Ll Sl .Jl - L2 S12.J2]
O '131 L2 C12 + Ll Cl '131 + L2 C12 '132 O O O O
(4.29)
Pokud nás zajímají pouze polohy, rychlosti a zrychlení, můžeme ještě matice symbolicky vynásobit podle rovnic 4 .21-4.23 a obdržíme rovnou
(4.30)
(4.31)
Porovnáme-li výsledky s rovnicemi 4.1 - 4.6, vidíme, že jsme jinou cestou dospěli ke stejným výrazům.
Přímé použití výsledných vztahů 4.30 -4.32 je výpočetně efektivnější než maticové násobení 4 .21-4.23. Musíme mít ovšem k dispozici nástroj pro symbolické násobení a úpravu výrazů (např. Maple) .
4 .1 .2 I nverzní kinematika
4.1.2.1 Klasický přístup
Rovinný manipulátor řešený v tomto příkladu je jedním z nejjednodušších a jeho inverzní kinematický model můžeme relativně snadno získat i v uzavřeném tvaru (analyticky) .
64 KAP. 4 : KINEMATIKA - PŘÍKLADY
Obr. 4.2 : S chéma rov inného man ipulátoru RR pro anal yt ické řešení inve rzn í k inemat ik y
4 . 1 . 2 . 1 . 1 Polohy V úloze inverzní kinematiky pro polohy hledáme hodnoty '!9l a '!92 při zadané poloze koncového efektoru x a y. Jedná o řešení soustavy dvou nelineárních algebraických rovnic 4. 1 a 4.2.
1 . Výpočet vzdálenosti c a test realizovatelnosti:
c > Ll + L2 Podmínky: c < ILl - L2 1
c = O /\ Ll = L2 2. Výpočet úhlu a :
nemá řešení nemá řešení 191 libovolné, 192 = ±1r
a = atan2 (y , x)
(4.33)
(4.34)
c = Ll + L2 191 = a, 192 = O, jedno řešení Podmínky: c = ILl - L2 1 /\ Ll > L2 191 = a , 192 = ±1r, dvě řešení
c = ILl - L2 1 /\ Ll < L2 '!9l = a - 1r, '!92 = ±1r, dvě řešení Není-li splněna ani jedna z podmínek, platí:
a výpočet pokračuje.
3. Výpočet 192 pomocí kosinové věty:
(4.35)
(4.36)
Vidíme, že úloha má dvě řešení schematicky naznačená na obr. 4 .2 .
4. 1 : PŘÍKLAD: KINEMATIKA ROVINNÉHO MANIPULÁTORU RR
4. Výpočet úhlu {3 pomocí kosinové věty:
L� = L� + c2 - 2cL1 cos {3 L2 - L2 + c2 {3 = arccos( 1 L
2 ) 2 l C 5 . Výpočet úhlu '!9l : Předpokládejme '!92 > O , pak
'!91 = Ci - {3
Tím je problém inverzní kinematiky pro rychlosti vyřešen.
65
(4.37)
(4.38)
(4.39)
4.1 . 2 . 1 .2 Rychlosti a zrychlení Řešení inverzní úlohy pro rychlosti je jednodušší než pro polohu. Jedná se totiž o soustavu lineárních algebraických rovnic 4.3 a 4.4, kterou přepíšeme do tvaru
[�] = [-1,'c::'; f,�:' -1,'c::'] [::] ( 4.40)
Podobně pro zrychlení :
4.1.2.2 Iterační řešení pomocí jakobiánu
4. 1 . 2 . 2 . 1 Analytický jakobián Pro výpočet analytického Jakobiánu použijeme přímo vztahy 4 . 1 a 4 .2 .
J = [ 8X 8X ]
8rh 8rh .!!JL .!!JL 8rh e�2
Po provedení parciálních derivací dostaneme Jacobián ve tvaru
J - [-L2 812 - Ll 81 -L2 8l2] 2x2 - L2 C12 + Ll Cl L2 C12
( 4 .42)
(4.43)
Pro určení singulárního stavu manipulátoru vypočteme determinant Jakobiánu:
( 4.44)
Vidíme, že singulární poloha je definována podmínkou '!92 = O.
66 KAP. 4 : KINEMATIKA - PŘíKLADY
4.1.2.2.2 Geometrický jakobián
J = [Jl J2] Jl = Zo x (r2 - ro ) = Zo x r2
J, = [! �1 �] [�:] _ [�J2] _
( 4.45) ( 4.46)
( 4.47)
Vyloučíme prvky příslušející souřadnici z a podle 4.45 dostaneme výsledný geometrický Jakobián
(4.50)
Vidíme, že je shodný s analytickým Jacobiánem, což ovšem obecně neplatí.
4.1.2.2.3 Ukázka konvergence inverzní úlohy Jakmile máme sestaven jakobián soustavy, můžeme aplikovat postupy popsané v kap. 3.2 .2 a iterační numerickou metodou řešit inverzní úlohu pro polohy.
Pro konkrétní rozměry Ll = 1 , L2 = 1 , počáteční natočení v kloubech q = [0.7, - 1 .4]T a cílovou polohu x = [0.5 , oJT porovnáme vlastnosti algoritmu s inverzí jakobiánu a s jeho transpozicí.
Na obr. jsou výsledky pro měřítko a = 0 .5 a toleranci é = le - 3. Vidíme, že inverze jakobiánu podstatně rychleji a přímo směřuje k cíli, zatímco pro transpozici je potřeba více kroků (3 vs. 47) .
Chování algoritmu s transpozicí můžeme vylepšit výpočtem a podle rov. 3.23. Celkové posouzení výhodnosti použité metody závisí na kontextu úlohy, např. použitém hardware. Nevýhoda vysokého počtu kroků u algoritmu s transpozicí může být vyvážena podstatně snadnějším výpočtem transpozice matice oproti její pseudoinverzi.
4. 1 : PŘíKLAD: KINEMATIKA ROVINNÉHO MANIPULÁTORU RR
0.8
0.6
I 0.4 >.
0.2
O O
-0.2 O 0.5
O Inverze J + Transpozice J
O O
+ +
1 x [ml
1.5
Obr. 4.3: Ukázka konvergen ce iteračn í metody inverzní k inemati cké úlohy (porovnání inve rze a t ranspoz ice jakob iánu)
4.1 .3 Statika
67
Definujme vnější sílu , = [Fx , Fy]T působící na koncový efektor E. Pro výpočet momentů 'í v pohonech využijeme jakobián J2x2 .
_ JT _ [( -L2 S12 - Ll Sl ) Fx + (L2 C12 + Ll Cl ) Fy] 'í - 2 2' -x -L2 S12 Fx + L2 C12 Fy
Dosazením konkrétních hodnot získáme momenty v obou pohonech.
(4.51)
Uvažujme dále, že na koncový efektor působíme i momentem v ose z, vektor zobecněných vnějších sil tedy bude mít tvar: , = [Fx , Fy , Mz]T . Je zřejmé, že musíme vytvořit nový jakobián, který zahrne i vliv změny kloubových souřadnic na natočení koncového efektoru vzhledem k pevnému rámu (označíme t.p) .
Jednoduchou úvahou nebo z matice 4.28 zjistíme závislost t.p na q:
t.p = ql + q2
Jakobián pak bude mít následující tvar: [-L2 S12 - Ll Sl J3X2 = L2 C12 7 Ll Cl
(4.52)
(4.53)
68 KAP. 4 : KINEMATIKA - PŘÍKLADY
:�� I --e-- Inverze J 1 �� - + - Transpozice J
+++.r, ++� '++.
"kt--kt-+.r, O ++,
"'+ -kt--kt-J tolerance
., 1 0 20 30 40 50
krok iterace Obr. 4. 4: vývoj ch yb y iterační metod y
a momenty v kloubech získáme
(4. 54)
4.1 .4 Manipulovatelnost
Nyní vyšetříme rychlostní a silovou manipulovatelnost. Budeme uvažovat neredundantní manipulátor rL = r = 2, použijeme tedy jakobián J2x2 z rov. 4. 43.
Elipsoid pro n = 2 se redukuje na elipsu, kterou snadno graficky vyjádříme. Z kap. 3. 5 víme, že rychlostní manipulovatelnost vyjadřujeme z vlastních čísel a vektorů matice A = J JT , zatímco silovou manipulovatelnost z matice inverzní A = (JJT)-l .
Další zpracování je na počítači snadné, výsledky jsou na obr. 4. 5 a 4.6. Můžeme si všimnout, že se skutečně potvrzuje teoretický poznatek a elipsy vyjadřující rychlostní a silovou manipulovatelnost jsou na sebe kolmé.
Čísla vedle jednotlivých elips na obrázku jsou hodnoty míry manipulovatelnosti podle Yoshikawy (rov. 3. 41) .
4 . 1 : PŘÍKLAD : KINEMATIKA ROVINNÉHO MANIPULÁTORU RR
0.5
I o >-
-0.5
-1
- 1 .5 -"I -1).5 I) 2 2.�
Obr. 4.5 : Elips y v yjadřující rychlostní manipulovatelnost p ro rovinný RR manipuláto r
1 .5
0.5
I O O >-
-0.5
-1
-1 .5 -0.5 O 0.5 1 1 .5 2
x [m]
Obr. 4.6: Elips y v yjadřující silovou manipulovatelnost p ro rovinný RR manipuláto r
69
70 KAP. 4 : KINEMATIKA - PŘÍKLADY
4.2 Příklad : Kinematika rovinného manipulátoru RT
V tomto příkladu ukážeme tvar transformační matice pro jednoduchý manipulátor s jednou rotační a druhou translační vazbou. Na obr. 4.7 je jeho schéma, pro označení kloubových souřadnic použijeme označení qi , i = 1 , 2 (ql odpovídá rotaci systému 1 vůči O; q2 odpovídá translaci systému 2 vůči 1) .
Yo
Obr. 4.7: S chéma rovinného Sestavíme dílčí transformační matice a po je- manipulátoru RT
jich vynásobení získáme přímý kinematický mo-del. Odvodíme jakobián a vyřešíme inverzní úlohu a zjistíme možnost singularit manipulátoru.
4.2 .1 Přímá kinematika
4.2.1.1 Maticová metoda - numericky
Z obrázku je zřejmé, že při počáteční poloze manipulátoru q = [O , OlT splývají počátky systémů 1 a 2 . Transformační matice sestavíme snadno do tohoto tvaru:
[Cl -Sl O O] T _ Sl Cl O O
10 - O O 1 O O O O 1
Podobně jednoduše zapíšeme diferenciální operátory:
(4.55)
( 4.56)
Polohu, rychlost a zrychlení bodu E koncového efektoru pak můžeme numericky vypočítat podle formálně stejných vztahů jako v příkladu 4 . 1 .
4 .2 : PŘÍKLAD: KINEMATIKA ROVINNÉHO MANIPULÁTORU RT 71
4.2.1 .2 Maticová metoda - symbolicky
Podobně jako V minulém příkladu můžeme násobení provést symbolicky a dostaneme následující výsledek:
Derivací výsledného vztahu pak získáme rychlost koncového efektoru:
4.2 .2 I nverzní kinematika
4.2.2.1 Analytický jakobián
(4.57)
(4.58)
Analytický jakobián vznikne parciální derivací funkcí 4.57 podle rov. 3 .4. Předpokládáme, že nás zajímá poloha manipulátoru a derivujeme proto podle proměnných x a y.
( 4 .59)
4.2.2.2 Geometrický jakobián
Geometrický jakobián získáme podobně jako v předcházejícím příkladu.
( 4.60)
(4.61)
(4.62)
Vidíme tedy, že obě metody vedou v tomto případě na shodný tvar jakobiánu.
72 KAP. 4 : KINEMATIKA - PŘÍKLADY
4.2 .3 Singulární poloha
Singulární polohu zjistíme z determinantu jakobiánu:
detJ = -b - q2 = O (4.63) a tedy
(4 .64) Je zřejmé, že se jedná o takovou polohu manipulátoru, kdy je koncový efektor E totožný s počátkem systému O.
4.3 Příklad : Rovinný manipulátor RRR
Tento příklad je rozšířením předchozího rovinného RR manipulátoru o další rameno. Ukážeme formální jednoduchost tvorby přímé kinematiky, transformační matice budou mít opět shodnou strukturu. Při tvorbě jakobiánu uvidíme, jak může být daný mechanismus redundantním nebo neredundantním manipulátorem v závislosti na volbě kartézských souřadnic.
Obr. 4.8: RRR rovinný manipulátor
4.3. 1 Přímá kinematika
Transformační matice můžeme převzít z příkladu 4 . 1 a mají formálně stejný tvar:
[
� -Si O Li �
l Ti,i-l = Si Ci O Li Si pro i = 1 , . . . , 3 (4.65) O O 1 O
O O O 1
4 .3 : PŘÍKLAD : ROVINNÝ MANIPULÁTOR RRR
Po symbolickém vynásobení dostaneme výslednou matici
[C123 -S123 O L3 C123 + L2 C12 + Ll Cl ] T - T T T - S123 C123 O L3 S123 + L2 S12 + Ll S l
30 - 10 21 32 - O O 1 O O O O 1
73
(4.66)
Nyní můžeme zapsat rovnice pro polohu koncového efektoru E a s použitím rov. 2 . 105 i pro jeho natočení <p:
x = L3 C123 + L2 C12 + Ll Cl Y = L3 S123 + L2 S12 + Ll Sl <p = atan2(T30 [2, 1J/T30 [1 , 1] ) = q1 + q2 + q3
4.3 .2 Analytický jakobián
(4.67) (4.68) (4.69)
Na základě odvozených rovnic pro polohu a orientaci koncového efektoru E můžeme vytvořit dva jakobiány, podle charakteru řešené inverzní kinematické úlohy.
poloha a orientace koncového efektoru - výsledkem je čtvercový jakobián r-L3 S123 - L2 S12 - Ll Sl -L3 S123 - L2 S12
J3x3 = L3 C123 + L2 C12 + Ll Cl L3 C123 + L2 C12 1 1
u kterého můžeme spočítat determinant
-L3 S123] L3 C123
1 (4.70)
(4.71)
Po úpravě zjistíme, že singulární stav manipulátoru nastane pro
pouze poloha koncového efektoru (bodu E) - v jakobiánu se objeví pouze derivace funkcí x a y a má tedy obdélníkový tvar (redundantní manipulátor)
-L3 S123 - L2 812 L3 C123 + L2 C12
-L3 S123] (4.72) L3 C123
74 KAP . 4 : KINEMATIKA - PŘÍKLADY
4.3.3 Manipulovatelnost
Z důvodů názornosti (na papíře) a porovnání s příkladem 4 . 1 použijeme pro výpočet a zobrazení manipulovatelnosti jakobián J2x 3 , budeme tedy pracovat s redundantním manipulátorem. Rychlostní manipulovatelnost bude vyjadřovat schopnost pohybu v osách x a y .
Manipulovatelnost vyjádříme stejně jako v příkladu 4.1 . Některé polohy manipulátoru jsou na obr. 4.9 . Délky ramen jsme volili tak, aby celková délka . byla shodná s rovinným RR manipulátorem. Při porovnáním s obr. 4.5 vidíme, že na okrajích a hranicích pracovního prostoru je manipulovatelnost malá nebo nulová (stejně jako u RR manipulátoru) . Naopak, v oblasti kolem počátku souřadnicového systému manipulovatelnost značně vzrostla. Mezi oběma extrémy jsou hodnoty mírně vyšší. Požadovanou polohu koncového efektoru [x , yJ je
1 .5
0.5
o
-1 �� __ � ____ � ____ � ____ � ____ -u -0.5 o 0.5
x [mJ 1 .5 2
Obr. 4.9: El ipsy vyjadřují cí ry chlostní man ipulovatelnost p ro rov inný RRR man ipuláto r
možné u redundantního manipulátoru realizovat nekonečným počtem různých řešení [q1 , Q2 , Q3] . Na obr. 4 . 10 je uveden jednoduchý příklad dvou možných řešení se znázorněnými elipsami rychlostní manipulovatelnosti . Vidíme, že elipsy mají jiný sklon i velikost a z pohledu rychlostní manipulovatelnosti tedy obě řešení nejsou rovnocenná.
4 .3 : PŘíKLAD: ROVINNÝ MANIPULÁTOR RRR
o
-0.2
-0.4
I -0.6 >.
-0.8
-1
-1 .2
0- - - - - - - - - - - - - - - -G-----J'---+<
o 0.2 0.4 0.6 0.8 x [m]
1 .2 1 .4 1 .6
Obr. 4 . 10 : Porovnán í dvou různý ch řešen í inverzn í úloh y pro poloh y manipulátoru RR (elipsami v yznačen y př íslušné r ychlostn í man ipulovatelnost i)
75
76 KAP. 4 : KINEMATIKA - PŘíKLADY
4.4 Příklad : Prostorový manipulátor RRR (antropomorfní ruka)
Předchozí příklady byly rovinné a sestavení dílčích transformačních matic tedy bylo velice snadné.
Tento příklad je velmi jednoduchou prostorovou úlohou, na které však opět ukážeme některé teoretické poznatky. Manipulátor má tři stupně volnosti (obr. 4. 1 1) , můžeme tedy v rámci pracovního prostoru libovolně definovat polohu koncového efektoru.
Sestavení dílčích transformačních matic pro prostorové mechanismy již může být dosti ná- L, ročné na představivost. Pro definici mechanismu proto použijeme Denavit-Hartenbergovy parametry s nimiž pak např. velmi snadno sestavíme model v Robotic Toolboxu pro Matlab (kap. 10. 1 .4) . Obr. 4. 1 1 : S chéma prostoro-
Následně na základě těchto parametrů jedno- vého manipulátoru RRR duše sestavíme dílčí transformační matice, zís-káme jakobián a vyřešíme přímou i inverzní úlohu kinematiky a zjistíme možné singulární stavy manipulátoru.
4.4. 1 Přímá kinematika pomocí O H parametrů
Transformační matice sestavíme pomocí Denavit-Hartenbergových parametrů postupem popsaným v kap. 2.4. Na obr. 4 . 1 1 jsou příslušným způsobem zakreslené souř. systémy. Odpovídající DH parametry jsou v tab. 4 . 1 .
Tab. 4 .1 : Denavit-Hartenbergovy parametry pro prostorový RRR manipulátor
těleso a · 1. ai {Ji di 1 7i /2 O ql Ll 2 O L2 q2 O 3 O L3 q3 O
4 .4 : PŘÍKLAD : PROSTOROVÝ MANIPULÁTOR RRR (ANTROPOMORFNÍ RUKA) 77
Dílčí transformační matice pak mají podle rov. 2 .123 následuj ící tvar:
[CI O 81
{I
] TlO = 81 O -Cl (4.73)
O 1 O O O O
[
� -82 O L, C'] 82 C2 O L2 82 (4.74) T21 = � O 1 O
O O 1
a výsledná matice vypadá taktol :
[CI�3 -C1823 81 cl (L,c, + L3c23) ]
T - 8lC23 -81 823 -Cl 81 (L2C2 + L3C23) (4.75) 30 - O Ll + L282 + L3823 823 C23 O O O 1
Tím máme odvozen přímý kinematický model pro polohy.
4.4.2 Analytický jakobián
Analytický jakobián získáme parciální derivací funkcí x, y, z a bude mít násle
dující tvar:
-Cl (L2 82 + L3 823) -81 (L2 82 + L3 823) ( 4.76)
L3 C23 + L2 C2
4.4.3 Singulární poloha
Singulární poloha manipulátoru nastane tehdy, když je determinant jakobiánu
roven nule:
Pro L2, L3 =J. O můžeme psát:
83 (C2 L2 + C23 L3) = O
Řešením jsou dva případy, kdy:
(4.77)
( 4.78)
lVýsledný vztah je odvozen v programu Maple a ručně upraven pomocí součtových trigonometrických vzorců.
78 KAP. 4 : KINEMATIKA - PŘÍKLADY
• S3 = O: stav, kdy je rameno manipulátoru "nataženo" podobně jako v příkladu 4 . 1
• C2 L2 + C23 L3 = O: odpovídá stavu, kdy koncový efektor E leží na ose rotace Za - viz obr. 4 .12 .
�' ��
: ----
I ������ q/ Xo
Obr. 4 .12 : Singulární poloha prostorového manipulátoru RRR
Zjistili jsme tedy, že singulární poloha manipulátoru je funkcí pouze q2 a q3 a nezávisí na Ql .
4.4.4 I nverzní kinematika
Postup řešení úlohy inverzní kinematiky pro polohy zde nebudeme uvádět , vychází totiž zcela z předchozích příkladů:
numerické řešení pomocí inverze jakobiánu - provedeme formálně shodným způsobem, jakobián je čtvercový.
analytické řešení - úlohu rozdělíme na rotaci roviny okolo osy Za a pohyb v této rovině. Řešení tak spočívá v použití funkce atan2 a postupu pro rovinný manipulátor RR (příklad 4 . 1 ) .
4 .5 : PŘÍKLAD: PROSTOROVÝ MANIPULÁTOR RR 79
4.5 Příklad : Prostorový manipulátor RR
Cílem tohoto příkladu je především upozornit na neuniverzální povahu DenavitRartenbergových parametrů. Otevřený kinematický řetězec totiž nelze definovat v libovolné počáteční poloze, což klade zvýšené nároky na představivost.
b)
Obr. 4 . 13 : Prostorový manipulátor RR: a) základIÚ schéma; b) poloha mechanismu a souřadnicových systémů při definici pomocí Denavit-Hartenberbových parametrů; c) poloha manipulátoru při zavedení offsetu a nenulových kloubových souřadnicích ql , q2
4.5.1 Přímá kinematika pomocí O H parametrů
Dílčí transformační matice sestavíme s využitím Denavit-Rartenbergových parametrů. Na obr. 4 . 13 je znázorněno schéma mechanismu (a) , jehož kinematiku budeme modelovat . Z předchozího textu víme, že DR parametry nejsou zcela univerzální, např. neumožňují definici určitého mechanismu v libovolné počáteční poloze.
Toto omezení platí i v tomto případě. Abychom mohli použít DR parametry, musíme mechanismus natočit do polohy 4 .13(b) . Odpovídající DR parametry jsou pak uvedeny v tab. 4.2 . Všimněme si také, že jsme ztotožnili střed souř. syst. O a 1 , oba klouby tedy na schématu splývají . Z konstrukčního hlediska se j istě jedná o podstatnou změnu, z hlediska kinematiky jsou ale obě soustavy ( a) a (b) totožné.
80 KAP. 4: KINEMATIKA - PŘÍKLADY
Tab. 4.2: Denavit-Hartenbergovy parametry pro prostorový RR manipulátor
těleso (Xi a ' � 1Ji di 1 7f /2 O ql O 2 O L2 q2 Ll
Nyní j iž můžeme snadno pomocí rov. 2 . 123 zapsat dílčí transformační matice:
[
C' O Sl
�]
T10 = Sl O -Cl O 1 O O O O
(4.79)
[
C' -S2 O L' C'] S2 C2 O L2 S2 T21 = � O 1 Ll
O O 1 ( 4 .80)
Celková transformační matice vznikne vynásobením dílčích matic a zavedením offsetu
7f 7f ( 4.81) ql = ql + '2 q2 = q2 + '2 a odpovídá obr. 4 .13 (c) .
[ S,S, SlC2 Cl slL,s, + CIL,
]
T _ -ClS2 -ClC2 Sl -clL2S2 + SILI (4 .82) 21 - O L2c2 C2 -S2
O O O 1
Tím máme sestaven přímý kinematický model pro polohy.
4 .6 : SHRNUTÍ
4.6 Shrnutí
• Teoretický výklad podaný v předchozích kapitolách je z pedagogického hlediska více než vhodné doplnit jednoduchými příklady. Tuto kapitolu textu je proto třeba chápat jako podstatnou součást textu a podle toho k ní přistupovat.
8 1
�...,. " o . o.f); � ,
• I zde platí podobně jako v matematice, že první polovinou úspěchu je pochopení látky, druhou pak její procvičení (ano - dril) . Doporučujeme si zde uvedené příklady samostatně přepočítat a v nejlepším případě přidat i jiné vlastní. Samozřejmostí je řešení na počítači.
• Na několika příkladech (4. 1 , 4 .2) jsme ukázali současně analytický i numerický přístup k řešení přímé kinematiky. Při řešení praktické úlohy se rozhodujeme podle nároků na rychlost a přesnost výpočtu, podle použitého hardware (PC vs. mikrokontroler) a také podle doby vymezené pro řešení .
• Detailně jsme rozebrali možnost analytického řešení inverzní úlohy u patrně nejjednoduššího mechanismu RR v rovině (4. 1 ) . Dobře jsou zde patrné některé charakteristické problémy inverzní úlohy (omezený pracovní prostor , dvojznačnost řešení) .
• Na obr. 4 .3 j e názorně vidět rozdíl mezi konvergencí inverzní úlohy s použitím inverze a transpozice jakobiánu. Posouzení vhodnosti metody opět závisí na kontextu úlohy.
• V příkladech 4 . 1 a 4 .3 jsme vypočítali a vykreslili elipsy manipulovatelnosti. Dobře můžeme srovnat rychlostní a silovou manipulovatelnost a také vidíme zvýšení manipulovatelnosti u redundantního RRR manipulátoru.
• U dvou prostorových příkladů 4.4 a 4 .5 jsme dílčí transformační matice sestavili s použitím DR parametrů. Na druhém z příkladů je dobře vidět nemožnost libovolné definice počáteční polohy mechanismu při definici DR parametrů.
82 KAP. 4 : KINEMATIKA - PŘÍKLADY
4.7 Kontrolní otázky
1 . Vyjádřete analytické řešení inverzní úlohy pro polohy pro manipulátor RT (příklad 4 .2) .
2 . Definujte DR parametry pro příklady 4.2 a příklad 4.3 a vyřešte přímou a inverzní úlohu v Matlab Robotic Toolboxu.
3. V Robotic Toolboxu vyzkoušejte řešení inverzní úlohy kinematiky pro rovinné mechanismy RR a RRR. Testuje vliv po- �--.,--čátečního nastavení v souvislosti s dvojznačností úlohy. Jak funguje algoritmus inverzní kinematiky v singulární poloze ?
4. V Matlabu si naprogramujte vlastní algoritmus řešení inverzní kinematiky. Vyzkoušejte konvergenci na příkladu rovinného RR manipulátoru při použití inverze, pseudoinverze a transpozice jakobiánu. Jak se algoritmus chová v okolí singularity ?
Kapitola 5
Kinematika soustav
s uzavřenou topologií
Obsah kapitoly
5.1 M etody řeš ení . . . . . . . . . . . . . . . . . . . . . . 84
5 .1 . 1 Metoda uzavřené sm yčk y . . . . .
5 .1 .2 Metoda rozpojené sm yčk y . . . . .
84
84
5.2 Pří klad: Č ty řč lenný mech ani smus. . . . . . . . . . . 87
5.2 . 1 Odvození vazebný ch rovni c
5.2 .2 Přímá kinematika .
5.2 .3 Inverzní kinematika
5.3 Sh rnutí . . . . . . . .
5.4 K ont rol ní otáz ky .
87
88
89
90
90
V této kapitole naznačíme řešení kinematiky soustav s uzavřenými smyčkami. Zjistíme, že možným (a používaným) řešením je "rozpojení" soustavy a řešení 'inverzní úlohy kinematiky pro tyto dva vzniklé otevřené řetězce.
Při výkladu teorie i v příkladech jsme se dosud zabývali pouze kinematikou mechanismů s otevřenou topologií. Jejich typickým příkladem jsou klasické sériové průmyslové roboty a manipulátory.
V praxi ale musíme řešit i kinematiku mechanismů s uzavřenými kinematickými smyčkami. Základní problém spočívá v tom, že tyto obsahují nezávislé i závislé kloubové souřadnice, čímž se výpočet značně komplikuje. Připomeňme, že v případě otevřeného řetězce obsahují rovnice pouze nezávislé kloub. souř. ,
84 KAP . 5 : KINEMATIKA SOUSTAV S UZAVŘENOU TOPOLOGIÍ
přímou kinematiku tak můžeme vyřešit vždy v uzavřeném tvaru a inverzní pak iterační metodou s využitím jakobiánu. V této kapitole ukážeme, že u mechanismů se smyčkami řešíme i přímou kinematickou úlohu numericky s použitím jakobiánu a obě úlohy (přímá a inverzní) se liší pouze tvarem jakobiánu.
5 .1 Metody řešení
Při výkladu metod řešení kinematiky mechanismů s uzavřenými smyčkami budeme uvažovat kinematický řetězec vázaný na začátku a na konci k základnímu pevnému tělesu. Zobecnění pro případ složitější topologie je pochopitelně možné.
5.1 .1 Metoda uzavřené smyčky
Uvažujme kinematický řetězec s n tělesy. Sestavíme-li jeho transformační matici vzhledem k souř. syst . O přes celou kinematickou smyčku zpátky do syst . O, dostaneme rovnici:
TlQT21 . . . Tn n-lTno = 14 ,
Too (q) = 14
(5. 1 ) (5.2)
kde 14 je jednotková matice o rozměru 4 x 4 a T nO je konstantní transformační matice mezi dvěma vazbami mechanismu s pevným rámem.
Uvážíme-li tvar transformační matice, vidíme, že jsme získali 12 nelineárních algebraických rovnic z nichž pouze 6 je nezávislých1 . Tyto rovnice můžeme řešit numericky.
Vidíme, že z n + 1 kloubových souřadnic je n + 1 - 6 nezávislých, které musíme pro přímou kinematiku určit . Ostatních 6 závislých vypočteme. Jakmile známe všechny kloubové souřadnice q, můžeme snadno kartézské souř. vypočítat stejně jako u otevřeného mechanismu.
Poznamenejme ještě, že pro rovinnou úlohu máme místo 6-ti rovnic tři.
5.1 .2 Metoda rozpojené smyčky
Princip této metody spočívá v rozpojení uzavřené smyčky v některé z vazeb, čímž dostáváme dva otevřené kinematické řetězce. Podle charakteru rozpojené
1 Matice T má vždy tvar podle rov. 2.60 a obsahuje tedy 3 x 3 + 3 = 12 nenulových prvků. Přitom víme, že prvky rotační matice jsou vzájemně vázány a k jej ich definici postačují tři parametry. Společně s třemi rovnicemi pro polohu tedy máme 6 nezávislých rovnic.
5 . 1 : METODY ŘEŠENÍ 85
Obr. 5 .1 : Schéma metody rozpojené smyčky
vazby pak musí koncové efektory E a E' (obr. 5 . 1) splňovat určité podmínky tak, aby byl zachován charakter uzavřeného řetězce.
Pro rozpojení vybíráme vazbu, jej íž kloubová souřadnice nás nezajímá (např. v ní není umístěn pohon) .
Podmínky uzavřenosti smyčky pro jednotlivé typy vazeb definujeme následujícím způsobem:
sférická vazba v prostoru - poloha obou koncových efektorů musí být shodná, tedy:
E E' ro = ro Tnor� = To'oTnlolr�:
Máme tedy 3 vazbové rovnice.
(5.3)
(5 .4)
rotační vazba v prostoru - poloha obou bodů musí být shodná, navíc směry os rotační vazby musí být shodné
o - o {ZE - zE' , E E' ro = ro 1\ nebo
(r� - r6) x (rf - r6' ) = O (5 .5)
kde z� je osa rotace v rozpojené vazbě a bod P leží na této ose. Dostáváme tedy 3+2=5 vazbových rovnic.
86 KAP. 5 : KINEMATIKA SOUSTAV S UZAVŘENOU TOPOLOGIÍ
posuvná (prizmatická) vazba v prostoru - směry os posuvné vazby musí být shodné, osy musí být totožné a tělesa se vzájemně pohybují pouze ve směru této osy. Uvažujme body A, B, C ležící na tělese n a F, G, H ležící na 71,' , přičemž A, B, F, G leží na ose. Dostáváme tyto tři podmínky:
(r� - r�) x (rr - rg) = O rovnoběžnost os (5.6)
(r� - r�) x (r� - rr) = O bod D leží na přímce AB (5 .7)
(r� - r�) . (rr - r�) = konst tělesa se vzájemně kolem osy neotáčí (5.8)
válcová vazba v prostoru - podobně jako u prizmatické vazby, mimo poslední podmínku
pevná vazba v prostoru - poloha tří nekolineárních bodů je shodná na obou tělesech
rovinné úlohy - v případě rovinných mechanismů je situace pochopitelně podstatně jednodušší. Rotační vazba má podmínku stejnou jako sférická vazba v prostoru (2 vazbové rovnice) , prizmatická vazba je definována úhlem a vzdáleností (2 rovnice) .
Vazebné podmínky upravíme do tvaru f = O, podle počtu rovnic určíme počet závislých souřadnic, které budeme počítat.
Z vektoru q vybereme hledané závislé souřadnice q a pro ně sestavíme jakobián. Dále postupujeme podle rov. 3. 1 1 a 3 . 12 s uvažováním, že cílová hodnota jé = O . Rovnice pak přejdou do tvaru:
Xk = !(qk)
qk+l = qk - J-1Xk
(5 .9) (5 . 10)
Poznamenejme, že vektor kartézských souřadnic Xk zde má význam zobecněné odchylky rozpojené vazby .
. Tímto způsobem vypočteme závislé souřadnice a známe tedy celý vektor q. Následně můžeme určit polohu a orientaci libovolného bodu mechanismu nástroji pro otevřený řetězec. Tím je přímá kinematická úloha vyřešena.
5 . 2 : PŘíKLAD: OTYŘČLENNÝ MECHANISMUS 87
5.2 Příklad: Čtyřčlenný mechanismus
V tomto příkladu ukážeme řešení přímé a inverzní úlohy kinematiky pro polohy u jednoduchého rovinného mechanismu s uzavřenou topologií. Použijeme metodu rozpojené smyčky.
Obr. 5.2: Schéma rovinného čtyřčlenného mechanismu s uzavřenou topologií
5 .2 .1 Odvození vazebných rovnic
Definujeme souř. syst. podle obr. 5.2 a s použitím rov. 5 .4 můžeme snadno psát:
(5. 1 1)
kde T20 je totožná s maticí v příkladu 4 . 1 , r� a rf jsou v našem případě nulové vektory a matice Tso vyjadřuje konstatní transformaci z bodu A do bodu D.
[
C6 -S6 O L, C6]
[�
O O
�]
S6 C6 O L3 S6 Tso = 1 O (5. 12 ) T., = � O 1 O O 1
O O 1 O O
88 KAP. 5 : KINEMATIKA SOUSTAV S UZAVŘENOU TOPOLOGIÍ
Po symbolickém vynásobení matic a převedení výrazů na levou stranu dostáváme dvě výsledné vazebné podmínky
T20r: - T50T65rf = [L2 C12 + Ll cl - L3 C6 - d]
= O L2 S12 + Ll Sl - L3 S6
které lze formálně zapsat takto:
Další postup již závisí na povaze řešené úlohy.
5.2 .2 Přímá kinematika
(5. 13)
(5 . 14)
Připomeňme, že studovaný mechanismus má jeden stupeň volnosti a je tedy popsán jednou nezávislou souřadnicí, v našem případě ql . Zbývající souřadnice q2 a q6 jsou závislé.
Přímou úlohu definujeme takto:
• známe hodnotu natočení ql
• hledáme polohu bodů B a C.
Z rovnice 5 . 14 vyplývá, že se jedná o problém řešení soustavy dvou nelineárních algebraických rovnic o dvou neznámých. Řešení provedeme numericky s použitím jakobiánu, formálně shodně jako provádíme řešení inverzní úlohy pro otevřený řetězec.
Řešení úlohy má dvě hlavní části: .
1 . Výpočet závislých proměnných q2 a Q6 ' Na základě znalostí funkcí x a y sestavíme analytický jakobián:
J - [g� g�l - [-L2 S12 L3 S6 ] - !tlL !tlL - L2 C12 -L3 C6 OQ2 OQ6
Numerickou iterační metodou nalezneme neznámé q2 a q6 .
(5 .15)
· 2 . Výpočet polohy bodů B a C. Známe-li hodnoty všech kloubových souřadnic mechanismu, můžeme snadno pomocí přímé úlohy pro otevřený řetězec (vzniklý rozpojením vazby) vypočítat polohu bodů mechanismu:
rg = T20 (ql , q2)rf
r� = TlO (ql )rf
(5. 16) (5 . 17)
5 .2 : PŘíKLAD : OTYŘČLENNÝ MECHANISMUS 89
5 .2.3 I nverzní kinematika
Definujme úlohu takto:
• známe polohu bodu C
• hledáme hodnoty všech kloubových souřadnic.
Z obrázku 5 .2 je zřejmé, že rozpojením mechanismu v bodě C přechází inverzní úloha v problém totožný s inverzní úlohou otevřeného řetězce RR v příkladu 4. 1 . Kloubové souřadnice ql a q2 tedy vypočteme numerickou iterační metodou s využitím jakobiánu podle rov. 4.43:
(5. 18)
90 KAP. 5 : KINEMATIKA SOUSTAV S UZAVŘENOU TOPOLOGIÍ
5 .3 Shrnutí
• V převážné části předchozího textu jsme se zabývali pouze otevřenými kinematickými řetězci . Cílem této kapitoly bylo naznačit a na jednoduchém příkladu ukázat řešení vázaných systémů.
• Stručně jsme charakterizovali metodu uzavřené smyčky, která je principiálně i implementačně velice jednoduchá, vede však na větší počet rovnic, než následující metoda rozpojené smyčky.
• Na příkladu rovinného čtyřčlenného mechanismu jsme jasně demonstrovali, že v případě uzavřených kinematických smyček se přímá i inverzní úloha řeší formálně shodným postupem - numerickou iterační metodou s využitím jakobiánu, který podle typu úlohy pouze různým způsobem sestavujeme.
5.4 Kontrolní otázky
1 . Vysvětlete princip metody uzavřené smyčky a popište typ a počet rovnic, které při jejím použití řešíme.
2 . Popište princip metody rozpojené smyčky, vysvětlete na rovinném příkladu.
3. Objasněte formulaci podmínek uzavřenosti smyčky pro prostorovou úlohu, ukažte na příkladu.
4. Jakým matematickým postupem se řeší přímá úloha kinematiky pro polohy pro uzavřené řetězce? Srovnejte s přímou úlohou kinematiky pro polohy u otevřených řetězců a vyjádřete se k možnosti nalezení analytického řešenÍ.
5. V Matlabu naprogramujte řešení přímé úlohy kinematiky čtyřčlenného mechanismu. Jak se bude algoritmus lišit od řešení inverzní úlohy kinematiky pro otevřený řetězec, které jste naprogramovali při studiu předchozí kapitoly?
Kapitola 6
Dynamika
prostorových
mechanismů
Obsah kapitoly
6.1
6.2
Ú vod
Zák ladní metody a tvar y r ovnic .
6.2 .1 Lagrangeovy rovnice II . druhu
6.2.2 Metoda uvolňování (Newton) .
6 .2 .3 Úlohy dynamiky . . . . . . . .
92
93
93
94
95
6.3 Kinetostatik a ( inver zní úloha dy namik y) . . . . . 95
6.4
6.5
6.6
6.3.1 Staticky ekvivalentní přenesení síly a momentu 96
6.3.2 Silový a momentový bivektor . . . . . . . 97
6.3.3 Statika otevřeného kinematického řetězce 97
6.3.4 Zahrnutí setrvačných účinků . . . . . . . 99
6.3.5 Průmět síly a momentu do kloubové souřadnice . 100
Maticová metoda vy cházející z LR2 . . . . . . . . . 101
6.4.1 Výpočet kinetické a potenciální energie 101
6.4.2 Derivace energií a sestavení rovnic . . 103
6.4.3 Doplnění pasivních odporů v kloubech 104
Shrnutí . . . . . . . . 104
Kontrolní otázk y . . 105
92 KAP. 6 : DYNAMIKA PROSTOROVÝCH MECHANISMŮ
6.1 Úvod
Při návrhu, konstrukci a řízení mechanických (mechatronických) soustav velmi často vystačíme s kinematickým modelem. Typickým příkladem může být řízení stacionárního průmyslového robotu, např. při svařování. Bude-li požadovaná rychlost pohybu relativně malá a pohony robotu dostatečně předimenzované, pak budou dynamické síly zanedbatelné a pro řízení lze použít pouze kinematiku. Případně můžeme provést výpočet statických sil v pohonech postupem uvedeným v 3.4.
Pohled na výrobní linku některé z našich automobilek nám však jednoznačně říká, že v řadě důležitých aplikací nejsou dynamické efekty zanedbatelné. V takových případech nás může zajímat:
• Jak 'velké jso'u dynam'ické slly/momenty v pohonech pf'i daném defino'vaném pohybu soustavy? Jedná se o řešení inverzní dynamické úlohy, často používáme metodu kinetostatiky, která přímo pracuje s kinematickými transformacemi.
• Zdá se, že dynamické efekty budou mít vliv na chování soustavy, pohony nemůžeme dostatečně pfedimenzovat. Jak velká bude odchylka reálného chování od naplánované kinematiky? Jde o otázku simulace dynamického systému, tedy o přímou úlohu dynamiky. U složitých soustav použijeme k sestavení rovnic maticové metody založené na Lagrangeových rovnicích druhého druhu. Výsledné rovnice (ODE) integrujeme standardním postupem.
• Chceme testovat různé způsoby Nzení mechanické soustavy a také vliv dynamiky pohonů a případně další subsystémů. Opět přímá úloha dynamiky, tentokrát musíme výsledné mechanické rovnice doplnit rovnicemi ostatních prvků a řízení. Pak můžeme mluvit o modelování mechatronického systému (mechanismus s (elektrickými) pohony a řízením) nebo o komplexních modelech. Rovnice takového systému jsou navzájem vázané a nelze tedy samostatně simulovat jednotlivé části.
Mimo tyto základní úlohy můžeme dynamický model soustavy potřebovat pro některé pokročilé metody řízení nelineárních soustav. Inverzní dynamika je zde použita pro tzv. globální linearizaci soustavy a vyžaduje řešení v reálném čase. Jako velice důležitá se zde objevuje otázka rychlosti výpočtu.
V následujícím textu nejprve uvedeme přehled základních metod, které jsou vhodné pro jednoduché a převážně rovinné soustavy. Studenti s nimi byli seznámeni v prvním stupni studia v předmětu Dynamika.
6 . 2 : ZÁKLADNÍ METODY A TVARY ROVNIC 93
V případě prostorových soustav s více stupni volnosti již použití "tužky a papíru" selhává a tvorbu dynamického modelu je nutno automatizovat . Byla vyvinuta řada metod (tzv. dynamických formalismů) , které se mohou významně lišit z hlediska tvaru výsledných odvozených rovnic a tím i obtížnosti a rychlosti numerické integrace.
Při výkladu metod budeme vycházet z kinematického popisu, kterým jsme se zabývali v předchozí kapitole, a omezíme se na otevřené kinematické řetězce.
6.2 Základní metody a tvary rovnic
V základních kurzech Dynamiky jsou přednášeny následující tři metody pro sestavení dynamického modelu:
• metoda Lagrangeových rovnic druhého druhu (analytická dynamika)
• metoda uvolňování vycházející z druhého Newtonova zákona (vektorová dynamika)
• metoda redukce použitelná převážně pro 1D problémy (typicky rotorové a převodové soustavy)
První dvě metody jsou základní a principiálně z nich vychází řada dynamických formalismů.
6.2 .1 Lagrangeovy rovnice I I . druhu
Metoda vychází z principů analytické dynamiky. Postup sestavení pohybových rovnic můžeme popsat takto:
• zvolíme n nezávislých zobecněných souřadnic qi
• sestavíme kinetickou a potenciální energii a upravíme je tak, aby byly funkcí zobecněných souřadnic a jejich derivací
• derivacemi podle Lagrangeovy rovnice druhého druhu
i = 1, . . . , n (6. 1 )
získáme n pohybových rovnic
• výsledný model je tedy ve tvaru soustavy ODE.
94 KAP . 6 : DYNAMIKA PROSTOROVÝCH MECHANISMŮ
Běžný maticový zápis výsledné soustavy je následující:
M(q)q + C(q, q)q + b(q) + g(q) = fe
kde q je vektor nezávislých zobecněných souřadnic, M matice hmotnosti, C matice coriolisových a odstředivých sil, b vektor sil viskózního tření, g vektor gravitačních sil, fe zobecněné síly vztažené k zobecněných souřadnicím.
(6 . 2)
Formálně jiný, ale obsahem shodný tvar v rov. 6.5 1 je uveden u maticové metody založené na Lagrangeových rovnicích.
6.2 .2 Metoda uvolňování (Newton)
Metoda vychází z druhého Newtonova pohybového zákona. Postup lze charakterizovat takto:
• uvolníme všechna tělesa, rozpojené vazby nahradíme ekvivalentním silovým působením
• dostaneme 3n rovnic v rovině nebo 6n rovnic v prostoru s m > 6n neznámými
• doplníme (m - 6n) vazbových rovnic
• rovnice upravíme do vhodného tvaru pro numerické řešení
• získáváme soustavu diferenciálně-algebraických rovnic (DAE)
• řešíme na počítači numerickou integrací.
Výslednou soustavu rovnic zapisujeme ve tvaru:
M(q)q = f(t , q, q) + JT(q, t)A g(q, t) = 0
kde q je vektor obecně závislých souřadnic, M( q) je matice hmotnosti ,
(6.3) (6.4)
f(t, q, q) reprezentuje příspěvky odstředivých, Coriolisových, viskózního tření a vnějších sil, g(q, t) je vektor vazbových podmínek,
6 .3 : KINETOSTATIKA (INVERZNÍ ÚLOHA DYNAMIKY) 95
J(q, t) je Jakobián éJg/éJq, A je vektor Lagrangeových multiplikátorů související s vazbovými silami.
Numerické řešení DAE je řádově komplikovanější než ODE a související poznámky jsou j iž mimo rámec tohoto textu. Cenné poznatky a inspiraci lze nalézt v [20, 19, 15] .
6.2 .3 Úlohy dynamiky
Podobně jako v kinematice rozlišujeme i v dynamice dvě základní úlohy:
Přímá úloha dynamiky - na základě známého vnějšího silového působení a počátečního stavu soustavy určíme chování soustavy (časovou závislost kinematických veličin poloha, rychlost, zrychlení) . Jedná se tedy o řešení soustav ODE/DAE integrací .
Nepřímá úloha dynamiky - na základě známého chování soustavy (zadané průběhy kinematických veličin) určujeme síly resp. momenty v pohonech, které toto chování vyvolají. Jde o řešení soustavy algebraických rovnic.
Vidíme, že na rozdíl od kinematiky je v dynamice snadnější řešení nepřímé úlohy.
6 .3 Kinetostatika (inverzní ú loha dynamiky)
Základní metody "ručního" sestavení pohybových rovnic soustavy jsou omezené na velmi jednoduché (nejčastěji rovinné) problémy s několika málo stupni volnosti .
Kinetostatika je relativně jednoduchou1 metodou, která umožňuje řešení úlohy inverzní dynamiky pro velmi složité a rozsáhlé prostorové kinematické struktury. Na základě zadaného časového průběhu kinematických veličin v kloubových souřadnicích (tedy polohy q (t) , rychlosti ČI (t) a zrychlení či (t) ) vypočítáme vektor sil v kloubových souřadnicích r (t) , který zadaný pohyb realizuje.
Kinetostatika aplikuje ď Alembertův princip, který převádí dynamickou úlohu na statickou přidáním virtuálních setrvačných sil působících na těleso. Postup výpočtu je následující:
lJednoduchost metody spočívá v možnosti snadno pochopit její princip a fyzikální základ. Ctenář nechť se nenechá odradit velkým množstvím indexů u silového bivektoru, se kterým metoda pracuje. Je totiž potřeba označit, o kterou veličinu se jedná, ke kterému bodu j i vztahujeme a ve kterém souř. syst. vyjadřujeme.
96 KAP. 6 : DYNAMIKA PROSTOROVÝCH MECHANISMŮ
• vypočteme zrychlení a úhlové zrychlení těžiště každého tělesa - využijeme přitom maticových transformačních vztahů popsaných v předchozím textu
• podle ď Alembertova zákona vypočteme setrvačné síly a momenty
• vyřešíme statickou úlohu, kdy mezi působící síly zahrneme i setrvačné účinky.
V dalším textu vysvětlíme detaily metody a použití budeme demonstrovat na řešených příkladech. Jak dále uvidíme, pro otevřený kinematický řetězec metoda vede na snadno implementovatelný maticový numerický formalismus.
6.3 .1 Staticky ekvivalentní přenesení síly a momentu
V kap. 3.4 jsme uvedli způsob výpočtu zobecněných sil v kloubových souřadnicích (pohonech) T v závislosti na zobecněné síle v kartézských souřadnicích působící na koncový efektor pomocí jakobiánu. V následujícím popíšeme jinou metodu vycházej ící ze základního axiomu statiky o ekvivalentním silovém působení a využívající prostorové transformace kinematických veličin.
Obr. 6 . 1 : Staticky ekvivalentní přenesení síly z bodu L do bodu P Uvažujme sílu FL a moment ML působící v bodě L (silový bivektor) . Sta
ticky ekvivalentní působení vztažené k bodu P určíme podle následujících vztahů (pro názornost dalšího výkladu uvedeme vlevo vektorový a vpravo maticový zápis) :
f(p) = f(L) m(p) = mrL) + S(rPL) f(L)
kde S(rPL ) E ]R3x3 je antisymetrická matice vektoru rPL .
(6.5)
(6.6)
6 .3 : KINETOSTATIKA (INVERZNÍ ÚLOHA DYNAMIKY)
6.3.2 Silový a momentový bivektor
Pro zjednodušení zápisu a výpočtů zavedeme pojem bivektor.
S(L)i = [ f(L)i 1 m(L)i
97
(6.7)
kde m(L)i je moment vzhledem k bodu L vyjádřený v systému i. Z předchozího víme, jak provedeme staticky ekvivalentní změnu vztažného bodu, maticový zápis s využitím bivektoru je tento:
S(P)i = [ S(;rL) II S(L)i = NfLS(L)i kde S(P)i je ekvivalentní silový bivektor vzhledem k bodu P.
(6.8)
Chceme-li vyjádřit silový bivektor v j iném souřadnicovém systému j, jednoduše použijeme rotační matici.
fj = Rjfi mj = Rjmi
S použitím bivektoru můžeme stejnou transformaci zapsat takto:
kde Rj je rozšířená rotační matice.
6.3.3 Statika otevřeného kinematického řetězce
(6.9) (6 .10)
(6 . 1 1)
Uvažujme obecný prostorový manipulátor s n tělesy vázanýIIŮ posuvnými nebo rotačními vazbami v kloubech. Schematicky je znázorněný na obr. 6 .2 . Manipulátor je v tíhovém poli Země a na koncový efektor působí vektor zobecněných sil (síly a momenty) ""f. Uvažujme, že manipulátor je v klidu, v kloubech tedy musí působit síly T , které jej v rovnováze udržují . Naším cílem bude tyto zobecněné síly v pohonech vypočítat .
Předpokládejme, že nás zajímá kloub Oi-l ve kterém působí moment 'i ' Budeme tedy uvažovat všechny vnější síly působící na tělesa 'l , . . . , n . Ze základních kurzů statiky a dynaIIŮky totiž víme, že vnitřní s'tly nemají na celkový pohyb soustavy vliv. Síly resp. momenty v kloubech i , . . . , n jsou vnitřní síly a proto je nebudeme uvažovat. Další postup je j iž zřejmý:
• uvážíme všechny síly působící na tělesa i, . . . , n
98 KAP. 6 : DYNAMIKA PROSTOROVÝCH MECHANISMŮ
Obr. 6.2: Schéma obecného manipulátoru pro statický výpočet
• vyjádříme je jako bivektory
• přepočteme je do souřadnic systému i - I • přeneseme je do bodu Oi-l • a sečteme.
Vliv síly a momentu působících na koncový efektor vyjádříme jako bivektor (předpokládáme, že zatížení je vyjádřeno v systému O)
E [ ffE)o ] S(E)O = E J.L(E)O Bivektor dále přepočteme do souřadnic syst. i - I
a přeneseme do bodu Oi-l
E - E S(E)i-l = Ro;i-l S(E)O
Vliv tíhy j-tého tělesa jako bivektor zapíšeme takto:
Gj SGj - [f(Tj)O] (Tj)O - O G " T fefj)o = [0, 0, -mj9]
a přepočet a přenesení je podobné.
(6. 12)
(6. 13)
(6 . 14)
(6. 15)
(6. 16)
(6. 17)
(6. 18)
6 .3 : KINETOSTATIKA (INVERZNÍ ÚLOHA DYNAMIKY) 99
Veškeré silové účinky těles i, . . . , n vzhledem k Oi-I pak jsou
(6. 19)
Soustava je ve statické rovnováze a proto musí platit :
(6.20)
kde S(�i_l )í-I je silový bivektor generovaný pohonem Ti ve vazbě Di-I . Jeho výpočet ukážeme dále.
6.3.4 Zahrnutí setrvačných účinků
Metoda kinetostatiky rozšiřuje předchozí statickou analýzu o vliv dynamických účinků, které vznikají při pohybu mechanismu. Podle ď Alemberta vypočteme následujícím způsobem setrvačné síly a momenty j-tého tělesa
(6.21)
(6.22)
Pro vyjasnění výkladu popíšeme význam a způsob výpočtu jednotlivých použitých proměnných detailně:
• a��j{ je zrychlení těžiště j-tého tělesa vyjádřené v souřadnicích systému i - I - vypočteme podle rov. 2 .79
• ff/:')'i-I je setrvačná síla j-tého tělesa, která má působiště v těžišti Tj tohoto tělesa a je vyjádřená v souřadnicích systému i - I
• Ij je matice setrvačnosti j-tého tělesa vyjádřená v souř. syst. j (a je tedy konstantní)
• vektor Wj a jeho antisymetrická matice nj popisují úhlovou rychlost tělesa j vůči systému i - I a jsou vyjádřeny v syst. j .
• vektor éj představuje úhlové zrychlení tělesa j vzhledem k syst. i - I a je vyjádřené v syst. j .
Vypočtený setrvačný moment mf,J,j)j podle rov. 6 .23 j e v syst. j , musíme proto jeho souřadnice přepočítat do i - 1 .
mSj . = R.. . mSj . (Tj )�-I J, (�-I) (Tj )) (6.23)
100 KAP. 6: DYNAMIKA PROSTOROVÝCH MECHANISMŮ
Nyní již můžeme definovat bivektor vztažený k bodu Ty:
a přenést ho do bodu o.i-l
[ rSj 1 s _ (T )i- l S(Tj)i-l - m� (Tj )i- l
(6 .24)
(6.25)
Na závěr zapíšeme celkovou kinetostatickou rovnováhu bivektorů doplněnou o setrvačné účinky:
(6.26) j=i j=i
6.3.4.1 Výpočet matice O a vektoru c
Z rov. 6.23 vyplývá, že pro výpočet setrvačných účinků potřebujeme určit veličiny Cj , Oj a Wj '
V předchozím textu jsme naznačili dvě možnosti:
• součet dílčích matic podle popisu v kap. 2 . 1 .4 • submatice matic V a A: Z rov. 2 .72 je zřejmé, že:
O = submatrix(Vj,(i_l) , 1 : 3 , 1 : 3)
Podle rov. 2 .91 pak platí:
E = submatrix(Aj,(i_l ) , 1 : 3, 1 : 3) - 02
6.3.5 Průmět síly a momentu do kloubové souřadnice
(6.27)
(6.28)
Řešení kinetostatické úlohy dokončíme tak, že vyjádříme "průmět" síly resp. momentu do osy pohonu Oi-l '
V případě rotačm1w pohonu platí:
�" - S t T" T [ O ] t - (Oi_l )i- l Oi-l (6 .29)
kde Ti je moment v pohonu. Pro prizmatický (translační) pohon platí podobně:
T" T [OtO"-l] Ti = S(Oi_ l )i-l (6.30)
6 .4 : MATICOVÁ METODA VYCHÁZEJící Z LR2 101
Opakováním výše popsaného postupu pro i = 1 , . . . , n vypočteme všechny neznámé síly v pohonech. Pochopitelně, že popsaný postup umožní zahrnout mimo tíhovou i další vnější síly nebo momenty působící na libovolné těleso.
Tím jsme pro definovaný pohyb manipulátoru a z toho vyplývající setrvačné účinky určili odpovídající zatížení ve všech pohonech. Tím je inverzní úloha dynamiky (kinetostatika) vyřešena.
Z matematického hlediska můžeme konstatovat, že jsme získali soustavu ODE v plně implicitním tvaru
f(q, Čl, q, t) = T(t) (6.31)
Jak jsme již řekli a jak je z rovnice zřejmé, řešení inverzní úlohy dynamiky je snadné. Zadané vektory q, Čl a q dosadíme a vypočteme potřebné síly v pohonech T .
Naproti tomu, řešení přímé úlohy dynamiky je s tímto tvarem rovnic obtížné a vyžaduje speciální matematické metody2 . V případě jednoduchých soustav můžeme rovnice do výhodnějšího tvaru ručně (s použitím Maple) převést, j inak je ale vhodnější použít jiné metody pro sestavení rovnic.
6.4 Maticová metoda vycházející z LR2 Metoda vychází z kinematického popisu (tak zjistíme rychlosti) a Lagrangeových rovnic druhého druhu (6. 1 ) .
6.4.1 Výpočet kinetické a potenciální energie
Eki = � [ voT vop dV (6.32)
Rychlost elementu dV je vyjádřená v systému O. Pomocí základních rovnic využívajících jakobián a rotační matici dostaneme kinetickou energii i-tého tělesa takto:
(6.33)
kde I� je matice setrvačnosti í-tého tělesa vyjádřená v systému i pevně spojeném s tělesem a tudíž je při pohybu konstatní a Ro je rotační matice ze systému i
2V Matlabu je pro tento typ úlohy k dispozici řešič ode 15 i .
102 KAP. 6: DYNAMIKA PROSTOROVÝCH MECHANISMŮ
do O. Jednotlivé jakobiány vypočteme podle stejného předpisu jako v rov. 3 .6, změní se pouze význam některých vektorů.
J};'i) = [j�i) j�i) J}fi) = [j�i) j�i)
ZJO'-I] Zj-I x (r(Ti) - rj-I)]
Zj-I
. (Ti) O Jpj . (Ti) O JRj
O] O]
pro prizmatickou vazbu,
pro rotační vazbu,
(6.34)
(6.35)
(6.36)
kde Zj-l je vektor osy rotace (j - 1 )-tého kloubu , r(Ti) je p�lohový vektor těžiště i-tého tělesa vzhledem k počátku syst. O a rj-I je polohový vektor počátku syst . j - 1 vzhledem k počátku O. Všechny vektory jsou vyjádřené v souřadnicích pevného systému O.
Provedeme sumaci rovnice 6.33 pro všech n těles a dostaneme výslednou celkovou kinetickou energii
n n n Ek = I: Eki = � L L b'ij (q)q'iqj = � i{ B(q) <i
i=l i=1 j=1 n
B (q) = L mi JWi) T JWi) + J�i) T �o I! Rfo J�i) i=1
Matice B E }Rnxn je:
• symetrická
• pozitivně-definitní
• obecně závislá na stavu manipulátoru q.
Potenciální energii vypočteme takto:
n E - � E - T (Ti) P - L..t Pi - -mi go ro
i=l
(6.37)
(6.38)
(6.39)
kde gf . [O, O, -gV je vektor tíhového zrychlení , r�T'i) je poloha těžiště i-tého tělesa vyj ádřená v syst. O.
6 .4 : MATICOVÁ METODA VYCHÁZEJÍCÍ Z LR2 103
6.4.2 Derivace energií a sestavení rovnic
Derivujeme kinetickou a potenciální energii podle předpisu Lagrangeových rovnic II. druhu
� (&Ek) _ � b . . ( ) " . + � dbij (q) ' . _ � b . . ( ) " . + � � &bij (q) . ' . dt & ' . - � �J q qJ � dt % - � tJ q % � � & qk% qt j=1 j=1 j=1 j=1 k=1 qk
n n n L bij (q)iiJ + L L hijkqkqj + 9i = Ti j=1 j=1 k=1
h " k - &bij (q) _ � &bjk (q) tJ - &qk 2 &qi Uvedených n rovnic zapíšeme do maticového tvaru
B(q)q + C (q, q)q + g(q) = T
kde matice C( q, q) E IRnxn je taková, že n n n
L CiAj = L L hijkqkqj j=1 j=1 k=l
Volba matice C není jednoznačná. Zvolme element matice n
Cij = L Cijkqk k=l
Cijk = � (&bij + &bik _ fJbjk ) 2 Oqk oqj Oqi kde Cijk jsou Christoffelovy koeficienty3 . Díky symetrii matice B platí
Cijk = Cikj Matici C nazýváme Coriolisova a lze dokázat, že matice
B - 2C E IRnxn
je antisymetrická matice.
3Také Chrístoffelovy symboly, angl. Christoffel symbols of the fi rst type.
(6.40)
(6.41)
(6.42)
(6.43)
(6.44)
(6.45)
(6.46)
(6.47)
(6.48)
(6.49)
(6.50)
104 KAP. 6: DYNAMIKA PROSTOROVÝCH MECHANISMŮ
6.4.3 Doplnění pasivních odporů v kloubech
Rovnici 6 .45 můžeme jednoduše doplnit o pasivní odpory, kterými jsou:
• viskózní tření Fvq, kde F,u E IRnxn je diagonální matice se (zápornými) koeficienty příslušejícími jednotlivým kloubům
• Coulombovské (suché) tření Fe sgn(q) , kde Fe E IRnxn je opět diagonální matice koeficientů,
Výsledná rovnice má pak tvar
B(q)q + C(q, q)q + F'uq + Fe sgn(q) + g(q) = T (6.51)
Vidíme, že na rozdíl od metody kinetostatiky (rov. 6 .31) máme k dispozici zápis soustavy ODE, který lze jednoduše převést do lineárně implicitního tvaru a řešit relativně snadno numericky na počítači.
6.5 Shrnutí
• Stručně jsme zmínili základní tři metody používané pro ruční sestavování dynamických modelů. Uvedli jsme typický tvar výsledných soustav ODE resp. DAE.
• Formulovali jsme dvě základní úlohy dynamiky.
.. .. ' (} () �. , .". .... . � . �
• Podrobně jsme popsali metodu kinetostatiky, která umožňuje řešení inverzní úlohy dynamiky pro otevřený řetězec a vychází přímo z kinematického popisu mechanismu.
• Dále jsme popsali metodu principiálně vycházející z Lagrangeových rovnic druhého druhu. Jejím výstupem jsou rovnice použitelné v přímé i inverzní úloze dynamiky pro otevřený řetězec.
• Výklad maticových metod sestavení pohybových rovnic mechanické soustavy může být dosti nenázorný a obtížně pochopitelný. Pro dostatečné zvládnutí problému lze doporučit podrobné studium nejjednodušších příkladů v těchto skriptech a vlastní experimentování při řešení konkrétního problému na počítači .
• Ověření správnosti výsledného odvozeného modelu přitom lze provádět porovnáním různých metod nebo porovnáním s referenčním automaticky vytvořeným modelem (např. MatlabjSimMechanics - viz 10. 1 .5) .
6 .6 : KONTROLNÍ OTÁZKY
6.6 Kontrolní otázky
1. Popište základní princip sestavení pohybových rovnic metodou uvolňování. Na jaký matematický problém vede přímá úloha dynamiky?
2. Popište princip sestavení pohybových rovnic pomocí Lagrangeových rovnic druhého druhu. Jaké rovnice dostaneme? Na
105
jaký matematický problém vede přímá úloha dynamiky? c.--_." ... A inverzní úloha? Srovnejte s metodou uvolňování .
3 . Vysvětlete princip metody kinetostatiky. Na jakou úlohu dynamiky se dá použít a proč?
4. Zapište maticový tvar pohybových rovnic, který získáme pomocí maticové metody vycházející z LR2. Srovnejte s metodou kinetostatiky.
106 KAP. 6 : DYNAMIKA PROSTOROVÝCH MECHANISMŮ
Kapitola 7
Dynamika - příklady
Obsah kapitoly
7. 1 Příklad: Dynamika fyzikálního kyvadla 107
7. 1 . 1 Klasicky "ručně" pomocí LRlI 108
7. 1 .2 Maticově pomocí jakobiánů . . 109
7. 1 .3 Kinetostatika . . . . . . . . . . 110
7.2 Příklad: Dynamika rovinného manipulátoru RR . . 113
7.2 .1 Klasicky " ručně" pomocí LRlI 113
7.2 .2 Maticově pomocí jakobiánů 115
7.2 .3 Kinetostatika . . . . . . . . . . 117
7.3 Příklad: Dynamika prostorového RR manipulátoru 120
7.3 .1 Klasicky "ručně" pomocí LRII . . . . . . . . . . . . 120
7 .1 Příklad: Dynamika fyzikálního kyvadla
Fyzikální kyvadlo je jedním z nejjednodušších mechanických systémů. Pohybuje se v tíhovém poli Země a neuvažujeme pasivní odpory ani žádné vnější síly, jedná se tedy o konzervativní systém.
Na obr . 7. 1 je vidět poloha a orientace souřadnicových systémů O a 1 , které jsme volili v souladu s příkladem 4 . 1 a můžeme tedy použít odvozené kinematické transformační vztahy.
Nejprve sestavíme pohybovou rovnici běžným způsobem pomocí Lagrangeových rovnic II. druhu.
Následně aplikujeme maticové metody (kinetostatika a Lagrangeovy rovnice) a na závěr zjistíme, že:
108 KAP. 7: DYNAMIKA - PŘÍKLADY
T
Obr. 7. 1 : Fyzikální kyvadlo (rovinný manipulátor R s jedním stupněm volnosti
• všechny postupy vedou na stejné pohybové rovnice a
• maticové metody jsou značně pracnější.
Poznamenejme proto, že smyslem tohoto příkladu je doplnění výkladu metod a ukázka jejich konkrétního použití, i když kvůli jednoduchosti úlohy nemůže vyniknout jej ich výhoda. Maticové metody je možno totiž velice dobře algoritmizovat a tím potenciálně řešit složité prostorové soustavy, které "ručním" způsobem zpracovat nelze.
7 .1 .1 Klasicky " ručně" pomocí LRI I
Ruční sestavení pohybové rovnice fyzikálního kyvadla je velice snadné. Nejprve vypočteme kinetickou a potenciální energii.
Derivujeme
1 2 1 ' 2 1 L2 ' 2 Ek = 2mVT + 2IzTfJ = 2 (IZT + m 4)fJ L Ep = m g 2819
d (8Ek ) L2 .. dt 8iJ = (IzT + m 4 )fJ
8Ek = O 8fJ 8Ep L -- = m g -c19 8fJ 2 a sestavíme výslednou rovnici :
(7. 1)
(7.2)
(7.3)
(7.4)
(7.5)
(7.6)
7. 1 : PŘÍKLAD : DYNAMIKA FYZIKÁLNÍHO KYVADLA 109
7.1 .2 Maticově pomocí jakobiánů
Transformační matici TIO (rov. 4 .7) převezmeme z příkladu 4 . 1 . Definujeme a vypočteme potřebné polohové vektory
rIT) = [-��] r(T) - T r(T) o - 10 1
a podle předpisu 6 .36 vypočteme jednotlivé jakobiány:
Jo (T) - zO x r(T) - ZAO r(T) Pl - o o - o o
kde zg je antisymetrická matice vektoru zg
Tím jsme vektorové násobení převedli na maticové a získáme jakobiány
J(T) -P -
J(T) -R -
[-J L SťJ] 2 L eť} O m
(7.7)
(7.8)
(7.9)
(7. 10)
(7. 1 1)
(7. 1 2)
Matici RIO získáme jako submatici TlO a konstantní matici setrvačnosti vzhledem k těžišti II definujeme takto
[O O O ] 11 = O O O O O fT z
(7. 13)
Pak již můžeme vypočítat matici B, která pro Tl, = 1 degeneruje na skalární hodnotu bl l .
(7. 14)
Vidíme, že matice je vzhledem k proměnné 73 konstantní a proto jsou všechny Christoffelovy koeficienty a tím i celá matice C nulové.
1 10 KAP. 7 : DYNAMIKA - PŘÍKLADY
Zbývá ještě určit vliv tíhové síly
_ T . (T) _ T J(T) _ 1
L 91 - m go J Pl - go P - "2 m 9 Ci} a můžeme psát výslednou rovnici :
1 .. 1 ( 4: m L 2 + lz}ď + "2 m 9 LGď = 7
kde 7 je moment působící ve vazbě, např. moment motoru.
7.1 .3 Kinetostatika
7.1.3.1 Kinematické veličiny
(7. 15)
(7. 16)
Základní transformační matice TlO (rov. 4 .7) , DlO (rov. 4 . 12) , VlO , AlO , ŤlO převezmeme z příkladu 4 . l .
Nejprve vypočteme a určíme veličiny podstatné pro výpočet setrvačné síly a momentu. Vypočteme zrychlení těžiště
(T) •• (T) 1 .. . 2 [ -� L (Ci) J2 + Si}
J) ] ao = TlOrl = - 2 L (-Ci}
o1} + Si} 1} ) (7 .17)
Z matice VlQ určíme snadno matici n jako submatici (rov. 6.27) : [O -1} O] nio = J O O O O O (7. 18)
Podobně získáme podle rov. 6.28 vektor a matici úhlového zrychlení. [O -J O] El = J O O O O O (7. 19)
Je vidět, že výsledek odpovídá našemu očekávání (úhlová rychlost a zrychlení pouze okolo osy z) . , 7.1.3.2 Vliv tíhové síly
Uvažujeme pouze zatížení tíhovou silou
(7.20)
7. 1 : PŘíKLAD : DYNAMIKA FYZIKÁLNíHO KYVADLA
ze které vytvoříme silový bivektor (moment je nulový)
sG - [f�)Ol - [O (T)O - O - -mg
a přeneseme jej do bodu 00 .
G NOoT G S(Oo)O = o s(T)O =
O O O
O -ml g
O O O
-� L C1} m g
kde
7.1 .3.3
NOoT -o -
1 O O O O
-� Ll Sl
Vliv pohybu soustavy
O O 1 O O 1 O � L S19 O -� L C19
� L C1) O
Nyní můžeme vypočítat setrvačnou sílu a moment:
O O O 1 O O
O] T
O O O O 1 O
Moment, který je nyní v systému 1, transformujeme do O:
1 1 1
(7.21)
(7.22)
O O O O (7.23)
O 1
(7.24)
(7. 25)
(7. 26)
(soustava má pouze jeden stupeň volnosti a proto jsou oba momenty rovny) .
112 KAP. 7 : DYNAMIKA - PŘÍKLADY
Nyní můžeme setrvačnou sílu a moment zapsat jako bivektor
1 . .. "2 m L (Ci) ťJ2 + Si) ťJ)
1 . . . "2 m L ( -c." ťJ + S." ťJ2 )
O O O
-IzT J Silový bivektor přeneseme do bodu 00 takto:
S NOoT S s(oo)O = o S(T)O =
7.1 .3.4 Výsledná rovnice
� m L (c." J2 + S." J) 1 . . . "2 m L ( -c." ťJ + S." ťJ2 )
O O O
- (Iz + �L2 m) J
(7. 27)
(7.28)
Uvažujeme-li nulové vnější působení na koncový efektor SE = O, pak kinetostatickou rovnováhu zapíšeme takto:
(7.29)
Silový bivektor v pohonu pak je
(7.30)
. Potřebný moment v pohonu spočítáme podle 6.29
(7.31)
7 .2 : PŘÍKLAD : DYNAMIKA ROVINNÉHO MANIPULÁTORU RR 1 13
7.2 Příklad: Dynamika rovinného manipulátoru RR
Obr. 7.2: Rovinný manipulátor RR (dvojité kyvadlo)
7.2 .1 Klasicky " ručně" pomocí L R I I
Nejprve definujeme čtyři kartézské souřadnice popisující polohu obou hmotných středů těles.
Ll Xl = 2" Cl Ll Y1 = 2" Sl
L2 x2 = L1 C1 + 2" C12 L2 Y2 = L1 s1 + 2" S12
Derivací těchto souřadnic vypočteme příslušné rychlosti
(7.32)
(7.33)
(7.34)
(7.35)
(7.36)
(7.37)
1 14 KAP. 7: DYNAMIKA - PŘÍKLADY
a z nich snadno sestavíme kinetickou energii
E 1( ( ' 2 ' 2 ) ( ' 2 ' 2 ) I ·2 I ( ' 1 . )2 ) k = 2" ml Xl + YI + m2 X2 + Y2 + zlql + z2 q + q2 .
= (� ( ml Li + m2 L�) + � ( IZI + Iz2 + m2 Li + m2 Ll L2 C2) ) qi+ + (� m2 Ll L2 C2 + l m2 L� + Iz2) q2 ql + (� Iz2 + � m2 L�) q5
Podobně získáme pomocí kartézských souřadnic potenciální energii
Ep = mlgYI + m2gY2
(7.38)
(7.39)
(7.40)
(7.41)
Nyní můžeme podle rov. 6 . 1 derivovat obě energie a získat dvě pohybové rovnice. Derivaci můžeme provádět ručně nebo s použitím softwaru pro symbolické manipulace s výrazy (např. Maple) . Pro první proměnnou ql dostaneme
d (BEk ) (1 2 2 1 2 ) ..
dt Bql = 4: ml Ll + m2 Ll + m2 Ll L2 C2 + 4: m2 L2 + IzI + Iz2 ql + (� m2 Ll L2 C2 + l m2 L� + Iz2) 112 - � m2 Ll L2 q5 82-- m2 Ll ql L2 q2 82
BEk = O Bql
BEp 1 1 � = - ml 9 Ll cl + m2 9 Ll Cl + - m2 9 L2 cl2 u� 2 2
a podobně pro druhou proměnnou q2
(7.42)
(7.43)
(7.44)
d (BEk ) (1 1 2 ) " (
1 2 ) " dt Bq2 = 2 m2 Ll L2 C2 + 4: m2 L2 + Iz2 ql + 4: m2 L2 + Iz2 q2-
- � m2 Ll ql L2 q2 82 (7.45)
BEk 1 . 2 1 . . ( ) Bq2 = - 2 m2 Ll ql L2 82 - 2 m2 Ll ql L2 q2 82 7.46
BEp 1 ( ) Bq2 = 2 m2 9 L2 C12 7.47
Nakonec z jednotlivých derivovaných členů sestavíme podle rov. 6 . 1 výsledné rovnice. Vidíme, že jsme dostali dvě vzájemně vázané diferenciální rovnice druhého řádu. K vůli přehlednosti a řešení na počítači je vhodné je zapsat do maticového tvaru.
Srovnáním s rov. 7.72 může zvídavý student snadno dokázat, že odvozené rovnice jsou shodné.
7 .2 : PŘÍKLAD : DYNAMIKA ROVINNÉHO MANIPULÁTORU RR 1 1 5
7 .2 .2 Maticově pomocí jakobiánů
Matice pro kinematické transformace převezmeme z příkladu 4 . 1 , podobně jako v předchozí úloze 7. 1 a s jejich pomocí si připravíme vektory potřebné pro výpočet jednotlivých geometrických jakobiánů.
(Tl) _ [ I L O O] T rl - - 2 I (Tl) _ T (Tl) ro - 1Q rl (T2) T (T2) ro = 20 r2
Dále vypočteme jakobiány pro těleso 1 :
a tedy
. (TI) _ o X (TI) _ JPl - Zo ro -
J(T1) _ P -
J(Tl) _ R -
[- l L, s, ! Ll Cl
O
[Hl Podobně pro těleso 2 :
• (T2) o (T2) JPI = zD X ro . (T2) 1 ( (T2) 1 ) Jp2 = zD x ro - ro
J(T2) _ p -
J(T2) _ R -
[_1 L2 S12 - Ll Sl 12 2 L2 Cl2 + Ll Cl O
[ Hl
�l
(7.48)
(7.49)
(7.50)
(7.51)
(7.52)
(7.53)
(7.54)
(7.55)
(7.56)
(7.57)
(7.58)
1 16 KAP. 7: DYNAMIKA - PŘÍKLADY
Podle rov. 6.38 vypočteme výslednou matici
Z prvků této matice můžeme snadno vypočítat Chrístoffelovy koeficienty a následně celou matici C.
1 Obll Clll = - (- ) = O (7.63) 2 Oql 1 Obll 1 CU2 = C12l = 2 ( Oq2 ) = -2m2L2L182 = h('I32) (7.64)
C122 = (ob12 _
! Ob22 ) = h('I32) (7.65) Oq2 2 Oql C2ll = -h('I32) (7.66) C2l2 = C22l = O (7.67) C222 = O (7.68)
C = [ h('I32)-ď� h('132) ('130
" 1 + -ď2)] (7.69) -h( '132)'131
Definujeme-li vektor go = [O, -9, OlT (rovinná úloha) , snadno vypočteme členy
1 1 91 = (2 ml Ll + m2 Ll) 9 Cl + 2 m2 9 L2 C12 1 92 = 2 m2 9 L2 Cl2
(7.70)
(7.71)
Tím máme vypočteny všechny členy rovnice 6.45 a obě pohybové rovnice můžeme zapsat v maticovém tvaru takto:
7 .2 : PŘÍKLAD : DYNAMIKA ROVINNÉHO MANIPULÁTORU RR 1 1 7
7.2.3 Kinetostatika
7.2.3.1 Kinematické veličiny
Předpokládáme, že základní transformační matice převezmeme z řešení kinematiky v příkladu 4 . 1 . Dále snadno zjistíme, že veličiny a6Tl) , fh , El budou shodné s příkladem dynamiky jednoduchého kyvadla 7. l .
Zrychlení těžiště tělesa 2 vypočteme podle 2 .79 takto:
(7.73) [- t L2 4i C12 - t L2 jiI 812 - (Žl 42 L2 C12 - t jh L2 812 - t lŽ� L2 C12 - Cl lŽi Ll - Sl jiI Ll] = -2 L2 lŽi 812 + 2 L2 jiI C12 - lŽI 42 L2 S12 - 2 lŽ� L2 812 + 2 ih L2 CI2 - 81 lŽi Ll + CI jiI Ll
O
Úhlovou rychlost tělesa 2 vypočteme podle vztahu 2 .52 a dostaneme [ O -(Ž2�- (h O�] O2 = rl2 ; rll
a podobně zrychlení
7.2.3.2 Vliv tíhy těles
Silové bivektory pro obě tělesa v pevném souř. syst. sestavíme snadno.
SWl)O = [f&J)o] = [O -m19 O O O OJ T
sfi2)0 = [O -m29 O O O OJ Dále bivektory přeneseme do bodů 00 a 01 . Vyjádříme tak:
• vliv tíhové síly tělesa 1 na pohon 1
Gl NOOTl Gl s(Oo)O = o S(Tl)O =
O -m1 9
O O O
- � Ll Cl ml 9
(7.74)
(7.75)
(7.76)
(7.77)
(7.78)
118 KAP. 7: DYNAMIKA - PŘÍKLADY
• vliv tíhové síly tělesa 2 na pohon 1
o
G2 NOOT2 G2 -m2 9
O 8(00)0 = o 8(T2)0 = O
O -(� L2 C12 + Ll Cl ) m2 9
• vliv tíhové síly tělesa 2 na pohon 2
-Sl m2 9 -Cl m2 9
O O O
� L2 S2 Sl m2 9 - � L2 C2 Cl m2 9
7.2.3.3 Vliv pohybu soustavy
Nyní vypočítáme setrvačné síly a momenty:
fSl (Tl) (Tl)O = -ml3.Q
fS2 (T2) (T2)0 = -m23.Q
mrA) l = -llgl + fhllWl = [ � ] -[zl 1§
Momenty převedeme do souř. syst. O a vytvoříme bivektory.
[ fSl 1 Sl (Tl)O 8(Tl)0 = R mSl 10 (Tl)l [ fS2 1 S2 (T2)0 8(T2)0 = R mS2 20 (T2)2
Dále bivektory přeneseme do bodů 00 a 01 ' Vyjádříme tak:
(7.79)
(7.80)
(7.81)
(7.82)
(7.83)
(7.84)
(7.85)
(7.86)
7.2 : PŘÍKLAD : DYNAMIKA ROVINNÉHO MANIPULÁTORU RR
• vliv setrvačného bivektoru tělesa 1 na pohon 1
t ml Ll (Cl lir + sd Ž1 ) - :2 ml (-Sl lir + C l ch ) Ll
o Sl NOOTI Sl 8(00)0 = o 8(T1)0 = o
o -1 (4 Iz1 + Lr ml) ih
• vliv setrvačného bivektoru tělesa 2 na pohon 1
S2 NOOT2 S2 8(00)0 = o S(T2)0
• vliv setrvačného bivektoru tělesa 2 na pohon 2
7.2.3.4 Výsledná rovnice
1 19
(7.87)
(7.88)
(7.89)
Opět neuvažujeme vnější sílu působící na koncový efektor, kinetostatickou rovnováhu pro pohon 1 zapíšeme takto:
G1 G2 Sl S2 Tl O 8(00)0 + 8(00)0 + 8(00)0 + 8(00)0 + 8(00)0 = (7.90)
Rovnováhu pro pohon 2 zapíšeme takto
(7.91 )
Momenty v pohonech pak již spočítáme snadno vynásobením bivektoru 8'(00)0 resp. 8'(bl )1 osou pohonu podobně jako v příkladu fyzikálního kyvadla. Rovnice jsou již značně obsáhlé a nebudeme je zde proto vypisovat. Jednoduše se dá dokázat, že dostaneme stejné rovnice jako z maticové nebo klasické "ruční" metody.
120 KAP. 7 : DYNAMIKA - PŘÍKLADY
7 .3 Příklad : Dynamika prostorového RR manipulátoru
Uvažujme mechanismus podle obr. 4. 13. Těleso 1 rotuje kolem pevné osy otáčení Zo , těleso 2 se pohybuje obecným prostorovým pohybem. Vyšší srozumitelnosti výkladu přispěje zjednodušení, při kterém těleso 2 nahradíme nehmotnou tyčkou s hmotným bodem na konci (koncový efektor) .
7.3. 1 Klasicky " ručně" pomocí LRI I
Používáme-li Lagrangeovy rovnice druhého druhu, musíme nejprve definovat zobecněné souřadnice a na jejich základě vypočítat kinetickou a potenciální energii. V tomto příkladu zvolíme dvě kloubové souřadnice ql a q2 podle obr. 4.13.
Kinetickou energii vypočteme takto:
1 .2 1 2 Ek = 2,Ilql + 2,m2v2 (7.92)
kde V2 je velikost vektoru rychlosti hmotného bodu 2, který musíme vyjádřit pomocí zobecněných souřadnic a jejich derivací.
V příkladu 4.5 jsme odvodili transformační matici soustavy, ze které snadno získáme souřadnice koncového efektoru (hmotného bodu) .
x = slL2s2 + clLl y = -c1L2s2 + SILI Z = L2C2
Derivací souřadnic a součtem druhých mocnin snadno získáme
2 (L2 + L2 L2 2 ) · 2 2L L . . L2 · 2 V2 = I 2 - 2C2 ql - I 2C2Q2ql + 2q2
Kinetickou a potenciální energii celé soustavy pak vypočteme takto:
E 1 1 '2 1
( (L2 L2 L2 2) ' 2 2 L . . L L2 ' 2 ) k = 2, 1 Ql + 2, m2 I + 2 - 2 C2 ql - 2 C2 q2 Ql I + 2 Q2
Ep = m2gL2c2
(7.93) (7.94) (7.95)
(7.96)
(7.97)
(7.98)
7 .3 : PŘÍKLAD: DYNAMIKA PROSTOROVÉHO RR MANIPULÁTORU 121
Standardním postupem pak derivujeme energie a obdržíme výsledné dvě pohybové rovnice, které zapíšeme v maticovém tvaru takto:
Bq + f = [��] B (q2 )
= [ll + m2 L� + m2 L� - m2 L� c� . -m2 L2 C2 Ll
f( . . ) = [m2 L2 82 42 (2 L2 C2 41 + ČJ2 Ll)]
Q2 , q1 , q2 - (L2 C2 4i + g) m2 L2 82
(7.99)
(7. 100)
(7. 101)
Ačkoli se jedná o dosti jednoduchou soustavu a provedli jsme zjednodušení tělesa 2 na hmotný bod, postup tvorby rovnic je j iž poměrně pracný. Výhodné je zde použití softwaru pro symbolickou manipulaci s výrazy, např. Maple.
122 KAP. 7: DYNAMIKA - PŘÍKLADY
Kapitola 8
Kinematika a dynamika
neholonomních soustav
Obsah kapitoly
8.1 Holonomní a neholonomní vazby
8.2 N eholonomní soustavy . . . . . .
8.3 Kinematika neholonomnÍch soustav
8.3.1 Bod v rovině . . . . . . .
8 .3 .2 Kotouč valící se po rovině
8.3.3 Jízní kolo . . . . . . , . .
8 .3 .4 Diferenciálně řízený robot
· 123
· 125
· 125
126
128
129
. . 130
8.4 Dynamika neholonomních soustav . . . . . . . . . . 133
8.4. 1 Příklad: Diferenciálně řízený robot , . . , . . . . . . 133
8.5 Shrnutí . . . . . . . . . . . . 136
8.6 Kontrolní otázky . . . . . . . . . . . . . . . . . . . . . 136
Nejzajímavějšími případy neholonomních soustav jsou pro nás kolová vozidla. V této kapitole se dozvíme, jak sestavovat jejich kinematické modely použitelné např. pro řízení.
8 . 1 Holonomní a neholonomní vazby
Uvažujme soustavu s n stupni volnosti popsanou zobecněnými souřadnicemi q = [q1 , . . . , qn]T , Soustava je vázána k < n holonomními vazbami
hi (q) = O, i = 1, . . . , k (8. 1 )
124 KAP. 8 : KINEMATIKA A DYNAMIKA NEHOLONOMNÍCH SOUSTAV
Derivací vazbové rovnice dostáváme
d ( )
ah.i . ah.i . [ahi ahi ] •
dt hi q = aq1
q1 + . . . + aqn qn = aql " ' " aqn q = O (8.2)
Definujme vektor Wi vyjadřující diferenciál holonomní vazbové podmínky a rovnici pak zapíšeme takto:
(8.3)
Vidíme, že vektor Wi1 je ortogonální (kolmý) k rychlosti q. Vazebnou podmínku 8 .1 pro polohy jsme tímto převedli na podmínku pro rychlosti.
v
x
Obr. 8. 1 : Vázaný pohyb bodu po kružnici
Příkladem může být hmotný bod v rovině (q = [x , y]T , kdy jeho pohyb omezíme na kružnici (obr. 8.1 ) . Vazbová rovnice pro polohy je pak ve tvaru
(8.4)
Vazebnou podmínku pro rychlosti W vypočteme pomocí parciálních derivací
W = [%� %�] = [2x 2y] (8.5)
Uvážíme-li závislost souřadnic x a y při pohybu po kružnici , snadno dokážeme platnost vztahu 8.3.
V řadě praktických úloh máme vazebné podmínky definovány primárně pro rychlosti (rov. 8 .3) . Důležitou otázkou pak je, zda existuje odpovídající vyjádření pro polohy (rov. 8 . 1 ) . Podle toho rozlišujeme:
holonomní vazby - pro dané Wi , kde i = 1 , . . . , k lze najít odpovídající funkce hl , . . . , hk ·
1 V angl. lit. PfafFia n constra int.
8 .2 : NEHOLONOMNÍ SOUSTAVY 125
neholonomní vazby - pro dané Wi nelze nalézt odpovídající funkce. Systémům s těmito vazbami říkáme neholonomní systémy.
Otázka praktického důkazu, zda je či není možné nalézt vazebné podmínky pro polohy a zda se tedy jedná o nehol onom ní systém, je značně komplikovaná a přesahuje rámec tohoto textu2 .
8 .2 Neholonomní soustavy
Nejdůležitější případy mechanických neholonomních soustav jsou:
• kolové dopravní prostředky (jízdní kola, automobily, . . . , kolové mobilní roboty)
• modelování pohybu a řízení lodí • problematika robotické manipulace s pevnými tělesy • soustavy, u kterých se zachovává moment hybnosti (kosmická technika,
"gymnastický" robot)
Neholonomní soustavy jsou oproti holonomním podstatně složitější a to jak z hlediska modelování, tak z hlediska řízení.
Typickým příkladem je problém pohybu osobního automobilu. Uvažujemeli pohyb v rovině, pak je jeho stav dán souřadnicemi q = [x , y, )OV. Pohyb automobilu je v každém okamžiku možný pouze v jednom směru definovaném orientací zadní (neřízené) a přední (řízené) nápravy. Tím je dán směr rychlosti soustavy, můžeme tedy formulovat vazebnou podmínku pro rychlosti. Z vlastní zkušenosti dále dobře víme, že v rovině může automobil zaujmout libovolnou polohu q a že je možné mezi všemi možnými polohami přecházet. Jinak řečeno, není možné formulovat vazebnou podmínku pro polohy a jedná se tedy o neholonomní systém.
Obtížnost řízení neholonomních soustav pak dobře ilustruje problém parkování automobilu.
8 .3 Kinematika neholonomních soustav
Uvažujme soustavu popsanou n zobecněnými souřadnicemi jejíž pohyb je vázán k neholonomriími vazbami, které můžeme zapsat jako
i = 1 , . . . , k (8.6)
2Zájemce o tuto problematiku lze odkázat na oblast geometrického nelineárního řízení (Geometrie nonl inear contro l ) .
126 KAP. 8 : KINEMATIKA A DYNAMIKA NEHOLONOMNÍCH SOUSTAV
kde Wi E jRlxn je řádkový vektor. Všechny vazbové podmínky pak vyjádříme maticí A( q) E jRkxn takto:
A(q) q = O (8.7)
kde
A(�
=
[�:l
(8.8)
Wk
Nalezneme n - k lineárně nezávislých vektorů Sj E jRnx\ pro které platí
A(q) Sj = O j = l , . . . , (n - k) (8.9)
a zapíšeme je do matice S (q) = [Sl , . . . , Sn-k] E jRnx (n-k) , dostáváme tedy
A(q)S(q) = O (8 . 10)
Bez důkazu uveďme, že
(8 . 1 1)
kde II je vhodně zvolený vektor. Pro nás je podstatné, že lze tento vektor interpretovat jako vektor kinematických vstupních proměnných (víée v příkladech) .
8.3 .1 Bod V rovině
Nejjednodušším neholonomním systémem, který může být považován za model valícího se kotouče nebo diferenciálně řízeného robotu, je "orientovaný bod"3 .
y
x
Obr. 8 .2 : Pohyb bodu v rovině s neholonomními podmínkami
3Popisovaný model je zjednodušující myšlenkovou konstrukcí, která nemá odpovídající fyzikální realizaci. Přesto je díky své jednoduchosti užitečný.
8 . 3 : KINEMATIKA NEHOLONOMNÍCH SOUSTAV 127
Jeho stav je určen vektorem q = [x , y, <p]T (n = 3) a může se pohybovat pouze ve směru lokální osy XL .
Neholonomní vazbovou podmínku (k = 1) tedy zapíšeme snadno
(8. 12)
Pro další práci j i potřebujeme vyjádřit v globálních souřadnicích, pro transformaci použijeme rotační matici .
(8. 13)
Po úpravě dostaneme rovnici
1; tan <p - iJ = ° (8 . 14)
kterou snadno vyjádříme ve tvaru rov. 8 .3
[tan <p - 1 OJ [�] = O (8. 15)
A(q)Čl = O (8. 16)
Nalezneme dva vektory s a zapíšeme je maticově: [COS <p 0] S = Si�<p � (8. 17)
Nyní j iž můžeme psát rov. 8 . 1 1 [COS <Pl Čl = Si� r.p 'Ul + (8. 18)
Je zřejmé, že vektor Ul můžeme interpretovat jako dopřednou rychlost v a vektor U2 jako rychlost "zatáčení bodu" <p.
Tím jsme odvodili kinematický model soustavy. Z hlediska pojmů zavedených pro holonomní manipulátory s otevřenou topologií můžeme tento model považovat za dopředný - na základě znalosti rychlostí v pohonech (tj . dopředná rychlost v a rychlost zatáčení <p) můžeme vypočítat kartézské rychlosti .
128 KAP. 8 : KINEMATIKA A DYNAMIKA NEHOLONOMNÍCH SOUSTAV
x
Obr. 8.3: Pohyb kotouče v rovině s neholonomními podmínkami
8.3 .2 Kotouč valící se po rovině
Uvažujme kotouč valící se po rovné podložce s ideálním bodovým kontaktem. Jeho stav je popsán vektorem x, y, !.p, e (n ---:- 4) , kde rJ je úhel odvalování kotouče. Ostatní souřadnice mají stejný význam jako v předchozím příkladu. Neholonomní vazby vyjadřují zamezení bočního pohybu kotouče (stejně jako v minulém příkladu) a navíc zamezení prokluzu v podélném směru (valení bez prokluzu) .
ÝL = O V = XL = rJ
(8 . 19)
(8.20)
kde r je poloměr kotouče. Podobně jako v předchozím příkladu tyto podmínky vyjádříme v globálních souřadnicích a dostáváme dvě vazbové rovnice (k = 2)
které snadno zapíšeme maticově
X - rCcprJ = O Ý - rScpJ = O
Wl = [1 O O -rccpJ W2 = [O 1 O -rscp]
Nalezneme n . - k = 2 vektory s a zapíšeme výslednou rovnici
(8 .21)
(8.22)
(8 .23)
(8.24)
(8.25)
Snadno můžeme Ul interpretovat jako rychlost rotace kotouče kolem jeho osy J a Ul jako rychlost rotace kolem svislé osy cp (zatáčení) .
8 .3 : KINEMATIKA NEHOLONOMNíCH SOUSTAV 129
8.3.3 Jízní kolo
Další, j iž poněkud zajímavější úlohou, je problém kinematiky jízdního kola. Na obr. 8 .4 je schéma s vyznačením lokálních souřadnicových systémů obou kol a s okamžitým středem otáčení C. Je zřejmé, že nebudeme řešit otázku stabilizace jízdy na kole, zajímá nás pouze kinematika za předpokladu, že kola neprokluzují v bočním ani podélném směru (dokonalé valení) . Dále budeme uvažovat hnané zadní kolo (bude mít vliv na vstupní proměnnou kinematického modelu) . Stav soustavy je dán polohou a natočením zadního kola (bod S)
x
Obr. 8.4: Jízdní kolo v rovině s neholonomními podmínkami
x, y, cp, úhlem natočení řídítek 'IjJ a úhlem pootočení zadního kola '!9; tedy n = 5.
(8.26)
Nejprve vyjádříme vztah pro poloměr otáčení zadního kola Te a vztahy mezi rychlostmi předního VF a zadního Vs kola.
Te = d tan(1f/2 - 'IjJ) Vs = cp Ta
. 1 VF = cp d ----:--::t.
sm 'f/ Vs = VF cos 'IjJ
Nyní formulujeme k = 3 neholonomní podmínky
ÝL = O ÝF = O xL = 13 T
(8.27) (8.28)
(8.29)
(8.30)
(8 .31) (8.32)
(8 .33)
130 KAP. 8 : KINEMATIKA A DYNAMIKA NEHOLONOMNÍCH SOUSTAV
a po jejich převedení do glob. souř. syst . dostáváme
které snadno vyjádříme jako
i; - T cos cp J = O Ý - T sin cp J = O
r . cp - d tan 'lj; 1) = O
[� � � � -rs: q = O -TC ]
O O 1 O -á tan 'Ij;
Dále vytvoříme n - k = 2 vektory
Sl = [O O O 1 OJ T
[ r t ol. O 1J T 82 = rc<.p TS<.p d an 'ť'
a můžeme psát výsledný kinematický model
O rc<.p O rs<.p
q = O Ul + á tan 'IjJ U2 1 O O 1
(8.34)
(8.35)
(8 .36)
(8.37)
(8.38)
(8.39)
(8.40)
kde Ul má význam rychlosti otáčení řídítek a U2 rychlosti otáčení zadního hnaného kola.
8.3.4 Diferenciálně řízený robot
Dvoukolový diferenciálně řízený podvozek je patrně nejčasteji zvoleným konstrukčním řešením v oblasti mobilní robotiky. Hlavní předností koncepce je jednoduchost (žádná táhla a mechanismy řízení jako např. u automobilu) a také relativně vysoká manévrovatelnost (otočí se na místě) . Používá se u různých experimentálních konstrukcí servisních robotů pro vnitřní prostředí a především v robotických soutěžích, např. robotický fotbal4 nebo soutěže ))myší v bludišti" .
Nejprve definujeme vektor stavu robotu (71, = 5)
(8.41)
8 .3 : KINEMATIKA NEHOLONOMNÍCH SOUSTAV
Obr. 8 .5 : Diferenciálně řízený fotbalový robot (zdroj : Robohemia tým [13] )
y
, , ,
/'� , , , , I , ' cp / I
/ ' . I
x
Obr. 8 .6 : Diferenciálně řízený robot
kde tJ A a /) B jsou natočení kol robotu.
131
Budeme-li vycházet z předchozích příkladů, nadefinujeme neholonomní vazbové podmínky takto:
XA = rtJA XB = rtJB
(8 .42)
(8.43)
4V této oblasti dosáhl pozoruhodných výsledků tým Robohemia z ÚAMT FEKT VUT v Brně (mistr Evropy v r. 2001 a 2002) [13] .
132 KAP. 8: KINEMATIKA A DYNAMIKA NEHOLONOMNÍCH SOUSTAV
Vzhledem k zřejmým vlastnostem soustavy
YA = YB (8 .44)
(8.45)
vidíme, že vazbové podmínky jsou závislé. Místo čtyřech tedy musíme definovat pouze dvě a jako třetí použijeme rov. 8.45 (k = 3) . Jak uvidíme dále, bude výhodné je vztáhnout k . těžišti robotu, dostaneme tedy:
Po převodu do glob. souř. syst. můžeme podmínky zapsat maticově [1 O O _ !.Se. -!.Se.] O 1 O -t -� 4 = 0 O O 1 - 2h 2h
Nalezneme vhodné ortogonální vektory a můžeme psát
!.Se. !.Se. 2 2 '!!::e. rs", 2 2"" q = r '!.Ll + r U2 - 2h 2h
O 1 1 O
(8.46)
(8.47)
(8.48)
(8.49)
(8.50)
kde Ul má význam rychlosti J A kola A a U2 rychlosti kola B. Tím je kinematický model vytvořen.
Kinematický model můžeme sestavit i j iným způsobem, kdy uvažujeme vektor q = [x, y, 'PF a za vstupní proměnné do modelu považujeme dopřednou rychlost v a úhlovou rychlost cp. Pracujeme pak se zdánlivě jednodušším kinematickým modelem (n = 3) , ale musíme dodatečně provádět transformace mezi [v, 'P] a [J A , JB] . Zmiňovaný jednodušší model je totožný s "orientovaným bodem" v příkladu 8 .3 . 1 .
8 .4 : DYNAMIKA NEHOLONOMNÍCH SOUSTAV 133
8.4 Dynamika neholonomních soustav
V řadě praktických úloh nám pro řízení neholonomní soustavy (vozidla, robotu) plně dostačuje kinematika, kterou jsme se zabývali v předchozí kapitole.
V případě, že tomu tak není, a dynamické efekty významně ovlivňují chování soustavy, musíme přistoupit k návrhu a simulaci dynamického modelu. Máme přitom k dispozici dva základní přístupy:
Nahrazení neholonomní podmínky silou - tento princip je implementačně jednoduchý a je možné jej použít v prostředí běžných simulačních nástrojů. Do systému však přidáme vysokou tuhost, což vede na stiff systém se všemi nepříznivými důsledky.
Redukce dynamického modelu - pomocí kinematických rovnic snížíme počet diferenciálních rovnic popisujících systém a dosáhneme podstatně rychlejšího výpočtu.
Uvedenou problematiku ukážeme na příkladu dynamiky diferenciálně řízeného robotu.
8.4.1 Příklad: Diferenciálně řízený robot
Dynamický model robotu budeme sestavovat "bez uvažování dynamiky kol" , jeho kinematika se tedy zjednoduší na případ "orientovaného bodu" v kap. 8.3 . 1 .
8.4.1 .1 Sestavení dynamického modelu metodou uvolňování (Newton)
Těleso má v rovině tři stupně volnosti , k popisu tedy použijeme tří zobecněných souřadnic
(8.51 )
Metodou uvolňování sestavíme tři pohybové rovnice soustavy. Na robot působí dvě akční síly od pohonů FA a FB podle obrázku 8.7.
Viskózní tlumící síla působí proti směru pohybu kolečka, tedy ve směru záporné osy XL' Velikost tlumící síly v kolečku A vypočteme pomocí rychlosti těžiště Ve a úhlové rychlosti okolo těžiště 'ÍJ takto:
(8.52)
Pohyb v příčném (bočním) směru je zamezen silou FR'
134 KAP. 8 : KINEMATIKA A DYNAMIKA NEHOLONOMNÍCH SOUSTAV
y
o
I
, , \
,
e-' fb�
/ , ,
, , , , _ �/FbA
Obr. 8.7: Schema diferenciálně řízeného robotu
Nyní již můžeme sestavit soustavu tří ODE:
v
/ , fi
x
I I
FA cos cp + FB cos cp - FR sin cp - b(± + h<jJ cos cp) - b(± - h<jJ cos cp) = mx , v # , V J
(8.53) FA sin cp + F B sin cp + FR cos cp - b( ± + h<jJ sin cp) - b( ± - h<jJ sin cp) = my
(8.54)
FAh - FBh - 2bh2<jJ = I<p (8.55)
Tyto rovnice snadno přepíšeme do maticového tvaru [m m ] [�] + [-
2b -2b
] [�] = [��:� I cp -2bh2 cp h
c�s CP] sm cp -h
(8.56)
V těchto třech rovnicích (n = 3) se vyskytují neznámé {x, y, <p, FR} a soustavu tedy v této podobě nemůžeme řešit. V případě holonomních soustav (běžných) doplníme rovnice vazbovými podmínkami (pro polohy) , tyto dvakrát zderivujeme a dostaneme soustavu DAE řešitelnou např. v Matlabu. Pro neholonomní soustavy se tento postup použít nedá, protože mají podmínky vyjádřeny pro rychlosti.
8 .4 : DYNAMIKA NEHOLONOMNÍCH SOUSTAV 135
8.4.1.2 Klasický způsob simulace neholonomní vazby
Běžný způsob modelování kolových vozidel např. v prostředí MatlabjSimulink nebo SimMechanics pracuje takto:
• těleso má v rovině 3 stupně volnosti • holonomní vazbovou sílu počítáme jako funkci rychlosti
V našem případě můžeme simulační model snadno sestavit , položíme-li
(8.57)
kde k je velké číslo (dobře funguje např. k = 1e-6 ; ) . Ve výsledku takto dojde k zamezení pohybu v lokální ose YL a tedy správ
nému pohybu vozidla (přesvědčit se můžeme vykreslením integrované hodnoty posuvu během pohybu) .
Nevýhodou tohoto přístupu je zakomponování vysoké "tuhosti" a tím vysokých vlastních čísel do celého systému. Důsledkem je (razantní) zpomalení simulace.
Naopak výhodná je možnost simulace bočního skluzu vozidla V případě, že přenášenou sílu omezíme (saturace) .
8.4.1 .3 Redukce modelu zahrnutím neholonomnich podmínek
Vyjdeme z kap. 8.3 . 1 a rov. 8 .18 zapíšeme ve tvaru:
q(q) = S(q)v(t)
což je v našem případě (přeznačeno pro další snadnější zápis)
v = [�l = [: l Derivujeme rov. 8.58 podle času
q = Sv + Sv
(8.58)
(8.59)
(8.60)
(8.61)
a odvozené vztahy dosadíme do 8.56. Celou rovnici pak zleva vynásobíme ST .
MSv + MSv + VSv = Bf - A TA
STMSv + ST(VSMS)v = STBf - ST AT ).. --..-.=(AS)T=O
(8.62)
(8.63)
136 KAP. 8 : KINEMATIKA A DYNAMIKA NEHOLONOMNÍCH SOUSTAV
Vidíme, že se nám podařilo odstranit neznámý vektor A, což lze považovat za klíčový úspěch. Výsledná rovnice společně s 8.58 tvoří nový redukovaný model systému
STMSv + ST (VS + MS)v = STBf q = SV
(8.64) (8 .65)
Výsledný model je soustavou pěti ODE prvního řádu se stavovým vektorem [v, w, x , y, cp] . Těleso v rovině je přitom popsáno 3 ODE druhého řádu (což odpovídá 6-ti ODE prvního řádu) . Došlo tedy k redukci dynamického systému vlivem neholonomní vazby. Těleso se stále může pohybovat ve všech třech stupních volnosti x, y, cp, v pravém slova smyslu však nemá tři mechanické stupně volnosti (má 2,5 stupně volnosti - "intuitivní popis neholonomního systému" ) .
8.5 Shrnutí • Definovali jsme charakter neholonomní vazby a uvedli nej
důležitější příklady neholonomních soustav, z nichž nás budou zajímat kolové soustavy (automobil, mobilní kolový robot) .
• Na několika příkladech jsme ukázali návrh kinematického modelu kolové soustavy.
• Na závěr jsme na příkladu diferenciálně řízeného robotu ukázali dva základní způsoby modelování dynamiky neholonomních soustav. První způsob je implementačně jednoduchý a umožňuje i modelování prokluzu, výpočetně je však pomalejší (stiff systém) . Druhý způsob spočívá v redukci dynamického modelu a vede na výpočetně velice efektivní algoritmus.
8 .6 .Kontrolní otázky
1 . Charakterizujte neholonomní vazbu a uveďte praktický příklad.
2 . Pro zadanou jednoduchou soustavu ( "Bod v rovině" , "Kotouč" apod.) sestavte kinematický model.
3. Popište dva základní způsoby modelování dynamiky neholonomních soustav. Uveďte příklad a popište výhody a nevý-hody obou přístupů. Jakým způsobem je možno modelovat c--____ ' pro kluz v neholonomní vazbě? Jaké jsou s tím spojené simu-lační obtíže?
Kapitola 9
Plánování trajektorie
manipulátorů
Obsah kapitoly
9.1 Dráha a trajektorie (path and trajectory) . . 137
9.2 Pohyb v kloubových souřadnicích . . . . . . . . 138
9.2 .1 Kubický polynom . . . . . . . . . . 138
9 .2 .2 Polynom 5. stupně . . . . . . . . . 139
9.2.3 Trajektorie zadaná několika body . 139
9.2.4 Příklad úlohy pro průmyslový robot 142
9.3 Kontrolní otázky . . . . . . . . . . . . . . . . . . . . . 143
V této kapitole se jen velmi stručně dotkneme rozsáhlé problematiky plánování dráhy a trajektorie pohybu soustavy. Popíšeme základní způsob generování trajektorie, který je možné obecně aplikovat na stacionární průmyslové roboty, mobilní roboty i další soustavy.
9 .1 Dráha a trajektorie (path and trajectory)
Nejprve ujasníme dva základní pojmy:
Dráha (path) - je množina bodů, kterými musí soustava projít na cestě z počáteční do koncové polohy. Dráhou je tedy křivka definující pohyb koncového efektoru svařovacího robotu nebo silnice z Brna do Dolních Kotěhůlek.
138 KAP. 9 : PLÁNOVÁNÍ TRAJEKTORIE MANIPULÁTORŮ
Trajektorie (trajectory) - je dráha + čas definující kdy se bude soustava nacházet v kterém místě dráhy. Svařovací robot např. musí udržet po celé křivce konstantní rychlost, zatímco řidič vozidla bude zrychlovat a zpomalovat podle dopravní situace.
Obecně můžeme postup řízení soustavy podle požadavků charakterizovat třemi kroky:
• plánování dráhy • plánování trajektorie • sledování trajektorie! .
Dráha i trajektorie mohou být definovány a počítány v kloubových nebo kartézských (operačních) souřadnicích. Mezi oběma vyjádřeními přecházíme pomocí přímé a inverzní kinematiky.
Základní způsoby plánování dráhy (např. pro stacionární průmyslové roboty) pracují s prostorově definovanými křivkami (úsečka, oblouk apod.) . Složitější metody pak umožňují hledání dráhy v prostředí se statickými nebo dokonce dynamickými překážkami. Příkladem může být plánování dráhy pro skupinu spolupracujících stacionárních robotů nebo pro mobilního robotického fotbalistu, kde kromě míče musíme sledovat a patřičně vyhodnotit i pohyb spoluhráčů a soupeřových robotů. Dodejme, že plánování dráhy je stále aktuálním vědeckým problémem.
9.2 Pohyb V kloubových souřadnicích
Základní úlohou je pohyb soustavy z počáteční polohy qo do koncové polohy QF v požadovaném čase. Funkci polohy, rychlosti a zrychlení budeme definovat pro každou i-tou kloubovou souřadnici nezávisle, proto můžeme bez ztráty obecnosti v dalším textu pracovat s jednou souřadnicí , kterou obecně označíme q.
9.2 .1 Kubický polynom
Uvažujme nejprve, že v počátečním a koncovém bodu vyžadujeme určitou polohu a rychlost. Definujeme tedy čtyři podmínky
q(to) = qo q(to) = qo
q(tF) = qF q(tF) = qF
1 Angl. path pla nn i ng, trajectory p lann ing, trajectory tracking.
(9. 1 ) (9.2) (9.3)
9 . 2 : POHYB V KLOUBOVÝCH SOUŘADNICÍCH 139
Tyto podmínky dokáže zajistit polynom třetího stupně
(9 .4)
Výše uvedené podmínky pak zapíšeme jako soustavu čtyř algebraických rovnic o čtyřech neznámých
qo = ao + al to + a2 t� + a3 tg t2 3 qF = ao + al tF + a2 F + a3 tF
cio = al + 2 a2 to + 3 a3 t� ciF = al + 2 a2 tF + 3 a3 t�
nebo ekvivalentně v maticové podobě
Pro tF =1= to je řešení možné vždy.
9.2 .2 Polynom 5. stupně
(9.5) (9.6) (9 .7) (9.8)
(9.9)
V dalším textu ukážeme, že při výpočtu trajektorie může být výhodné mimo polohu a rychlost vyžadovat definované i zrychlení v počátečním a koncovém bodě. Dostáváme tedy šest podmínek, kterým vyhovuje polynom pátého stupně
(9. 10)
Skalární i maticový tvar získáme podobně jako v předchozím případě. Je zřejmé, že řešením je v tomto případě nalezení šesti konstant ao - a5 .
9.2 .3 Trajektorie zadaná několika body
V praktickém případě budeme patrně generovat složitější trajektorii na základě několika zadaných uzlových bodů.
Pro interpolaci trajektorie můžeme použít dva přístupy:
Polynom vyššího řádu - Uvažujme případ tří uzlových bodů a definovaných podmínek pro qo , ql , q2 , cio , ci2 , iio a ii2 . Na začátku a na konci trajektorie tedy předepisujeme polohu, rychlost i zrychlení a v prostředním bodě pouze polohu. Definovali jsme 7 podmínek, vyhovující je tedy polynom
140 KAP. 9 : PLÁNOVÁNÍ TRAJEKTORIE MANIPULÁTORŮ
6. řádu. Tento přístup má dvě zásadní nevýhody: 1) při rostoucím počtu uzlů roste i řád polynomu a tím i obtížnost řešení, které je často vyžadováno v reálném čase; 2) polynomy vyšších řádů mohou mít nevhodný tvar (oscilace) .
Několik navazujících polynomů nižšího řádu - Mezi jednotlivými uzlovými body použijeme polynom třetího nebo pátého řádu, podle toho, zda vyžadujeme i předepsané zrychlení.
1
x O
-1 O 0.5 1 .5 2 2.5 3 3.5 4
5 O >
-5 O 0.5 1 .5 2 2.5 3 3.5 4
1 0
t1l O
-1 0 O 0.5 1 .5 2 2.5 3 3.5 4
Obr. 9 . 1 : Trajektorie interpolovaná pomocí kubického polynomu
Pro většinu praktických úloh je podstatně výhodnější druhý popsaný přístup. Uvažujme čtyři uzlové body zadané časem, polohou, rychlostí (a zrychlením
-,- pouze pro polynom 5.stupně) :
T = [0 , 1 . 5 , 3 , 4] ; Q = [0 , 0 . 2 , -0 . 5 , 0 . 3] ; dQ = [0 , 2 , 2 , -1] ; ddQ = [0 , 1 , 1 , 0] ; % pouze pro polynom 5 . stupne
Na obr, 9 . 1 a 9.2 je srovnání vypočtené trajektorie s použitým polynomem třetího a pátého řádu. Vidíme, že u polynomu třetí řádu se vyskytuje skoková
9 . 2 : POHYB V KLOUBOVÝCH SOUŘADNICÍCH 141
x O
-1 O 0.5 1 .5 2 2.5 3 3.5 4
5
> O
-5 O 0.5 1 .5 2 2.5 3 3.5 4
1 0 O
ro
-1 0 O 0.5 1 .5 2 2.5 3 3.5 4
Obr. 9.2 : Trajektorie interpolovaná pomocí polynomu 5. stupně
změna zrychlení, která může z pochopitelných důvodů2 vadit při praktické implementaci. Máme-li proto k dispozici dostatečný výpočetní výkon, používáme polynomy vyšších řádů.
9.2.4 Příklad úlohy pro průmyslový robot
Jako příklad uvedeme dvě úlohy řízení stacionárního průmyslového robotu, pro jednoduchost uvažujme prostorový manipulátor RRR z příkladu 4.4, který umožňuje libovolné polohování koncového efektoru:
2Skoková změna zrychlení znamená podle rov. F = ma skokovou změnu síly. Uvážíme-li např. mechanickou rovnici stejnosměrného motoru
vidíme, že bychom vyžadovali skokovou změnu proudu i. Elektrická část motoru je ale dynamickým systémem
di 1 ( R ' A. ' ) - = - u - t - c'f/<p dt L
a proto skok proudu realizovat nelze.
142 KAP. 9: PLÁNOVÁNÍ TRAJEKTORIE MANIPULÁTORŮ
Rychlý přesun z bodu A do bodu B Polohy bodů jsou dány v kartézském syst. souřadnicemi x, y, z a předpokládáme, že známe počáteční vektor kloubových souřadnic qA . Postup řešení bude následující:
• vypočteme inverzní úlohu kinematiky a získáme kloubové souřadnice qB
• trajektorii mezi bodem A a B interpolujeme pro každou kloubovou souřadnici nezávisle polynomem třetího řádu
• dobu přesunu mezi body určíme tak, abychom nepřekročili maximální zrychlení a rychlost dané konstrukcí manipulátoru.
Přesné řízení z bodu A do bodu B po přímce a konstantní rychlostí v - jedná se pochopitelně o složitější úlohu, uspokojivé řešení provedeme takto:
• úsečku AB rozdělíme na n úseků • pro všech n+ 1 uzlových bodů provedeme inverzní úlohu (pro polohy
i rychlosti) a získáme tak příslušné kloubové souřadnice a rychlosti • mezi uzlovými body pak interpolujeme polynomem (v kloubových
souř. ) • je zřejmé, že počet bodů musí být dostatečně vysoký, aby odchylka
trajektorie mezi uzly byla menší než tolerance daná charakterem úlohy.
Poznamenejme ještě, že otázka řízení stacionárních robotů i dalších typů soustav je samostatnou oblastí , která leží mimo rámec tohoto textu. V náročnějších aplikacích je výše popsané nezávislé řízení jednotlivých kloubů (n xSISO) nahrazeno centralizovaným řízením (MIMO) , např. metodou řízení s použitím inverzního dynamického modelu.
9.3 Kontrolní otázky
1 . Definujte pojmy "dráha" a "trajektorie" , uveďte příklady.
2. Vysvětlete generování trajektorie mezi dvěma body v kloubových souřadnicích.
3. Popište generování trajektorie zadané několika body.
4 . Srovnejte použití polynomu třetího a pátého stupně.
Kapitola 10
Přílohy
Obsah kapitoly
10. 1
10.1 Softwarové nástroje
10 .1 . 1 Přehled a rozdělení .
10. 1 . 2 Maple . . . . . . . .
10. 1 .3 Matlab . . . . . . .
10. 1 .4 Matlab + Robotic Toolbox
10. 1 .5 SimMechanics .
10 .1 .6 Adams . . . . .
. . . . . . . . . . 145
146
147
149
150
151
152
10.2 Matematika . . . . . . . . . . . . . . . . . . . . . . . . 152
10 .2 .1 Trigonometrie . . 152
10 .2 .2 Skalární součin . 153
10.2 .3 Operace s maticemi 153
10.2 .4 Speciální matice . 154
10.2.5 Vektorový součin . 155
10.3 Označování veličin . . . . . . . . . . . . . . . . . . . . 155
Softwarové nástroje
Praktické řešení úloh prostorové kinematiky a dynamiky soustav tuhých těles je jen v dosti omezené míře možné ručně na papíře. Použití maticových metod předpokládá naopak použití počítače.
V následujícím textu stručně popíšeme základní vlastnosti softwaru Maple jako zástupce skupiny programů pro symbolickou manipulaci s matematickými
144 KAP. 10 : PŘÍLOHY
výrazy a prostředí Matlab pro numerické výpočty. Dále zmíníme nástavby SimMechanics a Robotic Toolbox se zabudovaným formalismem pro automatický návrh kinematického nebo dynamického modelu.
10.1 .1 Přehled a rozdělení
Software používaný pro inženýrské výpočty v oblasti prostorové kinematiky a dynamiky můžeme rozdělit do tří skupin:
Symbolické výpočty a manipulace s matem. výrazy - Veškeré výpočty, tj . maticové násobení, součty, derivace, integrály atd. probíhají v symbolické podobě, čísla nejsou při výpočtech zaokrouhlována jako v běžných programovacích jazycích. Skupina těchto nástrojů se označuje CAS (Computer Algebra Systems) , známými programy tohoto typu jsou Mathematica, MathCad a také Maple, kterému se také budeme věnovat dále. Existují ovšem i knihovny symbolických výpočtů pro běžné programovací jazyky1 a samozřejmě je možné si takové algoritmy pro konkrétní problém naprogramovat .
Numerické výpočty - Maticové operace se provádí a jsou ukládány přímo numericky. Takto můžeme jednoduše pracovat v libovolném programovacím jazyku s doinstalovanou podporou maticových výpočtů. Významné aktivity v této oblasti vyvíjí uživatelská komunita kolem jazyku Python. Technickým a vědeckým standardem je v současné době jazyk Matlab, který je přímo postaven na numerické práci s maticemi a kterému se budeme věnovat dále.
Software pro automatickou tvorbu modelu - Maple i Matlab poskytují počítačovou podporu pro výpočty podle algoritmů popsaných v předchozím textu. Uživatel s jejich pomocí více či méně obtížně sám implementuje relativně složité maticové metody, software "pouze" ulehčuje rutinní matematické operace. Dalším krokem ke zjednodušení praktického řešení úloh je proto vytvoření programů, které sami sestavují kinematický nebo dynamický model (mají implementovanou maticovou metodu) . Práce uživatele se soustředí pouze na zadání topologie, geometrie soustavy, počátečních podmínek a buzení a na vyhodnocení výsledků. V následujícím textu se budeme věnovat Robotic Toolboxu pro Matlab, SimMechanics a stručně charakterizujeme
1 Pro jazyk Python například existuje knihovna NumPy.
1 0 . 1 : SOFTWAROVÉ NÁSTROJE 145
produkt MSC.Adams. Kinematický nebo dynamický formalismus implementovaný uvnitř takového softwaru může být opět symbolický nebo numerický, z hlediska uživatele a způsobu jeho práce to však nehraje zásadní roli. Rozdíl bude v rychlosti a přesnosti výpočtu.
Všechny tři skupiny programů mají v současné technické praxi význam a uplatnění pro určitý typ úloh. V případě potřeby jednoduché analýzy chování MBS můžeme vybrat nástroj ze třetí skupiny, sestavení modelu a obdržení výsledků bude rychlé. Pro řízení složitější mechatronické soustavy např. metodou inverze dynamiky potřebujeme analytický dynamický model a pravděpodobně se neobejdeme bez softwaru z první skupiny. Na druhou stranu, numerické výpočty jsou nezastupitelné v oblasti řešení nelineárních algebraických rovnic (inverzní kinematika) i při simulaci chování systémů (numerická integrace) .
10 .1 .2 Maple
Cílem této kapitoly je velmi stručně charakterizovat program Maple, zejména z hlediska jeho použití pro kinematiku a dynamiku MBS. Následující poznámky se týkají verze 9.5 .
10.1.2.1 Knihovny pro práci s maticemi
Jádro programu Maple neumí přímo pracovat s maticemi, musíme použít rozšiřující balíček, který je součástí instalace. V současné době máme dvě možnosti:
• balíček l inalg
• balíček LinearAlgebra - novější, podle dokumentace programu nahrazuje linalg, nemá však implementovány některé užitečné příkazy (např. j acobian) .
Matice a vektory jsou v obou balíčcích tvořeny jinými objekty, lze však mezi nimi provádět konverze. Doporučujeme primárně používat novější LinearAlgebra a pouze v případě potřeby přecházet ke staršímu linalg.
10.1 .2 .2 Derivace a substituce
Pro derivaci výrazů použijeme funkci diff . Např. :
x : = L*sin (phi (t) ) ; v : = diff ( x (t ) , t) ; % derivuj eme podle času
Proměnná, podle které derivujeme, nesmí být funkcí času, např.
146 KAP. 1 0 : PŘÍLOHY
y : = diff ( 2*x (t) , x (t) ) % nelze
nebude fungovat. V případě, že chceme použít Maple pro odvození pohybových rovnic pomocí LR II. druhu, musíme kinetickou energii nejprve derivovat podle rychlosti (není funkce času) a následně výsledný výraz podle času (rychlost je funkce času) . Řešením je použití substituce:
v : = subs (phi (t) =phi , v) ;
Konkrétní ukázku najdete v příkladu 10. 1 . 2 .4 .
10.1.2.3 Zjednodušování výrazů
Pro zjednodušování algebraických výrazů můžeme použít následující funkce (detailní popis a syntaxe v nápovědě k programu) :
• simplify (x)
• expand (x)
• combine (x)
• collect (L , x , y) - ve výrazu L vyhledá všechny členy obsahující x a y a vytkne je před závorku - velmi užitečné
• coeff (L , x) - doplněk k předchozímu příkladu, vrátí obsah závorky, která je násobená x.
Při zjednodušování komplikovanějších výrazů se musíme smířit s omezenými možnostmi algoritmů. Někdy je obtížné donutit program ke zjednodušení, které uživatel jasně vidí a nezbývá než použít substituci a "napovědět" tak programu.
V každém případě jsou ale možnosti programu obrovské a ve srovnání s "tužkou a papírem" značně posouvají hranice praktického použití metod popsaných v tomto textu.
10.1.2.4 Příklad: Matematické kyvadlo - sestavení dyn. modelu pomocí LR I I .
Uvažujme matematické kyvadlo (hmotný bod na nehmotné tyčce) . Všimněme si , jak při derivaci kinetické energie přecházíme pomocí substituce mezi závislostí a nezávislostí na čase.
restart : # Definice kartézských souřadnic : x : = Usin (phi ( t ) ) ; y : = -L*cos (phi (t) ) ;
1 0 . 1 : SOFTWAROVÉ NÁSTROJE
# Výpočet derivací (rychlostí) : dx : = diff (x , t ) j dy : = diff (y , t ) j # Kinetická energie Ek : = 1/2*m* (dx-2 + dy-2) j Ek : = simplify (Ek) j # Potenciální energie : Ep : = m*g*Y j # Odvození LR2 (tři výrazy v rovn�c� J Sou označeny LR1 - LR3) : # Substituce kv�li derivaci podle rychlost i : Ek : = subs (diff (phi (t) ,t )=dphi , Ek) j LR1 : = diff (Ek , dphi) j # Substituce k�li derivaci podle času : LR1 : = subs (dphi=diff (phi (t) , t ) , LR1) j LR1 : = diff (LR1 ,t ) j LR2 : = subs (phi (t) =phi , Ek) j LR2 : = diff (LR2 , phi) j LR3 : = subs (phi (t) =phi , Ep) j LR3 : = diff (LR3 , phi) j # Celková rovnice : Rovnice : = LR1 - LR2 + LR3 O j
10.1 .3 Matlab
147
Matlab je dnes standardním průmyslovým i univerzitním jazykem pro provádění vědecko-technických výpočtů. Studenti se s Matlabem setkávají v řadě jiných kurzů, proto ho zde budeme charakterizovat pouze velmi stručně.
Základem Matlabu (MATrix LABoratory) je efektivní2 (numerická) práce s maticemi. Syntaxe jazyka je jednoduchá, uživatel nemusí deklarovat datové typy. Již v základní distribuci je k dispozici mnoho funkcí pro práci s maticemi (vlastní čísla, dekompozice, maticová exponenciála, vyhledávání, speciální matice atd . ) . Mimo tyto základní vlastnosti zmíníme ještě:
• 2D a 3D vykreslování funkcí (grafika) , snadno použitelné
• řešení obyčejných diferenciálních rovnic (ODE) , různé řešiče • integrace se simulačním prostředím Simulink
• velké množství volně dostupných i komerčních rozšiřujících toolboxů
• velmi dobrá dokumentace. 2Významný je např. princip vektorizace. Určitá operace s jednotlivými prvky rozsáhlé
matice probíhá podstatně rychleji než cyklické procházení matice. Většina vnitřních funkcí je takto navržena a uživatel může své funkce také psát vektorizovaně.
148 KAP. 10 : PŘÍLOHY
10. 1 .4 Matlab + Robotic Toolbox
Volně dostupný Robotic Toolbox for Matlab vyvinul Peter I. Corke [4] . Slouží pro modelování kinematiky a dynamiky otevřených kinematický řetězců, tedy např. průmyslových robotů. Patří do kategorie nástrojů, které automaticky sestavují kinematický/dynamický model na základě uživatelem definované soustavy. Pro průmyslové roboty se standardně používá Denavit-Hartenbergova notace (kap. 2 .4) , která je použita i v tomto toolboxu. Mimo základní funkce pro přímou a inverzní kinematiku a dynamiku obsahuje toolbox i různé funkce pro prostorové kinematické výpočty a práci s mechanismy (rotační matice, kvaterniony, výpočet Yoshikawový míry manipulovatelnosti a další) . Užitečná je možnost grafického zobrazení vytvořeného robotu a jeho interaktivní ovládání .
Základem pro práci s toolboxem jsou dvě funkce:
• linkl :;; link ( [alpha , a , theta , dJ ) - definice jednoho tělesa ( I i nk)
• myrobot :;; robot (linkl , link2) - definice robotu (v tomto případě složeného ze dvou těles)
10.1.4.1 Příklad: Přímá a inverzní úloha rovinného manipulátoru RR
V tomto příkladu ukážeme možnost vytvoření modelu a řešení přímé a inverzní úlohy kinematiky pro manipulátor z příkladu 4 . l .
Nejprve definujeme soustavu:
cle , clear % Definuj eme " links " linkl :;; link ( [O L l O OJ ) link2 :;; link ( [O L2 O OJ ) % Vytvorime robot robot�planar_RR :;; robot ({linkl , l ink2})
Řešení přímé kinematiky provedeme takto:
q :;; [0 . 1 0 . 2J % kloubove souradnice T :;; fkine (robot_planar_RR , q) % vysledkem j e transformacni mat i ce x :;; T ( 1 : 3 , 4) % poloha bodu E j e subvektorem T
Inverzní kinematika:
qO :;; [0 . 1 0 . 2J ; % Pocatecni poloha robotu x_desired :;; [1 . 2 ; 0 . 2J % Cilova poloha bodu E [x yJ T_desired :;; eye (4 , 4) ;
10 . 1 : SOFTWAROVÉ NÁSTROJE
T_desired C l : 2 , 4) = x_desired % Cil definuj eme pomoci T M = [ 1 1 O O O O] % Vahovy vektor [x y z rotx roty rotz] : % O pokud neni dana souradnice vazana (unconstrained)
149
q = ikine Crobot_planar_RR , T_desired , qO , M) % inverzni kinematika plot Crobot_planar_RR , q) % vykresleni robotu ve vysledne poloze
Vidíme, že s pomocí Robotic Toolboxu můžeme vyřešit úlohy kinematiky bez nutnosti definice kinematického modelu, software to provede za nás. Zároveň je ale dobře patrné, že bez poměrně detailních znalostí problematiky (transformační matice) není možné tento nástroj používat.
10. 1 .5 SimMechanics
Simulink je v současné době standardním nástrojem pro simulaci dynamických systémů, zejména se zaměřením na návrh řízenÍ .
SimMechanics je rozšíření Simulinku vytvořené speciálně pro modelování MBS. Obsahuje knihovny bloků odpovídající reálným částem mechanismů (např. tělesa, vazby mezi nimi, akční členy, senzory, definici fyzikálního prostředí a další) . Uživatel pak s těmito bloky pracuje na fyzikální úrovni, jejich spojováním vytváří model reálného technického objektu. Na základě definovaných parametrů a propojení těchto "fyzikálních" bloků je pak automaticky vytvořen matematický model pro Simulink. Fyzikální signály (polohu, rychlost, sílu apod.) můžeme pomocí bloku Sensor převést na signály běžného Simu1inku a libovolně s nimi dále pracovat. Mohou tedy vstupovat do j iných submodelů, např. elektrických, tepelných, hydraulických či řídicích. Zpětně je možno výsledky výpočtů převést blokem Actuator na fyzikální signál (sílu, moment) a působit tak na mechanismus. SimMechanics tedy spojuje dvě podstatné výhody:
• automaticky vytváří dynamický model MBS na základě topologie a geometrie
• umožňuje přímé propojení na modely jiné fyzikální podstaty.
Díky popsaným vlastnostem je možné díky SimMechanics a Simulink sestavovat i poměrně komplikované modely prostorových mechatronických soustav včetně submodelů elektrických pohonů a algoritmů počítačového řízení.
Více informací najde čtenář v samostatné publikaci Modelování mechatroníckých systémů v MatlabjSímMechanícs [7] .
150 KAP. 1 0 : PŘíLOHY
10.1 .6 Adams
Jedním z nejznámějších komerčních nástrojů pro modelování MBS je program Adams americké firmy MSC Software. Firma vyvinula společně s NASA v r. 1965 program Nastran (NASA Structural Analysis Program) a v současné době nabízí řešení MD Nastran (multi discipline) , které integruje téměř všechny myslitelné výpočetní úlohy do jednoho prostředí (Nastran = metoda konečných prvků pro lineární a nelineární strukturální výpočty, Marc = nelineární strukturální výpočty, Dytran = interakce tekutin a pevných těles, Adams a mnoho dalších) .
Program Adams umožňuje řešit úlohy statiky, kinematiky a dynamiky MBS. Mimo základní úlohy můžeme do modelu zahrnout kontakty mezi tělesy a deformovatelná tělesa. Výsledky výpočtů lze různými způsoby vykreslovat a také exportovat do j iných programů MSC. Propracovaná je možnost optimalizace parametrů soustav.
Pro Adams je také vyvinuta řada specializovaných modulů, především pro automobilový průmysl (Adams/Car, /Car Vehicle Dynamics, /Car Suspension Design, /Tire a další) .
10.2 Matematika
10.2 .1 Trigonometrie
• Zjednodušování funkcí s jedním úhlem
sin(-u) = - sin u cos( -u) = cos a
. 7r sm(a + 2") = cos a
tan(a - 7r) = tan a sin2 u + cos2 U = 1
sin 2a = 2 sin a cos a cos 2a = cos2 a - sin2 a
( 10 . 1 ) ( 10 .2)
( 10.3)
( 10.4) (10 .5) ( 10.6) (10 .7)
10 . 2 : MATEMATIKA
• Součtové vzorce
sin( a ± {J) = sin a cos {J ± cos a sin {J cos( a ± {J) = cos a cos {J =F sin a sin {J . ± . R 2 ' a ± {J a =F f3
SIn a sm f/ = sIn -2
- cos -2-
a + {J a - {J cos a ± cos {J = ±2 cos -
2- cos -
2-
151
(10.8) (10.9)
(10. 10)
(10. 11 )
• Kosinová věta - a , b a c jsou strany trojúhelníka, , je úhel naproti straně c, pak platí
10.2 .2 Ska lární součin
Skalární součin dvou vektorů vypočteme v maticovém vyjádření takto:
10.2 .3 Operace s maticemi
• Pravidla pro součty a násobení matic
A I = I A = A A(B + C) = AB + AC (B + C)A = BA + CA
A + B = B + A AB -=J BA
• Operace s transpozicemi
(ABf = BT AT (A + Bf = AT + BT
IAI = IAT I (ATf = A
(10. 12)
(10. 13)
(10. 14) (10 . 15) (10. 16) (10 . 17) (10 . 18) (10 . 19)
(10.20)
(10.21)
( 10.22)
(10.23)
152 KAP. 10: PŘÍLOHY
• Inverze matice - je definována pouze pro čtvercovou matici
(A-1t1 = A AA-1 = I
C = AB C-1 = (AB)-l = B-1 A-I
(10 .24) ( 10 .25) (10 .26) (10.27)
10.2.4 Speciální matice
• Singulární matice - platí, že JA J = O
• Regulární matice - platí, že lA l =1= O
• Jednotková matice I
[1 O
I = O 1
O O
O O
O
• Symetrická matice je definována takto
• Antisymetrická maticé je definována takto
(10 .28)
(10 .29)
( 10 .30)
Zavedeme operátor S, který z vektoru a vytváří antisymetrickou matici takový, že
• Ortogonální matice - platí pro ni, že:
RT = R-1
RTR = RRT = I
3Také polosouměrná matice, angl. skew symmetric matrix.
( 10.31)
( 10.32)
( 10 .33)
(10 .34)
10 .3 : OZNAČOVÁNÍ VELIČIN
10.2.5 Vektorový součin
Vektorový součin
c = či x b maticově zapíšeme takto
Přitom platí:
c = Ab = S(a)b
S(aa + ,Bb) = aS(a) + ,BS(b)
RS(a)RT = S(Ra) R(a x b) = Ra x Rb
153
( 10.35)
(10 .36)
(10.37)
( 10.38) (10.39)
kde R je ortogonální matice a A antisymetrická matice. Pak také platí:
R = exp (A) ( 10.40)
kde expO je maticová exponenciála.
10.3 Označování veličin
Text skript zahrnuje poměrně širokou problematiku mechaniky tuhých těles a volba označení jednotlivých veličin představuje nelehký úkol. Identickými písmeny jsou často zapisovány různé veličiny, především ale najdeme prakticky v každé publikaci vlastní způsob označování .
V těchto skriptech jsme se přiklonili k označování obvyklému spíše v angl. literatuře (a počítačových programech) .
V celém textu platí konvence v označování maticových veličin (tučně a velkým písmenem) a vektorových veličin (tučně a malým písmenem) .
U veličin "s mnoha indexy" , např. v kinetostatice, je snahou dodržet tuto konvenci: Xti)b je veličina (např. poloha, rychlost, síla, moment) X bodu (tělesa) M vztažená k bodu A vyjádřená v souřadnicovém systému b.
Seznam nejdůležitějších veličin je na následující straně.
154
Seznam nejdůležitějších veličin
lL T Aba B(q) C(q, ČJ) Cep C12 Dba Eba f f(P)i g(q) I J ID(p)'i NfL
1 q R Rba
8ep 812 Gj
S(Tj)i
S(P)i Tba
· VM a Vba Zi
vektor zobecněných sil působících na koncový efektor antisymetrická matice vektoru Wba , který popisuje úhlovou rychlost tělesa vektor momentů působících na koncový efektor vektor sil resp. momentů v kloubových souřadnicích matice zrychlení matice setrvačných účinků v rovnici dynamiky MBS matice účinků závislých na rychlosti v rovnici dynamiky MBS zjednodušený zápis cos 'P zjednodušený zápis cos( 'Pl + 'P2) diferenciální operátor antisymetrická matice vektoru úhlového zrychlení tělesa e vektor vnějších sil působících na koncový efektor vektor síly vztažený k bodu P vyjádřený v syst . i vektor tíhových účinků v rovnici dynamiky MBS jednotková matice jakobián, také Jacobiho matice vektor momentu vztažený k bodu P vyjádřený v syst. i matice E ffi.6X6 pro staticky ekvivalentní změnu vztažného bodu vektor zobecněných kloubových souřadnic rotační matice rotační matice vyjadřující natočení systému b vůči a vyjádřená v systému a rozšířená rotační matice (dvojnásobná) poloha bodu M v syst , b, také homogenní souřadnice (vektor rozšířený o 1 ) zjednodušený zápis sin 'P zjednodušený zápis sin( 'Pl + 'P2) silový bivektor vyjadřující vliv tíhy tělesa j vztažený k těžišti tělesa Tj vyjádřený v syst. i silový bivektor vztažený k bodu P vyjádřený v syst. i transformační matice ze syst. b do syst. a rychlost bodu M vyjádřená (viděná) v syst. a rychlostní matice vektor rotace (translace) i-tého kloubu
Rejstřík
Adams, 146
Cardanovy úhly, 34 Christoffelovy symboly, 101 computed torque, 13 Computer Algebra System (CAS) , 140
d'Alembertův princip, 93 DAE, 92 Denavit-Hartenbergovy parametry, 37,
74, 77, 144 DH parametry, viz Denavit-Hart. par. diferenciální operátor, 30 DLS (damped least squares) , 51 dráha, 133 dynamika
inverzní, 93 přímá, 93
elipsoid manipulovatelnosti , viz manipulovatelnost elipsoid
Eulerovy úhly, 31 RPY, 33
gain scheduling, 13
homogenní souřadnice, 27
inverzní matice, viz matice inverze inverzní úloha kinematiky, viz kinema
tika inverzní
jakobián, 46 analytický, 46 geometrický, 46 inverze, 49 numerický, 47
pseudoinverze, 50 transpozice, 50
kinematika inverzní, 48, 61 , 69 , 76, 87 přímá, 41, 58, 67, 74, 86
konvergence inverzní úlohy, 49, 51 , 64 kvadratická forma, 53 kvaternion, 36
linearizace, 13 lineárně implicitní ODE, 102
manipulovatelnost, 66, 72 elipsoid, 53 rychlostní, 53 silová, 55 Yoshikawova míra, 54
manipulátor redundantní, 48 uzavřený, 82
Maple, 141 matice
antisymetrická, 94, 148, 149 ortogonální , 21 , 149 polosouměrná, viz matice antisy-
metrická rotační , 20
derivace, 22 inverze, 21 , 22
rychlostní, 29 transformační , 27
derivace, 28 inverze, 28
Matlab, 143
156
metoda rozpojené smyčky, 82 metoda tlumených nejmenších čtverců,
viz DLS (damped least squares)
metoda uzavřené smyčky, 82 MIMO systém, 12, 13
neholonomní soustavy dynamika, 128 kinematika, 121
ODE, 91 ortogonální transformace vektor. veli
čin, 42
plně implicitní ODE, 98 přímá úloha kinematiky, vzz kinema
tika přímá
redukce dynamického modelu, 131 Robotic Toolbox, 144 rychlost v prostoru, 23
silový bivektor, 94 silový elipsoid, 55 SimMechanics, 17, 145 singularita
Eulerových úhlů, 35 poloha manipulátoru, 51
BlBa systém, 12 souřadnice
kartézské, 41 kloubové, 41
statika otevřeného manipulátoru, 52, 65
stopa matice, 36
trajektorie, 133 transformační matice, viz matice trans
formační
vazba
holonomní, 1 19 neholonomní, 121
vektor a úhel, 35 víceznačnost řešení , 48
zrychlení v prostoru, 24
Použitá l iteratura
[1] Brát, V. : Maticové metody v analýze a syntéze prostorových vázaných mechanických systémů. Academia, Praha, 1 . vyd. , 1981.
[2] Buss, S. R. : Introduction to inverse kinematics with jacobian transpose, pseudoinverse and damped least squares methods, 2004.
[3] Caracciolo, L.; Luca, A. D. ; Iannitti, ·S . : Trajectory Tracking Control of a FourWheel Differentially Driven Mobile Robot. In ICRA, 1999, s. 2632-2638.
[4] Corke, P. : A Robotics Toolbox for MATLAB. IEEE Robotics and Automation Magazine, ročník 3, č. 1, Březen 1996: s. 24-32.
[5] Georgi, W. : Study material about Kinematics and Dynamics. 2005. URL http : //www . hs-weingarten . de/-georgi/robotik/public_html/ script/bsp_vorl . html
[6] Gholipour, A. ; Yazdanpanah, M. : Dynamic tracking control of nonholonomic mobile robot with model reference adaptation for uncertain parameters. In ECC2003: European Control Conjerence, Cambridge, UK, Sep. 2003.
[7] Grepl, R. : Modelování mechatronických systémů v MatlabjSimMechanics. BEN - technická literatura, 2007.
[8] Kratochvíl, C . ; Slavík, J . : Mechanika těles - dynamika. PC-DIR, skriptum VUT Brno, 1997.
[9] Loprais, A.: Mechanika manipulačních zařízení (PRaM). Vysoké učení technické, Brno, 1989.
[10] Murray, R. M.; Sastry, S . S.; Zexiang, L. : A Mathematical Introduction to Robotic Manipulation. Boca Raton, FL, USA: CRC Press, Inc. , 1994, ISBN 0849379814.
[ 11] Oh, K. M.-S. L. J., C. : Control of nonholonomic mobile robot using an RBF network. Artificial Lije Robotics, ročník 8, 2004: s. pp . 14-19 .
[12] Rektorys, K. : Přehled užité matematiky. SNTL, 1973.
158
[13] Robohemia: první český robotický fotbalový tým, webové stránky. 2007. URL http : //www . robohemia . cz/
[14] Rybička, J . : LaTeX pro začátečm'ky. KONVOJ, ISBN 80-7302-049-1 , 2003.
[15J Schwerin, R. : MultiBody System SIMulation. Numerical Methods, Algorithms, and Software. Lecture Notes in Computational Science and Engineering Vol. 7,
, Heidelberg: Springer, 1999.
[16] Sciavicco, L.j Siciliano, B.j S ciavicco , B . : Modelling and Control oj Robot Manipulators. Secaucus, NJ, USA: Springer-Verlag New York, Inc. , 2000, ISBN 1852332212.
[17J Slavík, J. j Stejskal, v. ; Zeman, V. : Základy dynamiky strojů. Vydavatelství ČVUT, Praha, Vyd. 1 . , ISBN: 80-01-01622-6 , 1997.
[18] Spong, M. W. ; Hutchinson, S . j Vidyasagar, M.: Robot Modeling and Control. New York: Wiley, 2005.
[19] Stejskal, v.; kol. : Mechanics Using Matlab, Leonardo Pilot project No. CZ/98/1/82500/PI/I .1 .1 .b/FPI. 2001 . URL http : //fsinet . fsid . cvut . cz/en/U2052/1eo . html
[20J Stejskal, V. ; Valášek, M. : Kinematics and dynamics oj machinery. Marcel Dekker, New York, 1996.
[21] Valášek, M.: Mechatronika. Praha : České vysoké učení technické, 1. vyd. , ISBN: 80-01-01276-X, 1995.
[22J Wood, G. D. : Simulating Mechanical Systems in Simulink with SimMechanics. Technická zpráva, The MathWorks, Inc., 3 Apple HiU Drive, Natick, MA, USA www.mathworks.com. 2003.
[23] Zboray, L . ; Ďurovský, F. : Stavové riadenie elektrických pohonov. Edícia vedeckých spisov FEI Košice, Vienala, 1995.
Tato publikace popisuje základní metody řešení úloh kinematiky a dynamiky prostorových soustav tuhých těles (MBS) s důrazem na aplikaci na počítači. Teoretický výklad maticových metod a algoritmů je doplněn řadou řešených příkladů. Získané poznatky mohou být užitečné nejen přímo pro řešení úloh popsanými metodami, ale i při práci s hotovými komerčními programy pro MBS typu Matlab/SimMechanics nebo MSC.Adams.
Na webu autora http://www. umtjme.vutbr. cz/�rgrepl/jsou k dispozici soubory pro Matlab a Maple a další informace.
Publikace je určena především studentům special izace Mechatronika na Ústavu mechaniky těles, mechatroniky a biomechaniky FSI VUT v Brně, ale také dalším zájemcům o popisovanou problematiku.