´ Uvod do mechatroniky, robotiky a syst´ em˚ uˇ r´ızen´ı pohybu
ˇ Martin Goubej, Martin Svejda, Miloˇs Schlegel Katedra Kybernetiky Z´apadoˇcesk´a univerzita v Plzni
Skriptum pro studenty doktorsk´ ych program˚ u v oboru automatick´e ˇr´ızen´ı Kvˇeten 2012
Abstrakt
C´ılem tohoto textu je poskytnout vˇseobecn´ y pˇrehled problematiky n´avrhu syst´em˚ u automatick´eho ˇr´ızen´ı pro mechatronick´e aplikace. V u ´vodu jsou nast´ınˇeny z´ akladn´ı okruhy probl´em˚ u, d´ale je pod´an struˇcn´ y pˇrehled typick´ ych aplikac´ı. Druh´ a kapitola je vˇenov´ ana detailn´ımu popisu obecn´eho syst´emu ˇr´ızen´ı pohybu. Je definov´ ana u ´loha ˇr´ıdic´ıho syst´emu a jeho ˇclenˇen´ı do dvou z´akladn´ıch vrstev gener´ atoru trajektorie a regul´atoru pohybu. Pro obˇe tyto u ´rovnˇe je uveden pˇrehled probl´em˚ u spojen´ ych s jejich n´avrhem. Pozornost je venov´ ana zejm´ena problematice generov´an´ı trajektorie. Pops´any jsou v praxi nejˇcastˇejˇs´ı u ´lohy ˇr´ızen´ı pohybu v jedn´e ose, synchronizovan´eho pohybu s elektronickou vaˇckou a koordinovan´eho pohybu ve v´ıce os´ach. Diskutov´any jsou moˇznosti pouˇzit´ı po ˇc´astech polynomi´aln´ıch a racion´aln´ıch interpolac´ı ˇ sen je probl´em flukve formˇe spline funkc´ı pro definici trajektori´ı pohybu. Reˇ tuac´ı rychlosti posuvu zp˚ usoben´ y nepˇrirozenou parametrizac´ı interpolaˇcn´ı funkce. V z´ avˇeru kapitoly jsou uvedeny z´akladn´ı pˇr´ıstupy k ˇreˇsen´ı probl´emu ˇcasovˇe optim´ aln´ıho ˇr´ızen´ı pro v´ıceos´e syst´emy. Tˇret´ı kapitola je pojata jako obecn´ y u ´vod do robotiky se zamˇeˇren´ım na pr˚ umyslov´e robotick´e manipul´atory. V u ´vodu jsou prezentov´any z´akladn´ı ˇ sen je probl´em urˇcen´ı pˇr´ıstupy k modelov´ an´ı a popisu pohybu robot˚ u. Reˇ polohov´ ych z´ avislost´ı manipul´ator˚ u a metody pro v´ ypoˇcet pˇr´ım´e a inverzn´ı kinematick´e u ´lohy. Srovn´av´any jsou s´eriov´e a paraleln´ı architektury manipul´ ator˚ u. Dalˇs´ı ˇc´ ast kapitoly je vˇenov´ana diferenci´aln´ı kinematice a ˇreˇsen´ı pohybov´ ych z´ avislost´ı rychlost´ı a zrychlen´ı pro s´eriov´e a paraleln´ı manipul´ atory. Pozornost je d´ale vˇenov´ana singul´arn´ım poloh´am. V z´avˇeru jsou prezentov´ any numerick´e metody pro ˇreˇsen´ı u ´loh inverzn´ı kinematiky manipul´ ator˚ u.
Skriptum slouˇz´ı jako doplˇ nkov´ y text pro studenty doktorsk´ ych program˚ u v oboru automatick´eho ˇr´ızen´ı na Katedˇre Kybernetiky s c´ılem poskytnout vˇseobecn´ yu ´vod do rozs´ ahl´e problematiky n´avrhu mechatronick´ ych syst´em˚ u. Detaily jednotliv´ ych metod a algoritm˚ u, pˇr´ıpadnˇe pˇr´ıklady konkr´etn´ıch aplikac´ı lze nal´ezt v literatuˇre, jej´ıˇz v´ yˇcet je uveden v seznamu referenc´ı.
vi
Obsah Seznam obr´ azk˚ u
xi
1 Mechatronika - oblast aplikac´ı vloˇ zen´ eho ˇ r´ızen´ı
1
ˇ ızen´ı pohybu v mechatronick´ 2 R´ ych syst´ emech ´ 2.1 Uvod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7 7
2.2
Senzory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8
2.3
Aktu´ atory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11
2.4
Elektrick´e pohony jako aktu´atory . . . . . . . . . . . . . . . . . . . . . .
13
2.5
Automatick´ a regulace v syst´emech ˇr´ızen´ı pohybu . . . . . . . . . . . . .
17
2.5.1
Gener´ ator trajektorie
. . . . . . . . . . . . . . . . . . . . . . . .
18
2.5.2
Regul´ ator pohybu . . . . . . . . . . . . . . . . . . . . . . . . . .
21
Robotick´e manipul´ atory . . . . . . . . . . . . . . . . . . . . . . . . . . .
23
2.6.1
Struktura robotick´ ych manipul´ator˚ u . . . . . . . . . . . . . . . . ˇ R´ızen´ı pohybu robotick´ ych manipul´ator˚ u . . . . . . . . . . . . .
25
CNC obr´ abˇec´ı syst´emy . . . . . . . . . . . . . . . . . . . . . . . . . . . .
29
3 Generov´ an´ı trajektorie pohybu ˇ ızen´ı pohybu v jedn´e ose . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1 R´ ˇ 3.1.1 Casovˇ e optim´ aln´ı pohyb z bodu do bodu . . . . . . . . . . . . . .
33
2.6
2.6.2 2.7
3.1.2 3.2 3.3
27
33 34
Spojit´ y pohyb po zadan´e trajektorii . . . . . . . . . . . . . . . .
38
Synchronizovan´ y pohyb - elektronick´e vaˇcky a pˇrevodovky . . . . . . . .
47
3.2.1
Synt´eza profilu elektronick´e vaˇcky . . . . . . . . . . . . . . . . .
48
Koordinovan´ y pohyb ve v´ıce os´ach . . . . . . . . . . . . . . . . . . . . .
51
3.3.1
Souˇradn´e syst´emy a jejich transformace . . . . . . . . . . . . . .
53
3.3.2
Druhy pohybu u v´ıceos´ ych syst´em˚ u. . . . . . . . . . . . . . . . .
56
vii
OBSAH
3.3.3
Metody interpolace pro popis trajektori´ı pohybu . . . . . . . . .
58
3.3.4
Omezen´ı fluktuac´ı zp˚ usoben´ ych nepˇrirozenou parametrizac´ı . . . ˇ Casovˇ e optim´ aln´ı pohyb pod´el zvolen´e trajektorie . . . . . . . . .
71
3.3.5
´ 4 Uvod do robotiky
76 91
4.1
Robotika jako v´ yznamn´ y obor mechatroniky . . . . . . . . . . . . . . . .
91
4.2
Robotick´e manipul´ atory . . . . . . . . . . . . . . . . . . . . . . . . . . .
92
4.3
C´ıle pr´ ace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
99
4.4
Pouˇzit´e architektury manipul´ ator˚ u . . . . . . . . . . . . . . . . . . . . . 101
4.5
4.4.1
Antropomorfn´ı manipul´ator se sf´erick´ ym z´apˇest´ım (AM+SZ) . . 101
4.4.2
Paraleln´ı plan´ arn´ı manipul´ator (PPM) . . . . . . . . . . . . . . . 102
4.4.3
Paraleln´ı sf´erick´e z´ apˇest´ı (PSZ) . . . . . . . . . . . . . . . . . . . 103
Kinematika manipul´ ator˚ u: Modelov´an´ı, polohov´e z´avislosti . . . . . . . . 106 4.5.1
Reprezentace obecn´eho pohybu v robotice . . . . . . . . . . . . . 107 4.5.1.1
4.5.2
4.6
Reprezentace polohy . . . . . . . . . . . . . . . . . . . . 107
4.5.1.2 Reprezentace rychlosti a zrychlen´ı . . . . . . . . . . . . 114 ´ Umluvy pro popis kinematiky manipul´ator˚ u . . . . . . . . . . . . 116 4.5.2.1
Denavit-Hartenbergova u ´mluva (D-H) . . . . . . . . . . 116
4.5.2.2
Khalil-Kleinfingerova u ´mluva (K-K) . . . . . . . . . . . 120
Polohov´e z´ avislosti manipul´ ator˚ u . . . . . . . . . . . . . . . . . . . . . . 128 4.6.1
Pˇr´ım´ y kinematick´ y probl´em pro s´eriov´e manipul´atory . . . . . . 128
4.6.2
Inverzn´ı kinematick´ y probl´em pro s´eriov´e manipul´atory . . . . . 131 4.6.2.1
Pˇr´ım´e analytick´e ˇreˇsen´ı jednoduch´ ych architektur manipul´ ator˚ u . . . . . . . . . . . . . . . . . . . . . . . . . 131
4.6.2.2
Specializovan´e metody pro ˇreˇsen´ı konkr´etn´ıch variant architektur manipul´ator˚ u . . . . . . . . . . . . . . . . . 139
4.6.2.3 4.7
Metody pro ˇreˇsen´ı obecn´ ych architektur manipul´ator˚ u . 143
Paraleln´ı manipul´ atory . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147 4.7.1
Dekompozice paraleln´ıho mechanismu na nez´avisl´e uzavˇren´e kinematick´e ˇretˇezce . . . . . . . . . . . . . . . . . . . . . . . . . . . 147
4.7.2
4.7.1.1
Pˇr´ım´ y kinematick´ y probl´em . . . . . . . . . . . . . . . . 152
4.7.1.2
Inverzn´ı kinematick´ y probl´em
. . . . . . . . . . . . . . 152
Dekompozice paraleln´ıho mechanismu na s´eriov´e manipul´atory . 153
viii
OBSAH
4.8
4.9
4.7.2.1
Inverzn´ı kinematick´ y probl´em
. . . . . . . . . . . . . . 155
4.7.2.2
Pˇr´ım´ y kinematick´ y probl´em . . . . . . . . . . . . . . . . 159
Kinematika manipul´ ator˚ u: Z´avislosti rychlost´ı a zrychlen´ı . . . . . . . . 186 4.8.1
Z´ avislosti rychlosti pro s´eriov´e manipul´atory . . . . . . . . . . . 188
4.8.2
Z´ avislosti zrychlen´ı pro s´eriov´e manipul´atory . . . . . . . . . . . 194
4.8.3
Z´ avislosti rychlosti a zrychlen´ı pro paraleln´ı manipul´atory . . . . 200
4.8.4
Singul´ arn´ı polohy manipul´atoru . . . . . . . . . . . . . . . . . . . 203
Numerick´ y pˇr´ıstup ˇreˇsen´ı kinematiky manipul´ator˚ u . . . . . ´ pro neredundantn´ı manipul´atory . . . . . . . . 4.9.1 IKU ´ pro redundantn´ı manipul´atory . . . . . . . . . . 4.9.2 IKU ´ pro polohy manipul´atoru v bl´ızkosti singularit . 4.9.3 IKU 4.9.4
. . . . . . . 207 . . . . . . . 209 . . . . . . . 211 . . . . . . . 214
Shrnut´ı . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 216
5 Z´ avˇ er
221
Literatura
223
ix
OBSAH
x
Seznam obr´ azk˚ u 1.1
Mechatronika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
2
1.2
Aplikace mechatronick´ ych syst´em˚ u . . . . . . . . . . . . . . . . . . . . .
3
1.3
Aktivn´ı tlumen´ı vibrac´ı . . . . . . . . . . . . . . . . . . . . . . . . . . .
5
1.4
Hardware in the loop . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
2.1
Struktura syst´emu ˇr´ızen´ı pohybu . . . . . . . . . . . . . . . . . . . . . .
8
2.2
Inteligentn´ı senzory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
2.3
11
2.4
Pohony a aktu´ atory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ´ Urovnˇ e ˇr´ıdic´ıho syst´emu . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.5
Struktura regul´ atoru pohybu . . . . . . . . . . . . . . . . . . . . . . . .
21
2.6
25
2.7
S´eriov´ a a paraleln´ı struktura robotick´ ych manipul´ator˚ u. . . . . . . . . . ˇ ızen´ı robotick´ R´ ych manipul´ator˚ u . . . . . . . . . . . . . . . . . . . . . .
2.8
CNC obr´ abˇec´ı syst´emy . . . . . . . . . . . . . . . . . . . . . . . . . . . .
29
3.1
ˇ Casovˇ e optim´ aln´ı pohyb v jedn´e ose . . . . . . . . . . . . . . . . . . . .
38
3.2
Pohyb po spojit´e trajektorii . . . . . . . . . . . . . . . . . . . . . . . . .
39
3.3
B´ azov´e funkce pro kvintick´ y uniformn´ı B-spline . . . . . . . . . . . . . .
45
3.4
Elektronick´ a vaˇcka a pˇrevodovka . . . . . . . . . . . . . . . . . . . . . .
47
3.5
Camedit - editor vaˇckov´ ych profil˚ u . . . . . . . . . . . . . . . . . . . . .
51
3.6
Souˇradn´e syst´emy v´ıceos´eho stroje . . . . . . . . . . . . . . . . . . . . .
53
3.7
Pohyby v´ıceos´ ych syst´em˚ u . . . . . . . . . . . . . . . . . . . . . . . . . .
56
3.8
Pohyby typu Multi-point . . . . . . . . . . . . . . . . . . . . . . . . . . . ˇ ızen´ı s´ıly . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . R´
57 58
3.10 Konstrukce B-spline b´azov´ ych funkc´ı . . . . . . . . . . . . . . . . . . . .
64
3.11 Dvourozmˇern´ a NURBS funkce . . . . . . . . . . . . . . . . . . . . . . .
65
3.9
xi
17
28
´ ˚ SEZNAM OBRAZK U
3.12 NURBS interpolace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
70
3.13 Korekˇcn´ı polynomi´ aln´ı funkce pro NURBS . . . . . . . . . . . . . . . . .
75
3.14 Generov´ an´ı profilu posuvu . . . . . . . . . . . . . . . . . . . . . . . . . . ˇ 3.15 Casovˇ e optim´ aln´ı profil posuvu . . . . . . . . . . . . . . . . . . . . . . . ˇ 3.16 Casovˇ e optim´ aln´ı pohyb . . . . . . . . . . . . . . . . . . . . . . . . . . .
82 85
4.1
Pouˇz´ıvan´e typy kloub˚ u (zleva P, R, U, S) . . . . . . . . . . . . . . . . .
93
4.2
Prvn´ı s´eriov´ y pr˚ umyslov´ y manipul´ator Unimate . . . . . . . . . . . . . .
96
4.3
Pravdˇepodobnˇe prvn´ı paraleln´ı manipul´ator Jamese Gwinnetta . . . . .
97
4.4
Goughova-Stewartova platforma (1954) a Stewartova platforma (1965) .
98
4.5
Nejzn´ amˇejˇs´ı typy pr˚ umyslov´ ych manipul´ator˚ u . . . . . . . . . . . . . . . 100
4.6
Antropomorfn´ı manipul´ ator se sf´erick´ ym z´apˇest´ım . . . . . . . . . . . . 102
4.7
Paraleln´ı plan´ arn´ı manipul´ ator . . . . . . . . . . . . . . . . . . . . . . . 103
4.8
Paraleln´ı sf´erick´e z´ apˇest´ı
4.9
Vyuˇzit´ı paraleln´ıho sf´erick´eho z´apˇest´ı (PM) v pr˚ umyslov´e aplikaci. . . . 105
88
. . . . . . . . . . . . . . . . . . . . . . . . . . 104
4.10 Transformace souˇradn´ ych syst´em˚ u . . . . . . . . . . . . . . . . . . . . . 107 4.11 Simulaˇcn´ı sch´emata generov´ an´ı polohy s.s. F2 vzhledem k s.s. F1 (parametr (0) oznaˇcuje poˇc´ ateˇcn´ı podm´ınky integr´ator˚ u) . . . . . . . . . . . . 115 4.12 D-H u ´mluva . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116 4.13 Zaveden´ı souˇradn´ ych syst´em˚ u pro AM+SZ dle D-H u ´mluvy . . . . . . . 119 4.14 D-H u ´mluva vedouc´ı na nejednoznaˇcnost v definici souˇradn´ ych syst´em˚ u pˇri pouˇzit´ı na rozvˇetven´e kinematick´e ˇretˇezce . . . . . . . . . . . . . . . 121 4.15 K-K u ´mluva . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122 4.16 Khalil-Kleinfingerova u ´mluva pro rozvˇetven´e kinematick´e ˇretˇezce . . . . 125 4.17 Zaveden´ı souˇradn´ ych syst´em˚ u pro AM+SZ dle K-K u ´mluvy . . . . . . . 127 4.18 Princip kompenzac´ı manipul´ atoru . . . . . . . . . . . . . . . . . . . . . . 129 4.19 Pˇr´ım´ a kinematick´ au ´loha pro SM+SZ (model v SimMechanicsu) . . . . 130 4.20 Translaˇcn´ı ˇc´ ast AM+SZ (zaveden´ı s.s. dle D-H u ´mluvy, viz Pˇr´ıklad 4.1) 132 ´ pro plan´ 4.21 IKU arn´ı s´eriov´ y manipul´ator typu RR . . . . . . . . . . . . . . 133 ´ pro translaˇcn´ı ˇc´ast AM+SZ . . . . . . . . . . . . . . 135 4.22 Moˇzn´ a ˇreˇsen´ı IKU ´ sf´erick´e z´ 4.23 IKU apˇest´ı . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136 ´ pro kinematick´ 4.24 Dvˇe moˇzn´ a ˇreˇsen´ı IKU y ˇretˇezec PSZ . . . . . . . . . . . 137 4.25 Sf´erick´e z´ apˇest´ı (obecn´ y pˇr´ıpad) . . . . . . . . . . . . . . . . . . . . . . . 141
xii
´ ˚ SEZNAM OBRAZK U
4.26 Paraleln´ı manipul´ atory a jejich reprezentace pomoc´ı plan´arn´ıho grafu s urˇcen´ım poˇctu nez´ avisl´ ych uzavˇren´ ych kinematick´ ych ˇretˇezc˚ u (podtrˇzen´ı oznaˇcuje aktivn´ı klouby manipul´atoru) . . . . . . . . . . . . . . . . 148 4.27 Princip rozpojen´ı uzavˇren´eho kinematick´eho ˇretˇezce . . . . . . . . . . . 150 4.28 Paraleln´ı manipul´ atory a jejich reprezentace pomoc´ı plan´arn´ıho grafu (staticky neurˇcen´e uzavˇren´e kinematick´e ˇretˇezce) . . . . . . . . . . . . . 154 ˇ sen´ı IKU ´ obecn´eho paraleln´ıho manipul´atoru . . . . . . . . . . . . . . 155 4.29 Reˇ 4.30 Coupler curve Σ (ˇcernˇe) a kˇrivka Γ odpojen´eho kinematick´eho ˇretˇezce PPM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162 4.31 3RRR plan´ arn´ı paraleln´ı manipul´ator . . . . . . . . . . . . . . . . . . . 163 4.32 Plan´ arn´ı 4-ramenn´ y mechanismus (coupler manipul´atoru) . . . . . . . . 164 4.33 Coupler curve - kˇrivka Γ (ˇcervenˇe) a kˇrivka Σ (ˇcernˇe) s vyznaˇcenou polohou koncov´eho efektoru 3RRR plan´arn´ıho paraleln´ıho manipul´aˇ arkovanˇe jsou vyznaˇceny trajektorie pohybu ramen plan´arn´ıho toru. C´ ´ 168 4-ramenn´eho mechanismu. Obr´azek demonstruje 6 r˚ uzn´ ych ˇreˇsen´ı PKU. 4.34 Moˇzn´e kombinace kinematick´ ych ˇretˇezc˚ uu ´pln´eho neredundantn´ıho plan´ arn´ıho paraleln´ıho manipul´atoru . . . . . . . . . . . . . . . . . . . . . . 171 4.35 Delta robot, CAD model (vlevo) a skuteˇcn´ y v´ yrobek firmy Abb. . . . . 172 4.36 Nejzn´ amˇejˇs´ı zjednoduˇsen´e architektury (mechanismy) prostorov´ ych paraleln´ıch manipul´ ator˚ u SSM, TSSM a jeho speci´aln´ı pˇr´ıpad MSSM. . . . 173 4.37 Dekompozice TSSM mechanismu . . . . . . . . . . . . . . . . . . . . . . 174 4.38 Ekvivalence mezi PSZ a TSSM . . . . . . . . . . . . . . . . . . . . . . . 175 √ p 4.39 Odvozen´ı matice rotace Rbi , ρ1 = 33 a1 , kC i O e k = ρ21 + (v − di )2 . . . 176 √
4.40 Pr˚ useˇc´ık koul´ı Ki a Ke definuj´ıc´ı kruˇznice ki , ρ2 = 33 a2 . . . . . . . . . 176 ˇ sen´ı PKU ´ pro PSZ, ˇcervenˇe jsou vyznaˇceny moˇzn´e trajektorie pˇr´ıpoj4.41 Reˇ n´ ych bod˚ u D i . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183 4.42 Speci´ aln´ı pˇr´ıpady SSM . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184 4.43 Odvozen´ı rychlosti z˙ i . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 195 4.44 Projekce rychlost´ı v kinematick´em ˇretˇezci PSZ (u je jednotkov´ y smˇerov´ y vektor aktu´ ator˚ u) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201 4.45 S´eriov´e singul´ arn´ı polohy AM+SZ . . . . . . . . . . . . . . . . . . . . . 204 4.46 Paraleln´ı singul´ arn´ı polohy PSZ, vektory O e D i b × C i D i b jsou line´arnˇe z´ avisl´e (leˇz´ı v jedn´e rovinˇe se vz´ajemn´ ym pootoˇcen´ım o 23 π) . . . . . . . 206
xiii
´ ˚ SEZNAM OBRAZK U
4.47 PSZ souˇcasnˇe v s´eriov´e i paraleln´ı singul´arn´ı poloze, α = β = γ = 0, n´ avrhov´e parametry ξ jsou odliˇsn´e od pˇredchoz´ıho pˇr´ıpadu, vektory (O e D i b × C i D i b ) jsou line´ arnˇe z´avisl´e, vektory C i D i a u jsou kolm´e . . 206 4.48 Prostory generovan´e vlastn´ımi vektory jakobi´anu J . . . . . . . . . . . . 209
xiv
1
Mechatronika - oblast aplikac´ı vloˇ zen´ eho ˇ r´ızen´ı N´azev
mechatronika“ byl poprv´e pouˇzit v roce 1969 japonsk´ ym inˇzen´ yrem firmy ” Yaskawa, kter´ a vyr´ ab´ı elektrick´e servopohony a dalˇs´ı automatizaˇcn´ı techniku. Mechatronika sjednocuje principy mechaniky, elektroniky, optiky, informatiky a automatick´eho ˇr´ızen´ı za u ´ˇcelem konstruov´ an´ı jednoduˇsˇs´ıch, u ´ˇcinnˇejˇs´ıch a spolehlivˇejˇs´ıch syst´em˚ u. (viz [1, 2, 3, 4]) Integrovan´ y elektrick´ y pohon, pr˚ umyslov´ y robot, automobilov´ y ABS syst´em, pevn´ y disk, digit´ aln´ı fotoapar´ at jsou vhodn´e pˇr´ıklady takov´ ych syst´em˚ u, nebot’ obsahuj´ı mechanick´e, elektronick´e, v´ ypoˇcetn´ı a ˇr´ıdic´ı komponenty a dosahuj´ı dˇr´ıve nev´ıdan´ ych moˇznost´ı. Mezi vˇsemi discipl´ınami mechatroniky (viz obr.1) m´a v´ yjimeˇcn´e postaven´ı technick´ a kybernetika a speci´ alnˇe automatick´e ˇr´ızen´ı (Control Systems), kter´e dod´av´ a mechatronick´ ym syst´em˚ um skryt´ y d˚ umysl. To se projevuje postupn´ ym pˇrechodem od mechanick´ ych funkc´ı k funkc´ım realizovan´ ym elektronick´ ymi programovateln´ ymi prostˇredky, kter´e je nahrazuj´ı v dokonalejˇs´ı podobˇe a nebo jsou dokonce zcela nov´e, mechanicky nerealizovateln´e. Zaˇrazen´ım senzor˚ u, aktu´ator˚ u a pokroˇcil´ ych algoritm˚ u zpˇetnovazebn´ıho ˇr´ızen´ı do mechanick´ ych soustav lze totiˇz ”mˇenit”z´akony mechaniky a t´ım docilovat dramatick´eho zv´ yˇsen´ı kvality mechatronick´eho produktu. Napˇr´ıklad zaˇrazen´ım piezo-senzor˚ u a aktu´ ator˚ u a zaveden´ım pˇr´ısluˇsn´e zpˇetn´e vazby lze zv´ yˇsit tuhost pruˇzn´ ych mechanick´ ych soustav nebo ekvivalentnˇe zmˇenit jejich vlastn´ı frekvence kmit´an´ı. Takov´ a (skryt´ a) modifikace dynamiky m˚ uˇze b´ yt vnˇejˇs´ım pozorovatelem interpretov´ana pouze jako podivuhodn´ a zmˇena fyzik´aln´ıch z´akon˚ u. Takov´eto syst´emy jsou ˇcasto naz´ yv´any aktivn´ımi mechanick´ymi soustavami [3], protol’e zmˇeny mechanick´ ych vlastnost´ı
1
ˇ ´ ˇ´ 1. MECHATRONIKA - OBLAST APLIKAC´ I VLOZEN EHO R IZEN´ I
p˚ uvodnˇe pasivn´ıho syst´emu je zde dosal’eno aktivn´ım p˚ usoben´ım nˇejak´eho pomocn´eho zdroje energie.Tˇesn´ a vazba mezi komunitou odborn´ık˚ u pracuj´ıc´ıch v oboru automatick´eho ˇr´ızen´ı a mechatronikou je vyj´ adˇrena tak´e zaloˇzen´ım nov´eho ˇcasopisu Mechatronics (Elsevier Ltd.) v roce 1991 ˇcleny spoleˇcnosti IFAC (International Federation of Autoˇ matic Control). Casopis publikuje kvalitn´ı pˇr´ıspˇevky s orientac´ı na praktick´e aplikace. Jeho souˇcasn´ y impaktn´ı faktor je 1.434. Pˇredpokl´ad´a se, ˇze autory pˇr´ıspˇevk˚ u budou jak pˇr´ısluˇsn´ıci akademick´e obce tak odborn´ıci z praxe. Dalˇs´ım ˇspiˇckov´ ym ˇcasopisem v t´eto oblasti je Transaction on Mechatronics vyd´avan´ y n´asleduj´ıc´ımi spoleˇcnostmi IEEE Industrial Electronic Society, IEEE Robot´ıce and Automation Society a ASME Dynamic and Control Division.
Obr´ azek 1.1: Mechatronika - jednotliv´e obory a discipl´ıny
Kl´ıˇcov´e probl´emy souˇcasn´e mechatroniky jsou vyvol´any snahou zv´ yˇsit kvalitu uˇzitn´ ych vlastnost´ı mechatronick´ ych produkt˚ u, kvalitu jejich implementace a jednoduchost provozov´an´ı a u ´drˇzby. K zlepˇsen´ı uˇzitn´ ych vlastnost´ı je obvykle tˇreba rozˇs´ıˇrit a zkvalitnit soubor senzor˚ u (virtu´ aln´ıch senzor˚ u) a akˇcn´ıch ˇclen˚ u ˇr´ıdic´ıho syst´emu, vyvinout a implementovat nov´e pokroˇcil´e algoritmy ˇr´ızen´ı s vestavˇen´ ymi funkcemi automatick´eho naladˇen´ı parametr˚ u a autokalibrac´ı, automatick´ ym uˇcen´ım, diagnostikou a komunikac´ı. Pro rychlou a kvalitn´ı implementaci potˇrebujeme v´ yvojov´e n´astroje pro n´avrh, simulaci a implementaci mechanick´e i ˇr´ıdic´ı ˇc´ asti syst´emu a v´ ykonn´ y hardware (DSP, FPGA, PLC, PAC). Koneˇcnˇe pro snazˇs´ı provozov´an´ı a u ´drˇzbu potˇrebujeme kvalitn´ı interface ˇclovˇek-stroj a kompatibiln´ı komunikaci. D˚ uleˇzit´e je aby jednotliv´e ˇc´asti syst´emu vytv´aˇrely synergetick´ y efekt.
2
Obr´ azek 1.2: Aplikace mechatronick´ ych syst´ em˚ u - pˇrehled podle R. Isermanna [3]
Oblast mechatroniky pokr´ yv´a ˇsirok´e spektrum aplikac´ı od spotˇrebn´ı elektroniky pˇres stroj´ırenstv´ı, energetiku, automobilov´ y pr˚ umysl aˇz po dopravu a letectv´ı (viz Obr. 1.2). V dalˇs´ı ˇc´ asti u ´vodu se pokus´ıme struˇcnˇe charakterizovat nˇekter´e v´ yznamnˇejˇs´ı oblasti mechatroniky, kter´ ym je v posledn´ı dobˇe vˇenov´ana pozornost. Poznamenejme vˇsak, ˇze tento v´ ybˇer je velmi omezen´ y a subjektivn´ı. Detailn´ı pˇrehled mechatronick´ ych syst´em˚ u jako inovativn´ıch produkt˚ u s vestavˇen´ ym ˇr´ıdic´ım syst´emem lze nal´ezt napˇr. v publikac´ıch [1, 3, 4]. D´ ale uved’me nˇekolik typick´ ych aplikac´ı mechatroniky demonstruj´ıc´ı koncepty vloˇzen´eho ˇr´ızen´ı a simult´ann´ıho inˇzen´ yrstv´ı.
Mikro a nano elektromechanick´ e syst´ emy (MEMS, NEMS) V souˇcasn´e dobˇe doch´ az´ı t´eˇz k dramatick´emu pokroku v oblasti mikrosenzor˚ u a mikroaktu´ ator˚ u zaloˇzen´ ych na technologi´ıch MEMS/NEMS (mikro/nano electro mechanical systems). V poˇc´ ateˇcn´ı f´ azi kolem roku 1990 existovaly individu´aln´ı sn´ımaˇce v jednom ˇcipu, napˇr´ıklad automobilov´ y havarijn´ı detektor pro airbag nebo tlakov´ y senzor pro spalovac´ı motory. Na poˇc´ atku nov´eho tis´ıcilet´ı jiˇz existovaly integrovan´e v´ıcen´asobn´e senzory s CAN nebo dalˇs´ımi typy komunikac´ı na kartˇe s tiˇstˇen´ ymi spoji. Pozdˇeji v t´eto podobˇe byly k dispozici jednotky pro dynamick´e ˇr´ızen´ı vozidla (VDC-vehicle dynamic
3
ˇ ´ ˇ´ 1. MECHATRONIKA - OBLAST APLIKAC´ I VLOZEN EHO R IZEN´ I
control), stabilizaci vozidla (ESP-electronic stability program) a monitorov´an´ı tlaku v pneumatik´ ach (TPMS- tire pressure monitoring system). Aˇckoliv velikost takov´ ych jednotek nen´ı obvykle vˇetˇs´ı neˇz krabiˇcka cigaret v posledn´ı dobˇe pokraˇcuje dalˇs´ı proces miniaturizace. Snaha je cel´e tyto obvody integrovat do jednoho ˇcipu. V t´eto podobˇe jiˇz existuj´ı MEMS akcelerometry, gyroskopy a t´eˇz cel´e 3-osov´e inerci´aln´ı mˇeˇr´ıc´ı jednotky (IMU- 3-axis inertial measurement unit).
Pr˚ umysl motorov´ ych vozidel (Automotive industry) Jedno z kl´ıˇcov´ ych odvˇetv´ı pr˚ umyslu, kde mechatronick´e syst´emy nalezly ˇsirok´e uplatnˇen´ı. Ne´ upln´ y seznam aplikac´ı zde obsahuje: elektronick´e ˇr´ızen´ı v´ ykonu, aktivn´ı ˇr´ızen´ı automobilu, automatick´e pˇrevodovky, mechatronick´e brzdov´e syst´emy (ABS, ESP, EHB, EMB), ”drive-by-wire”syst´emy, regulace rychlosti j´ızdy a odstupu, aktivn´ı odpruˇzen´ı, trakˇcn´ı ˇr´ızen´ı vlak˚ u, elektro-hydraulick´e brzdˇen´ı, vstˇrikovac´ı syst´emy pro dieselov´e a plynov´e motory.
Aktivn´ı tlumen´ı vibrac´ı mechanick´ ych soustav Vlivem r˚ uzn´ ych pˇr´ıˇcin doch´ az´ı k neˇz´ adouc´ımu vybuzen´ı vlastn´ıch frekvenc´ı mechanick´ ych soustav. C´ılem aktivn´ıho tlumen´ı vibrac´ı je bud’ modifikace bud´ıc´ı s´ıly tak, aby k vybuzen´ı vlastn´ıch m´ od˚ u v˚ ubec nedoˇslo anebo potlaˇcen´ı vibrac´ı zpˇetnou vazbou. V prv´em pˇr´ıpadˇe je bud´ıc´ı sign´ al nejprve tvarov´an takzvan´ ym vstupn´ım tvarovac´ım filtrem, kter´ y potlaˇc´ı vˇsechny frekvence odpov´ıdaj´ıc´ı vlastn´ım frekvenc´ım buzen´e soustavy. Takov´ ym zp˚ usobem m˚ uˇzeme napˇr´ıklad zabr´anit neˇz´adouc´ım kmit˚ um bˇremene zavˇeˇsen´eho na lanˇe port´ alov´eho jeˇr´ abu. Poˇzadovan´a rychlost v ose x a y je nejprve zpracov´ana vhodn´ ym vstupn´ım tvarovac´ım filtrem a teprve pot´e posl´ana na vstup regul´atoru rychlosti pˇr´ısluˇsn´e osy. V´ ysledek je pˇrekvapuj´ıc´ı, po ukonˇcen´ı pohybu je bˇremeno v klidu bez neˇz´adouc´ıch rezidu´ aln´ıch vibrac´ı. V druh´em pˇr´ıpadˇe je m´ısto kompenzaˇcn´ıho ˇr´ızen´ı pouˇzito zpˇetnovazebn´ı ˇr´ızen´ı. Pruˇzn´ a mechanick´a konstrukce mus´ı nyn´ı b´ yt dovybavena souborem ˇcidel (akcelerometr˚ u), akˇcn´ıch ˇclen˚ u (napˇr. piezoaktu´ator˚ u) a ˇr´ıdic´ı jednotkou s vhodn´ ymi algoritmy ˇr´ızen´ı. Jejich n´avrh je netrivi´aln´ı z´aleˇzitost a je zaloˇzen na metod´ach pˇriˇrazen´ı p´ ol˚ u, line´ arn´ım optim´aln´ım regul´atoru (LQR), metodˇe H∞ , metodˇe ˇr´ızen´ı v klouzav´em reˇzimu, rekonstrukci stavu line´arn´ıho syst´emu (Kalman˚ uv filtr) atd. Jako pˇr´ıklad uved’me potlaˇcen´ı torzn´ıch vibrac´ı hˇr´ıdele obr´abˇec´ıho stroje stavovou zpˇetnou vazbou (obr.4). Poznamenejme, ˇze vyznaˇcen´e u ´hly natoˇcen´ı stejnˇe jako pˇr´ısluˇsn´e
4
u ´hlov´e rychlosti nemus´ı b´ yt nutnˇe pˇr´ımo mˇeˇriteln´e, ale je moˇzn´e m´ısto nich pouˇz´ıt jejich odhad pˇr´ısluˇsn´ ym rekonstruktorem stavu.
Obr´ azek 1.3: Aktivn´ı tlumen´ı vibrac´ı - ˇr´ızen´ı pohybu pruˇzn´e hˇr´ıdele
Metoda ”Hardware in the loop”(HWIL) Bˇehem posledn´ıch desetilet´ı doˇslo k obrovsk´emu zv´ yˇsen´ı v´ ypoˇcetn´ıho v´ ykonu procesor˚ u a v d˚ usledku toho k masivn´ımu vyuˇz´ıv´an´ı poˇc´ıtaˇcov´e simulace bˇehem vˇsech f´az´ı v´ yvoje nov´ ych stroj˚ u a zaˇr´ızen´ı. Souˇcasnˇe bylo zavedeno hojn´e vyuˇz´ıv´an´ı pˇr´ıstupu hardware-inthe-loop jako levnˇejˇs´ı varianty v´ yvoje sloˇzit´ ych inˇzen´ yrsk´ ych syst´em˚ u. HWIL simulace je simulace v re´ aln´em ˇcase, kde jedna ˇc´ast syst´emu je simulov´ana numericky a druh´a ˇc´ast (obvykle sloˇzitˇejˇs´ı a obt´ıˇznˇejˇs´ı pro numerickou simulaci) je vloˇzena v re´aln´e hardwarov´e podobˇe. Tato efektivn´ı technika naˇsla svoje uplatnˇen´ı pˇredevˇs´ım v rychl´em n´avrhu prototyp˚ u (rapid prototyping) a testov´an´ı nov´ ych komponent. Jako ilustrace tohoto pˇr´ıstupu a softwarov´ ych n´ astroj˚ u vhodn´ ych pro tento u ´ˇcel je uveden propagaˇcn´ı obr´azek viz obr. materi´ al˚ u firmy Humusoft. O dvou podle naˇseho soudu d˚ uleˇzit´ ych oblastech mechatroniky, kter´ ymi se t´eˇz inˇ v Plzni, FAV, KKY), tj. o ˇr´ızen´ı pohybu a tenzivnˇe zab´ yv´ a pracoviˇstˇe autor˚ u (ZCU robotice bude podrobnˇeji pojedn´ano v n´asleduj´ıc´ıch kapitol´ach tohoto textu.
5
ˇ ´ ˇ´ 1. MECHATRONIKA - OBLAST APLIKAC´ I VLOZEN EHO R IZEN´ I
Obr´ azek 1.4: Hardware in the loop - princip simulace s vyuˇzit´ım softwarov´ ych n´astroj˚ u Matlab a hardwaru dSpace
6
2
ˇ ızen´ı pohybu R´ v mechatronick´ ych syst´ emech 2.1
´ Uvod
Problematika ˇr´ızen´ı pohybu je jedn´ım z kl´ıˇcov´ ych odvˇetv´ı v oblasti mechatronick´ ych syst´em˚ u a objevuje se v cel´e ˇradˇe aplikac´ı, od robotiky, pr˚ umyslov´ ych v´ yrobn´ıch linek, pˇres dopravu, letectv´ı aˇz po bio-medic´ınsk´e aplikace a kosmick´ y v´ yzkum. Z´akladn´ım principem je propojen´ı mechanick´ ych komponent s elektronick´ ymi prvky a pˇrid´an´ı inteligence ve formˇe ˇr´ıdic´ıho syst´emu. Toto spojen´ı pˇrin´aˇs´ı zv´ yˇsen´ı spolehlivosti a v´ ykonu cel´eho zaˇr´ızen´ı, redukci n´ aklad˚ u na v´ yrobu a u ´drˇzbu a moˇznosti implementace zcela nov´ ych pokroˇcil´ ych funkc´ı a vlastnost´ı. Tak jako v ostatn´ıch oblastech mechatroniky doch´ az´ı v syst´emech ˇr´ızen´ı pohybu k propojen´ı nˇekolika r˚ uzn´ ych vˇedn´ıch obor˚ u a technick´ ych discipl´ın (pohony a v´ ykonov´a elektronika, senzorika, teorie ˇr´ızen´ı, programov´an´ı a informatika). Z´ akladn´ı struktura je zobrazena na sch´ematu (2.1). Syst´em vyuˇz´ıv´a nejr˚ uznˇejˇs´ı senzory k z´ısk´ an´ı informac´ı o aktu´aln´ım stavu urˇcit´ ych fyzik´aln´ıch veliˇcin (poloha, rychlost, zrychlen´ı, s´ıla), kter´e jsou nezbytn´e k jeho ˇr´ızen´ı. Takov´ yto senzor m˚ uˇze b´ yt bud’ fyzick´e mˇeˇr´ıc´ı zaˇr´ızen´ı, ve formˇe nˇejak´eho pˇrevodn´ıku nebo senzor virtu´aln´ı - rekonstruktor stavu, kter´ y odhaduje hodnoty nezn´am´ ych veliˇcin, jejichˇz mˇeˇren´ı nen´ı pˇr´ımo dostupn´e. Na z´ akladˇe u ´daj˚ u ze senzor˚ u jsou vypoˇcteny potˇrebn´e akˇcn´ı z´asahy, kter´e jsou realizov´ any pomoc´ı akˇcn´ıch ˇclen˚ u - aktu´ator˚ u. Funkci aktu´atoru vykon´av´ a nejˇcastˇeji line´ arn´ı nebo rotaˇcn´ı pohon napojen´ y na mechanismus stroje a v´ ysledkem je pohyb mechanick´ ych ˇc´ ast´ı syst´emu. Kl´ıˇcovou roli zde hraje ˇr´ıdic´ı syst´em. Kvalita
7
ˇ´ ´ ´ 2. R IZEN´ I POHYBU V MECHATRONICKYCH SYSTEMECH
ROZHRANÍ OBSLUHY Parametrizace, ovládání, vizualizace
ŘÍDICÍ SYSTÉM
SENZORY
AKTUÁTORY Pohyb aktuátoru
Poloha, rychlost, síla
PRACOVNÍ MECHANISMUS STROJE
Obr´ azek 2.1: Struktura syst´ emu ˇ r´ızen´ı pohybu
pouˇzit´ ych algoritm˚ u z´ asadn´ım zp˚ usobem ovlivˇ nuje funkci a v´ ykon cel´eho zaˇr´ızen´ı. Jiˇz pˇri n´avrhu konstrukce je tˇreba br´ at zˇretel na zp˚ usob propojen´ı mechanick´ ych prvk˚ u s ostatn´ımi komponentami syst´emu, na zam´ yˇslenou funkci cel´eho zaˇr´ızen´ı a pˇredevˇs´ım na moˇznosti z hlediska jeho ˇr´ızen´ı. Jako pˇr´ıklad lze uv´est problematiku n´avrhu robotick´ ych manipul´ ator˚ u, kde kinematika robotu rozhoduje o jeho dynamick´ ych vlastnostech, velikosti a tvaru pracovn´ıho prostoru, poˇctu a typu pouˇzit´ ych akˇcn´ıch ˇclen˚ u, moˇznostech v prov´adˇen´ı pohyb˚ u a dalˇs´ıch u ´kon˚ u atd. Nelze tedy od sebe oddˇelit proceduru n´avrhu mechanick´e ˇc´ asti od v´ ybˇeru elektrick´ ych a elektronick´ ych komponent a vytv´aˇren´ı algoritm˚ u v ˇr´ıdic´ım syst´emu, ale vˇsechny tyto oblasti je tˇreba zohlednit jiˇz v poˇc´ateˇcn´ı f´azi vzniku nov´eho zaˇr´ızen´ı. Teprve d˚ usledn´e uplatˇ nov´an´ı princip˚ u tzv. simult´ ann´ıho inˇzen´yrstv´ı, kdy n´ avrh jednotliv´ ych subsyst´em˚ u prob´ıh´a souˇcasnˇe s respektov´an´ım vˇsech vz´ajemn´ ych vztah˚ u a vazeb, lze dos´ ahnout synergick´eho efektu, kdy v´ ysledn´ y produkt m´a vyˇsˇs´ı hodnotu nebo funkˇcnost neˇz pouh´a suma jeho ˇc´ast´ı.
2.2
Senzory
Senzory zajiˇst’uj´ı mˇeˇren´ı aktu´ aln´ıch u ´rovn´ı veliˇcin, potˇrebn´ ych pro ˇr´ızen´ı syst´emu. Podle vykon´avan´e funkce je lze rozdˇelit do n´ asleduj´ıc´ıch kategori´ı [1, 2, 3, 5, 6, 7]:
8
positional value Electronic type label
General tolerances to DIN ISO 2768-m Dimensional drawing SRM50, spring mounting plate Ø 66 B
46 ±0.5
36.6 ±0.5
10
34 23 ±0.3
Max. fitting space
3.2 +0.1
51.8 ±0.4
60 -0.3
2.2 Senzory
65.5 -0.2
0.5
7.4 7.6
max. Ø 47
Ø8 f7 Ø5.5 h7 9.46 2° ±3 l M4
9.5
In case of stranded exit Stranded cab length approx 200 mm with earthing
55.8
A
General tolerances to DIN ISO 2768-m PIN and wire allocation PIN
Signal
Colour of Wires
Explanation
1
Us
red
Supply voltage 7 … 12 V
2
GND
blue
Ground connection
3
REFSIN
brown
Process data channel
4
REFCOS
black
Process data channel
5
Data +
grey or yellow
RS-485-parameter channel
8
+ COS
pink
Process data channel
Obr´ azek 2.2: Inteligentn´ı senzory se s´ eriovou6 komunikac´ ı –- Sin/cosgreen enkod´ er StegData or purple RS-485-parameter channel 7 + SIN white Process data channel mann, 3-os´ y akcelerometr Bosch Caution: To ensure proper function, the screen connection strand (200 mm) MUST be connected. It is included in the supply.
Senzory polohy
Us and GND are internally connected to the screen by capacitors of 2.2 nF.
Accessories Pouˇz´ıvaj´ı se v aplikac´ ıch, kdy je vyˇzadov´ano pˇresn´e polohov´an´ı pracovn´ıho mechanismu. Screening: Connection technology The encoder housing for the integrated encoder is connecFixing technology ˇ Cidlo je ˇcasto integrov´ ano pˇr´ımo do tˇela vlastn´ıho pohonu kde napˇr´ıkladteduto the elektronicky motor, via the torque support. The connection Programming tool
space is thus screened via the motor housing such that, wi
hin the connection space, unscreened connection strands komutovan´ ych elektrick´ ych motor˚ u zajiˇst’uje spr´avnou funkci proudov´e/momentov´ e recan be used.
gulaˇcn´ı smyˇcky. Nejˇcastˇeji je pouˇz´ıv´an resolver nebo optick´ y sn´ımaˇc polohy. Resolver of view "B" funguje jako polohov´ y transform´ator s prim´arn´ım Direction a sekund´ arn´ım vinut´ım. Prim´arn´ı
ˇc´ast je buzena harmonick´ ym napˇet´ım o vysok´e frekvenci. Vlivem zmˇeny vz´ajemn´e in6 SICK-STEGMANN dukˇcnosti mezi obˇ ema vinut´ımi pˇri ot´aˇcen´ı rotoru je pak vyhodnocov´an u ´hel natoˇcen´ı
z pr˚ ubˇehu napˇet´ı na sekund´ arn´ı ˇc´asti. Resolvery b´ yvaj´ı doplnˇeny integrovan´ ym obvodem, tzv. R/D pˇrevodn´ıkem (resolver-to-digital), kter´ y zprostˇredkov´av´a informaci o absolutn´ı poloze v digit´ aln´ım form´atu s pˇresnost´ı 10-12 bit˚ u. Optick´e sn´ımaˇce pracuj´ı na principu vys´ılaˇce a pˇrij´ımaˇce svˇeteln´eho sign´alu, kter´ y proch´az´ı optickou mˇr´ıˇzkou. Stˇr´ıd´ an´ı matn´ ych a propustn´ ych oblast´ı pˇri ot´aˇcen´ı mˇr´ıˇzky zp˚ usobuje zmˇeny napˇet´ı v d˚ usledku zmˇeny mnoˇzstv´ı detekovan´eho svˇetla. Vyhodnocen´ım dvou navz´ajem f´ aˇ zovˇe posunut´ ych sign´ al˚ u ve tvaru obd´eln´ıkov´ ych pulz˚ u lze urˇcit smˇer ot´aˇcen´ı. Cidla se vyr´ ab´ı jako absolutn´ı (jedno i v´ıceot´aˇckov´a) nebo inkrement´aln´ı. Rozliˇsen´ı dneˇsn´ıch optick´ ych sn´ımaˇc˚ u je ˇr´ adovˇe 10-13 bit˚ u na ot´aˇcku. Jeˇstˇe vyˇsˇs´ı pˇresnosti dosahuj´ı tzv. sin/cos enkod´ery, kde sign´ al sn´ımaˇce m´a tvar dvou navz´ajem posunut´ ych harmonick´ ych funkc´ı. Vhodn´ ym interpolaˇcn´ım algoritmem lze pak odhadovat aktu´aln´ı polohu i rychlost s vˇetˇs´ım rozliˇsen´ım. V aplikac´ıch s menˇs´ımi n´aroky na pˇresnost lze vyuˇz´ıt jednoduch´e magnetick´e sn´ımaˇce s Hallovo sondami, kter´e detekuj´ı pˇribl´ıˇzen´ı magnetu ˇ upevnˇen´eho na rotor. Rozliˇsen´ı je v´ yraznˇe menˇs´ı neˇz u optick´ ych sn´ımaˇc˚ u. Cidla um´ıstˇen´ a v jednotliv´ ych pohonech stroje lze kromˇe vlastn´ıho ˇr´ızen´ı motor˚ u vyuˇz´ıt tak´e
9
12-20
ˇ´ ´ ´ 2. R IZEN´ I POHYBU V MECHATRONICKYCH SYSTEMECH
k odmˇeˇrov´an´ı polohy cel´eho stroje. U v´ıceos´ ych syst´em˚ u (roboty, manipul´atory, obr´abˇec´ı stroje) je moˇzn´e polohu jednotliv´ ych os pˇrev´est kinematickou transformac´ı na polohu koncov´e efektoru, kter´ a m´ a b´ yt ˇr´ızena. Nev´ yhodou uveden´eho zp˚ usobu mˇeˇren´ı je kumulace vˇsech nepˇresnost´ı v mechanick´e konstrukci stroje, coˇz m˚ uˇze m´ıt za n´asledek odchylku koncov´e polohy od skuteˇcn´e hodnoty. Pro zv´ yˇsen´ı pˇresnosti se tedy v nˇekter´ ych aplikac´ıch pˇrid´ avaj´ı dodateˇcn´a ˇcidla pro pˇr´ım´e odmˇeˇrov´an´ı polohy efektoru. V tomto pˇr´ıpadˇe se uˇz´ıvaj´ı napˇr. line´arn´ı inkrement´aln´ı sn´ımaˇce, laserov´e nebo magnetostrikˇcn´ı senzory. Senzory rychlosti Informace o aktu´ aln´ı rychlosti posuvu nebo ot´aˇcen´ı je pouˇz´ıv´ana na nejniˇzˇs´ı u ´rovni ˇr´ızen´ı pohybu jednotliv´ ych aktu´ ator˚ u. V dneˇsn´ı dobˇe se vˇetˇsinou nepouˇz´ıv´a speci´aln´ı sn´ımaˇc (dˇr´ıve napˇr. tachodynamo), ale rychlost je vypoˇctena z pr˚ ubˇeˇzn´eho mˇeˇren´ı polohy nˇekter´ ym z v´ yˇse uveden´ ych senzor˚ u. Jednou z moˇznost´ı je prost´e diferencov´an´ı sign´alu polohy, coˇz vˇsak pˇrin´ aˇs´ı vznik chyby v podobˇe kvantizaˇcn´ıho ˇsumu a zesilov´an´ı poruch mˇeˇren´ı vyskytuj´ıc´ıch se v oblasti vysok´ ych frekvenc´ı. Vypoˇcten´a rychlost tedy b´ yv´a dodateˇcnˇe zpracov´ ana, nejˇcastˇeji frekvenˇcn´ım filtrem typu doln´ı propust. Alternativn´ım zp˚ usobem je pouˇzit´ı vhodn´eho interpolaˇcn´ıho algoritmu, kter´ y napˇr´ıklad aproximuje nˇekolik posledn´ıch vzork˚ u polohy vhodnou hladkou funkc´ı nebo pouˇzit´ı rekonstruktoru stavu. Senzory zrychlen´ı Tyto sn´ımaˇce se pouˇz´ıvaj´ı nejˇcastˇeji u mobiln´ıch aplikac´ı (ˇr´ızen´ı pohybu autonomn´ıch robot˚ u, syst´emy j´ızdn´ı stability u automobilu atd.). Pracuj´ı na principu akcelerometr˚ u, kter´e ve spojen´ı s vhodn´ ym vyhodnocovac´ım obvodem ˇr´ızen´ ym mikroprocesorem dok´aˇz´ı vypoˇc´ıtat zrychlen´ı ve tˇrech os´ ach v prostoru. Kromˇe pˇr´ım´eho mˇeˇren´ı zrychlen´ı je lze vyuˇz´ıt tak´e napˇr´ıklad pro detekci specifick´ ych ud´alost´ı jako signalizace otˇres˚ u, voln´eho p´adu apod. Senzory s´ıly a momentu Taktiln´ı ˇcidla pracuj´ıc´ı na principu tenzometru se ˇcasto vyuˇz´ıvaj´ı v aplikac´ıch robotiky, kde stroj pˇrich´ az´ı do pˇr´ım´eho kontaktu s okoln´ım prostˇred´ım (pˇrek´aˇzky, obsluha, kontatkn´ı operace s obr´ abˇen´ ymi souˇc´ astmi...).
10
2.3 Aktu´ atory
Obr´ azek 2.3: Line´ arn´ı piezoaktu´ ator, elektrick´ y synchronn´ı servomotor s frekvenˇ cn´ım mˇ eniˇ cem
Doplˇ nkov´ e senzory Tyto senzory zprostˇredkov´ avaj´ı informace o veliˇcin´ach jako teplota, tlak, pr˚ utok atd., kter´e pˇr´ımo nesouvis´ı s ˇr´ızen´ım pohybu, ale vyuˇz´ıvaj´ı se napˇr´ıklad v nadˇrazen´em bezpeˇcnostn´ım syst´emu pro oˇsetˇren´ı havarijn´ıch stav˚ u stroje. V oblasti v´ yroby senzor˚ u pˇrevaˇzuje v posledn´ı dobˇe snaha integrovat veˇsker´e nutn´e zaˇr´ızen´ı vˇcetnˇe elektroniky do jednoho m´ısta, pokud moˇzno na jeden ˇcip. V´ ysledkem jsou tzv. inteligentn´ı nebo smart senzory. Vˇsechny operace zpracov´an´ı mˇeˇren´eho sign´ alu jako filtrace ˇsumu, linearizace, korekce driftu a hystereze nebo autokalibrace prob´ıhaj´ı uvnitˇr ˇcidla a na v´ ystupu je jiˇz informace v digit´aln´ı formˇe pˇren´aˇsen´a do ˇr´ıdic´ıho syst´emu po nˇekter´e ze standardn´ıch komunikaˇcn´ıch sbˇernic. Fyzick´ y senzor nˇekter´e z mˇeˇren´ ych veliˇcin b´ yv´a ˇcasto nahrazen ˇcidlem virtu´aln´ım v podobˇe rekonstruktoru stavu z d˚ uvod˚ uu ´spory n´aklad˚ u na instrumentaci nebo v pˇr´ıpadech kdy instalace ˇcidla nen´ı prakticky realizovateln´a. Ke slovu pak pˇrich´azej´ı techniky filtrace a odhadu stavu [8, 9, 10, 11, 12].
2.3
Aktu´ atory
Aktu´ atory v mechatronick´ ych syst´emech pln´ı funkci ˇr´ızen´ ych elektromechanick´ ych mˇeniˇc˚ u. Jejich u ´ˇcelem je na z´ akladˇe vstupn´ıch ˇr´ıd´ıc´ıch elektrick´ ych sign´al˚ u pˇrev´adˇet energii z nˇejak´eho zdroje (vˇetˇsinou elektrick´eho, pneumatick´eho nebo hydraulick´eho) na mechanick´ y pohyb ve formˇe s´ıly, momentu, u ´hlu, polohy apod. Podle principu funkce lze aktu´ atory rozdˇelit do tˇr´ı z´ akladn´ıch kategori´ı [3]:
11
ˇ´ ´ ´ 2. R IZEN´ I POHYBU V MECHATRONICKYCH SYSTEMECH
1. Elektromagnetick´ e aktu´ atory - Z´akladn´ım principem jejich funkce je interakce dvou magnetick´ ych pol´ı mezi pevnou a pohyblivou ˇc´ast´ı pohonu. Do t´eto skupiny patˇr´ı elektrick´e motory (stejnosmˇern´e, stˇr´ıdav´e synchronn´ı a asynchronn´ı, krokov´e, sp´ınan´e reluktanˇcn´ı...) nebo pohony pracuj´ıc´ı na principu elektromagnetu. Elektrick´e pohony prodˇelaly v posledn´ıch tˇriceti letech bouˇrliv´ y v´ yvoj pˇredevˇs´ım d´ıky pokroku v oblasti polovodiˇc˚ u, mikroprocesorov´e techniky a automatick´eho ˇr´ızen´ı. Protoˇze maj´ı dominantn´ı postaven´ı v oblasti robotiky a obr´abˇec´ıch stroj˚ u,na kter´e je zamˇeˇrena tato pr´ ace, je jim n´ıˇze vˇenov´ana zvl´aˇstn´ı sekce. 2. Pneumatick´ e a hydraulick´ e aktu´ atory - Pracuj´ı na principu stlaˇcen´eho pracovn´ıho m´edia ve formˇe vzduchu nebo hydraulick´eho oleje, kter´e je pˇriv´adˇeno pˇres regulaˇcn´ı ventily do pracovn´ıho prostoru pohonu, kde vytvoˇren´ y tlak uv´ad´ı do pohybu p´ıst nebo jin´ y podobn´ y mechanismus. Probl´emem pneumatick´ ych aktu´ator˚ u je stlaˇcitelnost vzduchu, kter´ a pˇrin´aˇs´ı obt´ıˇze pˇri pˇresn´e regulaci s´ıly nebo rychlosti/polohy p´ıstu. Proto se ˇcasto vyuˇz´ıvaj´ı pro prov´adˇen´ı pohyb˚ u, u kter´ ych nen´ı d˚ uleˇzit´ y samotn´ y pr˚ ubˇeh trajektorie ale pouze dosaˇzen´ı dvou koncov´ ych pozic (jednoduch´e manipul´ atory, z´ akl´ adac´ı stroje, u ´chopov´e efektory robot˚ u). Doplnˇen´ım aktu´ atoru o odmˇeˇrov´ an´ı tlaku v pracovn´ı komoˇre a pˇr´ısluˇsnou regulaci lze dos´ahnout plynul´eho polohov´ an´ı p´ıstu, pˇresto pneumatick´e servopohony nedosahuj´ı kvalit pohon˚ u elektrick´ ych. Tˇemito neduhy netrp´ı hydraulick´e pohony, kde lze dos´ ahnout pˇresn´eho polohov´ an´ı d´ıky pˇr´ım´e u ´mˇernosti mezi vysunut´ım v´alce a objemem hydraulick´eho m´edia. Uplatnˇen´ı naleznou v aplikac´ıch, kde je potˇreba vyvinout velk´e s´ıly, napˇr´ıklad v hydraulick´ ych lisech, brzd´ach, v´ ykonov´ ych robotech, stavebn´ıch stroj´ıch apod. Nev´ yhodou hydraulick´ ych a pneumatick´ ych aktu´ator˚ u je drah´ y provoz a u ´drˇzba, velk´e v´ ykonov´e ztr´aty a nutnost pouˇzit´ı hydraulick´eho agreg´ atu nebo kompresoru pro dod´avku pracovn´ıho m´edia. 3. Nekonvenˇ cn´ı aktu´ atory - Pracuj´ı na odliˇsn´em fyzik´aln´ım principu neˇz pˇredchoz´ı dvˇe skupiny. Patˇr´ı sem napˇr´ıklad magnetostrikˇcn´ı mikroaktu´atory, kter´e vyuˇz´ıvaj´ı zmˇenu struktury pevn´eho materi´alu pˇri p˚ usoben´ı magnetick´eho pole. Dalˇs´ım pˇr´ıkladem jsou piezoelektrick´e motory, kter´e vyuˇz´ıvaj´ı obr´acen´eho piezoelektrick´eho jevu. Po pˇriloˇzen´ı napˇet´ı na piezokeramick´ y materi´al doch´az´ı k jeho deformaci. Zapojen´ım velk´eho poˇctu tˇechto element˚ u za sebe vznik´a pˇri buzen´ı vhodn´ ym harmonick´ ym sign´ alem postupn´a vlna, kter´a se mechanicky pˇren´aˇs´ı na
12
2.4 Elektrick´ e pohony jako aktu´ atory
pohyblivou ˇc´ ast motoru. V´ ysledkem je rotaˇcn´ı nebo line´arn´ı pohyb. Nˇekdy b´ yvaj´ı oznaˇcov´ any jako ultrazvukov´e motory, protoˇze doch´az´ı k mechanick´ ym vibrac´ım v oblasti ultrasonick´ ych frekvenc´ı (aˇz stovky kHz). Pˇrestoˇze posuny jednotliv´ ych piezoelement˚ u jsou velmi mal´e (ˇr´adovˇe mikrometry), lze d´ıky vysok´e frekvenci dos´ ahnout velk´e rychlosti posuvu. V´ yhodou tˇechto aktu´ator˚ u jsou velmi mal´e rozmˇery, coˇz je pˇredurˇcuje pro pouˇzit´ı v mikroelektromechanick´ ych syst´emech. ˇ Casto jsou vyuˇz´ıv´ any napˇr. v optick´ ych zaˇr´ızen´ıch pro zaostˇrov´an´ı ˇcoˇcky v mikroskopech, fotoapar´ atech, kamer´ach apod. Dalˇs´ı pouˇzit´ı je v mikro-teleoperaˇcn´ıch syst´emech, kde lidsk´ y oper´ator prov´ad´ı pomoc´ı speci´aln´ıho haptick´eho rozhran´ı d´ alkovˇe ˇr´ızen´ y pohyb napˇr´ıklad pˇri medic´ınsk´ ych operac´ıch. Tak jako u senzor˚ u je i v oblasti aktu´ator˚ u obecn´ ym trendem decentralizace a vytv´ aˇren´ı inteligentn´ıch pohon˚ u s vloˇzen´ ym ˇr´ızen´ım, kter´e jsou schopny samostatnˇe pracovat jako pohonn´e jednotky. Veˇsker´e funkce nutn´e pro ˇr´ızen´ı pohybu jsou integrov´any uvnitˇr pohonu, kter´ y komunikuje s nadˇrazen´ ym syst´emem pomoc´ı rychl´e s´eriov´e sbˇernice (dnes nejˇcastˇeji komunikace a protokoly na b´azi pr˚ umyslov´eho ethernetu). Precizn´ı ˇr´ızen´ı pohybu negativnˇe ovlivˇ nuje cel´ a ˇrada faktor˚ u, kter´e se vyskytuj´ı v re´aln´ ych aplikac´ıch: neline´ arn´ı a ˇcasovˇe promˇenn´e poruchy ve formˇe tˇren´ı nebo z´atˇeˇzn´eho momentu, v˚ ule a hystereze v ˇr´ızen´ ych mechanismech, zmˇena dynamiky stroje v z´avislosti na aktu´aln´ım pracovn´ım bodu, koneˇcn´e rozliˇsen´ı senzor˚ u, n´ızk´a mechanick´a tuhost konstrukce atd.
2.4
Elektrick´ e pohony jako aktu´ atory
Elektrick´e pohony nach´ azej´ı v syst´emech ˇr´ızen´ı pohybu ˇsirok´e uplatnˇen´ı. D´ıky sv´e univerz´ alnosti, moˇznostem ˇsk´ alov´an´ı v´ ykonu a dynamick´ ym vlastnostem z hlediska regulace pˇrevaˇzuj´ı v poˇctu aplikac´ı nad ostatn´ımi typy aktu´ator˚ u (viz [13, 14, 15]). V´ yhody elektropohon˚ u lze shrnout v nˇekolika bodech: ˇ • Sirok´ y rozsah v´ ykon˚ u, od nˇekolika mW napˇr. u mikro elektromechanick´ ych syst´em˚ u aˇz po des´ıtky kW ve velk´ ych manipul´atorech • Velk´ y rozsah moment˚ u (ˇr´adovˇe mNm aˇz stovky Nm) a ot´aˇcek (jednotky aˇz statis´ıce ot´ aˇcek za minutu) • Schopnost velmi pˇresn´eho polohov´an´ı s vyuˇzit´ım ˇcidel zpˇetn´e vazby s vysok´ ym rozliˇsen´ım
13
ˇ´ ´ ´ 2. R IZEN´ I POHYBU V MECHATRONICKYCH SYSTEMECH
• N´ızk´a u ´roveˇ n hluku, neprodukuj´ı ˇz´adn´e zplodiny • Vysok´ au ´ˇcinnost, n´ızk´e ztr´ aty napr´azdno, vysok´a kr´atkodob´a pˇret´ıˇzitelnost • Moˇznost ˇctyˇr-kvadrantov´eho provozu, tedy schopnost rekuperovat elektrickou energii v obou smˇerech ot´ aˇcen´ı • Dlouh´ a ˇzivotnost (ˇr´ adovˇe des´ıtky let) - u modern´ıch bez´ udrˇzbov´ ych elektronicky komutovan´ ych pohon˚ u d´ ana v podstatˇe ˇzivotnost´ı loˇzisek • Snadn´ y pˇr´ıvod elektrick´e energie ze s´ıtˇe nebo baterie Pod pojmem elektrick´ y pohon se rozum´ı kromˇe vlastn´ıho motoru cel´ y soubor dalˇs´ıch prvk˚ u, kter´e dohromady tvoˇr´ı ned´ıln´ y celek. Motor mus´ı b´ yt buzen v´ ykonov´ ymi obvody, kter´e v potˇrebn´em mnoˇzstv´ı pˇriv´adˇej´ı elektrickou energii ze s´ıtˇe nebo baterie a pˇr´ıpadnˇe zajiˇst’uj´ı jej´ı rekuperaci. V pˇr´ıpadˇe stejnosmˇern´ ych motor˚ u je to nejˇcastˇeji ˇctyˇrkvadrantov´ y pulsn´ı zdroj, u stˇr´ıdav´ ych motor˚ u trojf´azov´ y napˇet’ov´ y stˇr´ıdaˇc se stejnosmˇern´ ym meziobvodem. Pro speci´aln´ı typy motor˚ u (krokov´e, reluktanˇcn´ı) jsou to zvl´aˇstn´ı obvody urˇcen´e pro dan´ y typ motoru. D´ale je pohon vybaven elektronick´ ym ˇr´ıdic´ım syst´emem ve formˇe jednoˇcipov´eho mikropoˇc´ıtaˇce nebo sign´alov´eho procesoru, kter´ y na z´akladˇe u ´daj˚ u ze zpˇetnovazebn´ıch ˇcidel prov´ad´ı regulaci z´akladn´ıch veliˇcin momentu, ot´ aˇcek a polohy. Poˇzadovan´e hodnoty tˇechto veliˇcin mohou b´ yt zad´av´any z nadˇrazen´eho ˇr´ıd´ıc´ıho syst´emu, kter´ y zajiˇst’uje napˇr´ıklad generov´an´ı trajektorie pohybu v ˇcase. V tomto pˇr´ıpadˇe funguje pohon pouze jako v´ ykonov´ y modul, kter´ y prov´ad´ı povely zad´avan´e nadˇrazen´ ym syst´emem. V posledn´ı dobˇe vˇsak mezi v´ yrobci pˇrevaˇzuje snaha integrovat veˇskerou inteligenci pohonu do jednoho zaˇr´ızen´ı - servomˇeniˇce, jehoˇz funkce lze snadno programovat, parametrizovat a pˇrizp˚ usobit konkr´etn´ı aplikaci pomoc´ı dod´avan´eho v´ yvojov´eho prostˇred´ı. Zvl´ aˇstn´ı problematikou je potom ˇr´ızen´ı pohybu ve v´ıce os´ach, kdy je nutn´e zajistit spr´ avnou synchronizaci vˇsech pohon˚ u pˇri vykon´av´an´ı koordinovan´eho pohybu. Dalˇs´ı souˇc´ ast´ı pohonu jsou potom pomocn´e elektrick´e prvky jako jistiˇce, stykaˇce, rel´e, kter´e zajiˇst’uj´ı bezpeˇcn´e pˇripojen´ı k elektrick´e s´ıti. Motor b´ yv´a ˇcasto s pracovn´ım mechanismem stroje spojen pˇres pˇrevodovku, kter´a umoˇzn ˇuje pˇrizp˚ usoben´ı potˇrebn´ ych ot´ aˇcek a momentu. Stejnosmˇ ern´ e motory jsou historicky nejstarˇs´ı elektrick´e stroje a v dneˇsn´ı dobˇe jsou ve velk´e m´ıˇre nahrazov´ any motory stˇr´ıdav´ ymi. V minulosti byly hojnˇe vyuˇz´ıv´any
14
2.4 Elektrick´ e pohony jako aktu´ atory
v mechatronick´ ych syst´emech s nutnost´ı pˇresn´eho ˇr´ızen´ı pohybu kv˚ uli sv´ ym velmi dobr´ ym regulaˇcn´ım vlastnostem a relativn´ı jednoduchosti ˇr´ıd´ıc´ıch a bud´ıc´ıch obvod˚ u. Z´ akladn´ım konstrukˇcn´ım prvkem tˇechto pohon˚ u je komut´ator - mechanick´e zaˇr´ızen´ı, kter´e zajiˇst’uje udrˇzov´ an´ı kolm´e polohy mezi rotorov´ ym a statorov´ ym magnetick´ ym tokem. V syst´emech ˇr´ızen´ı pohybu je pouˇz´ıv´ana nejˇcastˇeji konstrukce s permanentn´ımi magnety ve statoru a pohyblivou kotvou s vinut´ım, na kter´e se pomoc´ı kart´aˇc˚ u pˇriv´ad´ı proud ze zdroje. Kv˚ uli kluzn´emu kontaktu kart´aˇci s rotorem stroje vˇsak doch´az´ı k jejich postupn´emu opotˇreben´ı, proto je nutn´e obˇcasn´e ˇciˇstˇen´ı pˇr´ıpadnˇe v´ ymˇena. V m´ıstˇe dotyku s vinut´ım kotvy tak´e doch´az´ı pˇri velk´ ych rychlostech a zat´ıˇzen´ı k jiskˇren´ı, kter´e m´a za n´ asledek vznik elektromagnetick´eho ruˇsen´ı, zkracuje ˇzivotnost motoru a m˚ uˇze zp˚ usobit zkrat. Z tˇechto d˚ uvod˚ u se jiˇz stejnosmˇern´e motory prakticky nepouˇz´ıvaj´ı a jsou nahrazov´ any modern´ımi stˇr´ıdav´ ymi pohony. Asynchronn´ı motory vyuˇz´ıvaj´ı principu elektromagnetick´e indukce. Ve statoru je uloˇzeno tˇr´ıf´ azov´e vinut´ı, kter´e je nap´ajen´e harmonick´ ym napˇet´ım ze s´ıtˇe nebo ˇr´ızen´eho frekvenˇcn´ıho mˇeniˇce. Rotor je vyroben ve formˇe v´alcov´e klece, kter´a se skl´ad´ a z nˇekolika kovov´ ych tyˇc´ı, jeˇz jsou na konci vodivˇe spojeny. V d˚ usledku f´azov´eho posunu mezi napˇet´ımi v jednotliv´ ych f´az´ıch vznik´a toˇciv´e magnetick´e pole, kter´e prot´ın´a vodiˇce rotoru. V tˇech se vlivem zmˇeny magnetick´eho pole indukuje proud a vznik´a sekund´arn´ı magnetick´e pole, kter´e reaguje na pole statoru vznikem toˇciv´eho momentu. Toto magnetick´e pole a t´ım i krout´ıc´ı moment vznik´a pouze pokud relativn´ı rychlost ot´aˇcen´ı mezi statorov´ ym polem a pohybuj´ıc´ı se kotvou je nenulov´a. Ot´aˇcky motoru jsou tedy vˇzdy niˇzˇs´ı neˇz frekvence nap´ ajec´ıho napˇet´ı, vznik´a tzv. skluz a rychlost ot´aˇcen´ı je asynchronn´ı v˚ uˇci nap´ ajec´ı frekvenci. Tyto pohony vynikaj´ı pˇredevˇs´ım svoj´ı spolehlivost´ı, kter´ a je d´ ana jejich robustn´ı konstrukc´ı. V dˇr´ıvˇejˇs´ı dobˇe byly vyuˇz´ıv´any hlavnˇe v jednoduch´ ych aplikac´ıch, kde se nevyˇzadovalo pˇresn´e ˇr´ızen´ı momentu, ot´aˇcek nebo polohy. S rozvojem polovodiˇc˚ u, v´ ykonov´e elektroniky, mikroprocesorov´e techniky a s pˇr´ıchodem nov´ ych metod automatick´eho ˇr´ızen´ı (vektorov´e ˇr´ızen´ı, pˇr´ım´e ˇr´ızen´ı momentu, viz napˇr. [15]) doˇslo k velk´emu rozˇs´ıˇren´ı tˇechto pohon˚ u, kter´e nyn´ı sv´ ymi dynamick´ ymi vlastnostmi pˇredˇc´ı dˇr´ıve pouˇz´ıvan´e stejnosmˇern´e motory. Synchronn´ı motory s permanentn´ımi magnety tvoˇr´ı v souˇcasn´e dobˇe technologickou ˇspiˇcku v oblasti syst´em˚ u ˇr´ızen´ı pohybu. Svou dynamikou, vysok´ ym pomˇerem v´ ykonu k hmotnosti, moˇznostmi kr´atkodob´eho pˇret´ıˇzen´ı, rozsahem a pˇresnost´ı regulace nebo velk´ ym rozbˇehov´ ym momentem pˇredˇc´ı vˇsechny ostatn´ı typy pohon˚ u. Speci´aln´ı
15
ˇ´ ´ ´ 2. R IZEN´ I POHYBU V MECHATRONICKYCH SYSTEMECH
proveden´ı skˇr´ınˇe motoru, nejˇcastˇeji s vlastn´ım chlazen´ım, a jeho vnitˇrn´ı uspoˇr´ad´an´ı umoˇzn ˇuj´ı doc´ılit velmi kompaktn´ıho tvaru s mal´ ym momentem setrvaˇcnosti rotoru. V´ yhodou je pouˇzit´ı permanentn´ıch magnet˚ u, um´ıstˇen´ ych na rotoru. Ty zaruˇcuj´ı trval´e vybuzen´ı magnetick´eho pole, kter´e je okamˇzitˇe k dispozici a nemus´ı b´ yt vytv´aˇreno zprostˇredkovanˇe indukc´ı jako v pˇr´ıpadˇe asynchronn´ıho motoru. Rozˇs´ıˇren´ı tˇechto pohon˚ u opˇet umoˇznil rozvoj v oblasti v´ ykonov´e elektrotechniky a modern´ıch metod regulace. Zp˚ usob ˇr´ızen´ı se liˇs´ı od velk´ ych synchronn´ıch stroj˚ u, kter´e jsou vyuˇz´ıv´any jako gener´atory v elektr´ arn´ ach nebo velk´e motory v nˇekter´ ych provozech, kdy se rotor ot´aˇc´ı synchronnˇe s frekvenc´ı s´ıtˇe a mˇen´ı se pouze z´atˇeˇzn´ yu ´hel v z´avislosti na zat´ıˇzen´ı. Regulovan´ y servopohon s vektorov´ ym ˇr´ızen´ım pracuje v tzv. ventilov´em reˇzimu, kdy ˇr´ıdic´ı syst´em neust´ ale vyhodnocuje okamˇzitou polohu rotoru a tedy orientaci jeho magnetick´eho pole a ˇr´ıd´ı nap´ ajen´ı statoru tak, aby v´ ysledn´ y prostorov´ y vektor statorov´eho pole byl trvale kolmo orientovan´ y na pole rotoru. V podstatˇe tedy elektronika nahrazuje mechanick´ y komut´ ator pouˇz´ıvan´ y u stejnosmˇern´ ych motor˚ u. Synchronn´ı servomotory se vyuˇz´ıvaj´ı v aplikac´ıch s nejvyˇsˇs´ımi n´ aroky na pˇresn´e ˇr´ızen´ı pohybu, napˇr´ıklad v robotice nebo CNC obr´ abˇec´ıch stroj´ıch. Krokov´ e motory se vyznaˇcuj´ı speci´aln´ı konstrukc´ı statoru s vynikl´ ymi p´oly, na kter´ ych jsou um´ıstˇeny c´ıvky vytv´ aˇrej´ıc´ı magnetick´e pole. Rotor je vybaven bud’ permanentn´ımi magnety nebo obsahuje rovnˇeˇz vynikl´e p´oly z feromagnetick´eho materi´alu. V druh´em pˇr´ıpadˇe vznik´ a toˇciv´ y moment v d˚ usledku tzv. reluktanˇcn´ıho jevu, kdy se magnetick´ y obvod statoru a rotoru snaˇz´ı zaujmout polohu s nejmenˇs´ım magnetick´ ym odporem. Takov´ yto aktu´ ator se oznaˇcuje jako sp´ınan´ y reluktanˇcn´ı motor. Postupn´ ym pˇrep´ın´an´ım jednotliv´ ych c´ıvek doch´ az´ı ke krokov´an´ı - pootoˇcen´ı motoru o urˇcit´ y definovan´ yu ´hel (ˇr´ adovˇe 100-20000 krok˚ u na ot´aˇcku). Postupn´e pˇrep´ın´an´ı vinut´ı zajiˇst’uje zvl´aˇstn´ı elektronick´ y obvod. V´ yhodou tˇechto motor˚ u je jednoduch´a a levn´a konstrukce a moˇznost provozu bez zpˇetnovazebn´ıho ˇcidla. Nev´ yhodou je vyˇsˇs´ı hluˇcnost a pulzov´an´ı momentu, coˇz vypl´ yv´ a z jejich diskr´etn´ıho zp˚ usobu pohybu. Dynamick´e vlastnosti jsou nav´ıc znaˇcnˇe z´ avisl´e na zat´ıˇzen´ı a v urˇcit´ ych reˇzimech m˚ uˇze doch´azet k vynech´av´an´ı krok˚ u nebo nestabiln´ımu chodu. Tyto motory se uplatˇ nuj´ı v zaˇr´ızen´ıch typu skener˚ u nebo tisk´aren, k pohonu jednoduch´ ych robot˚ u a k ˇr´ızen´ı pohybu levnˇejˇs´ıch obr´abˇec´ıch stroj˚ u.
16
2.5 Automatick´ a regulace v syst´ emech ˇ r´ızen´ı pohybu
PROGRAM STROJE
OPERÁTOR
posloupnost příkazů, logické řízení
ovládání GENERÁTOR TRAJEKTORIE
žádaná trajektorie pohybu v čase
MONITOROVÁNÍ DIAGNOSTIKA bezpečnost
REGULÁTOR POHYBU zpětná vazba
řízení výkonových členů
SENZORY,AKTUÁTORY PRAC.MECHANISMUS
´ Obr´ azek 2.4: Urovnˇ eˇ r´ıdic´ıho syst´ emu
2.5
Automatick´ a regulace v syst´ emech ˇ r´ızen´ı pohybu
Z pohledu funkc´ı, kter´e v syst´emech ˇr´ızen´ı pohybu obstar´av´a ˇr´ıdic´ı syst´em, lze v nˇem obsaˇzen´e algoritmy rozdˇelit do nˇekolika z´akladn´ıch u ´rovn´ı: • Nejniˇ zˇ s´ı u ´ roveˇ nˇ r´ızen´ı - regul´ ator pohybu - Na t´eto u ´rovni prob´ıh´a ˇr´ızen´ı jednotliv´ ych aktu´ ator˚ u a v´ ykonov´ ych ˇclen˚ u na z´akladˇe pokyn˚ u z vyˇsˇs´ıch vrstev syst´emu. Regul´ ator pohybu ˇreˇs´ı probl´emy spojen´e se stabilizac´ı, sledov´an´ım referenˇcn´ıch trajektori´ı pohybu, potlaˇcen´ım poruch, kompenzac´ı nelinearit v mechanick´em obvodu atd. • Vyˇ sˇ s´ı u ´ roveˇ nˇ r´ızen´ı - gener´ ator trajektorie - Tato vrstva na z´akladˇe pokyn˚ u od oper´ atora nebo podle pˇredem naprogramovan´e posloupnosti operac´ı vytv´ aˇr´ı ˇcasov´ y pr˚ ubˇeh poˇzadovan´eho pohybu. Poˇzadavky na vykon´avan´ y pohyb se liˇs´ı podle konkr´etn´ı aplikace. M˚ uˇze se jednat o ˇz´adost o pˇresun manipul´atoru z jednoho bodu do druh´eho, pohyb robotick´eho ramene po urˇcit´e kˇrivce v prostoru nebo zapoˇcet´ı obr´ abˇen´ı pˇri konstantn´ı ˇrezn´e s´ıle. Poˇzadovan´ y pohyb m˚ uˇze b´ yt reprezentov´ an jako funkce zrychlen´ı, ot´aˇcek, polohy nebo momentu/s´ıly v ˇcase.
17
ˇ´ ´ ´ 2. R IZEN´ I POHYBU V MECHATRONICKYCH SYSTEMECH
ˇ adan´e hodnoty jednotliv´ Z´ ych veliˇcin jsou pak pˇred´av´any modulu regul´atoru pohybu, kter´ y mus´ı zajistit co nejpˇresnˇejˇs´ı vykon´an´ı napl´anovan´ ych trajektori´ı pomoc´ı jednotliv´ ych aktu´ ator˚ u syst´emu. • Monitorov´ an´ı a diagnostika - Tato u ´roveˇ n ˇreˇs´ı ot´azky bezpeˇcnosti provozu cel´eho zaˇr´ızen´ı. Jej´ı souˇc´ ast´ı b´ yv´ a syst´em detekce chyb a havarijn´ıch stav˚ u (v´ ypadek senzoru, aktu´ atoru nebo zdroje energie, kolize s pˇrek´aˇzkou v prostoru, u ´nik provozn´ı kapaliny atd.). Pokroˇcil´e metody detekce chyb zaloˇzen´e na modelu syst´emu a odhadov´ an´ı kritick´ ych pˇr´ıznak˚ u z mˇeˇren´ı vstupn´ıch a v´ ystupn´ıch sign´al˚ u umoˇzn ˇuj´ı vyhodnocovat zmˇeny v chov´an´ı syst´emu jeˇstˇe pˇred vznikem havarijn´ıch stav˚ u a bezpeˇcn´ ym odstaven´ım minimalizovat pˇr´ıpadn´e ˇskody (viz napˇr. [16]).
2.5.1
Gener´ ator trajektorie
´ Ulohy vyskytuj´ıc´ı se pˇri n´ avrhu t´eto vrstvy ˇr´ızen´ı lze rozdˇelit do nˇekolika z´akladn´ıch skupin: • Pl´ anov´ an´ı trajektorie v prostoru s pˇ rek´ aˇ zkami - Tyto u ´lohy se typicky vyskytuj´ı u aplikac´ı mobiln´ıch robot˚ u. Robot je vybaven senzory s omezen´ ym dosahem, kter´e umoˇzn ˇuj´ı detekci pˇrek´aˇzek v nezn´am´em prostˇred´ı. C´ılem je pˇresun z poˇc´ateˇcn´ı do c´ılov´e polohy v co nejkratˇs´ım ˇcase nebo pˇri nejmenˇs´ı ujet´e vzd´alenosti bez kolize s nˇekterou z pˇrek´ aˇzek [17, 18]. Uplatˇ nuj´ı se zde pˇredevˇs´ım metody strojov´eho vn´ım´ an´ı a umˇel´e inteligence, protoˇze analytick´e ˇreˇsen´ı vˇetˇsinou nen´ı moˇzn´e nal´ezt. Tato pr´ ace je zamˇeˇrena na pr˚ umyslov´e aplikace, kde pracovn´ı prostor je bud’to bez pˇrek´ aˇzek nebo s pˇrek´aˇzkami pˇredem urˇcen´ ymi. Tvar trajektorie v prostoru je tedy vˇetˇsinou pˇredem zad´an v programu stroje. • Generov´ an´ı trajektorie pohybu s ohledem na fyzick´ e omezen´ı jednotliv´ ych aktu´ ator˚ u - Kaˇzd´ y pohon jakoˇzto re´aln´e zaˇr´ızen´ı nem˚ uˇze vykon´avat libovoln´ y pohyb, jeho konstrukc´ı je omezena maxim´aln´ı rychlost, zrychlen´ı nebo moment/s´ıla. Dalˇs´ı omezen´ı plynou z vlastn´ı konfigurace stroje. Pˇriliˇs prudk´e akˇcn´ı z´asahy mohou zp˚ usobit poˇskozen´ı nebo nadmˇern´e opotˇreben´ı nˇekter´ ych mechanick´ ych prvk˚ u konstrukce. Gener´ ator mus´ı respektovat tato omezen´ı a jeho v´ ystupem mus´ı b´ yt na dan´em zaˇr´ızen´ı fyzik´alnˇe realizovateln´ y pohyb [19, 20, 21, 22]).
18
2.5 Automatick´ a regulace v syst´ emech ˇ r´ızen´ı pohybu
ˇ • Casovˇ e optim´ aln´ı pohyb pod´ el zvolen´ e trajektorie - V nˇekter´ ych u ´loh´ach (napˇr. manipul´ atory typu pick-and-place, vysokorychlostn´ı CNC obr´abˇen´ı) m˚ uˇze b´ yt poˇzadov´ ano proveden´ı pohybu v co moˇzn´a nejkratˇs´ım ˇcase. Toto vede na u ´lohy ˇcasovˇe optim´ aln´ıho ˇr´ızen´ı s omezen´ım jak na vstup tak na stav syst´emu [23, 24, 25, 26, 27]. • Tlumen´ı rezidu´ aln´ıch vibrac´ı mechanick´ e konstrukce - Pˇr´ıliˇs razantn´ı pohyb m˚ uˇze vybudit mechanickou ˇc´ast stroje na jeho rezonanˇcn´ıch frekvenc´ıch, pokud nen´ı zajiˇstˇena dostateˇcn´a tuhost konstrukce. Vznikaj´ıc´ı vibrace mohou m´ıt za n´ asledek zhorˇsen´ı kvality regulace nebo i poˇskozen´ı zaˇr´ızen´ı. Vhodn´ ym tvarov´ an´ım trajektorie pohybu pomoc´ı speci´aln´ıch tvarovac´ıch filtr˚ u lze tyto neˇz´adouc´ı kmity omezit [28, 29, 30, 31, 32]. Probl´em tlumen´ı vibrac´ı obecnˇe zasahuje i do vrstvy regul´ atoru pohybu (napˇr. pˇri aktivn´ı zpˇetnovazebn´ı kompenzaci). • Navazov´ an´ı r˚ uzn´ ych trajektori´ı - Od oper´atora nebo z nadˇrazen´eho syst´emu m˚ uˇze pˇrij´ıt povel k vykon´an´ı nov´eho pohybu jeˇstˇe v dobˇe, kdy pˇredchoz´ı pohyb nebyl dokonˇcen. Gener´ ator mus´ı za bˇehu zajistit plynul´e pˇrepl´anov´an´ı trajektorie (tzv. blending, viz [33]). • Synchronizovan´ y pohyb ve dvou a v´ıce os´ ach - elektronick´ e vaˇ cky a pˇ revodovky - Elektronick´a pˇrevodovka nahrazuje jej´ı mechanick´ y protˇejˇsek, c´ılem ˇr´ıdic´ıho syst´emu je udrˇzovat konstantn´ı pomˇer ot´aˇcek mezi dvˇema pohony, z nichˇz jeden je ˇr´ızen nez´avisle (pomysln´a vstupn´ı strana pˇrevodovky) a druh´ y odvozuje sv˚ uj pohyb na z´akladˇe pohybu prvn´ıho (v´ ystup pˇrevodovky). D˚ uvodem nasazen´ı m˚ uˇze b´ yt sn´ıˇzen´ı n´aklad˚ u oproti sloˇzit´emu mechanick´emu pˇrevodu nebo obt´ıˇznˇe realizovateln´e fyzick´e propojen´ı mezi dvˇema pohony na vˇetˇs´ı vzd´alenost. Zobecnˇen´ım je potom aplikace elektronick´e vaˇcky, kdy nam´ısto konstantn´ıho pomˇeru rychlost´ı je trvale udrˇzov´ana z´avislost polohy ˇr´ıd´ıc´ı (master) a ˇr´ızen´e (slave) osy. C´ılem je nahradit mechanick´e vaˇcky, kter´e pˇren´aˇs´ı pohyb centr´aln´ı (kr´alovsk´e) hˇr´ıdele na pracovn´ı mechanismus. Toho se vyuˇz´ıv´a u stroj˚ u s velk´ ym poˇctem pomocn´ ych pohon˚ u, kter´e je nutno vz´ajemnˇe synchronizovat a ˇr´ıdit jejich rychlost (textiln´ı, tiskaˇrsk´e, znaˇckovac´ı a ˇrezac´ı stroje). Z´avislost os master-slave (tzv. vaˇckov´ y profil) se zad´ av´ a nejˇcastˇeji pomoc´ı geometrick´e kˇrivky bud’to ruˇcnˇe, nebo pˇrepoˇcten´ım z tvaru mechanick´e vaˇcky, kter´a m´a b´ yt nahrazena ve vhodn´em
19
ˇ´ ´ ´ 2. R IZEN´ I POHYBU V MECHATRONICKYCH SYSTEMECH
softwarov´em n´ astroji. Kˇrivka, kter´a popisuje vaˇckov´ y profil by mˇela b´ yt hladk´a alespoˇ n v prvn´ıch dvou derivac´ıch pro plynul´e vykon´an´ı pohybu bez silov´ ych r´az˚ u, d˚ uleˇzit´ a je tak´e spojitost tˇechto derivac´ı na okraj´ıch pro zajiˇstˇen´ı plynul´eho periodick´eho pohybu. Pˇri v´ ypoˇctu vaˇckov´ ych profil˚ u nach´azej´ı uplatnˇen´ı metody polynomi´ aln´ı interpolace a teorie spline kˇrivek [34, 35, 36, 37, 38]. V´ yhodou elektronick´e vaˇcky je moˇznost snadn´e zmˇeny funkce stroje prostou u ´pravou vaˇckov´eho profilu v programu a vyˇsˇs´ı pˇresnost regulace, kter´a se nezhorˇsuje opotˇreben´ım mechanick´e vaˇcky. • Koordinovan´ y pohyb v prostoru u syst´ em˚ u s vˇ etˇ s´ım poˇ ctem aktu´ ator˚ u Pohyb v prostoru m˚ uˇze b´ yt realizov´an nejr˚ uznˇejˇs´ımi zp˚ usoby, od nejjednoduˇsˇs´ıho pohybu po pˇr´ımce nebo ˇc´ asti kruˇznice aˇz po prostorov´e spline kˇrivky, kter´e umoˇzn ˇuj´ı dostateˇcnˇe obecn´e tvarov´an´ı trajektorie [37, 39, 40]. Na stranˇe uˇzivatele je pohyb zad´ av´ an obvykle v souˇradnic´ıch koncov´eho efektoru stroje (pr˚ ubˇeh pozice a orientace). Uvnitˇr gener´ atoru trajektorie je ovˇsem nutn´e prov´est kinemaˇ sen´ı kinematick´ tickou transformaci na pohyby jednotliv´ ych d´ılˇc´ıch pohon˚ u. Reˇ ych transformac´ı je jednou ze z´ akladn´ıch u ´loh robotiky [19, 20, 41, 42]. Ve snaze sjednotit uˇzivatelsk´e rozhran´ı pˇri pr´aci se syst´emy ˇr´ızen´ı pohybu vydala organizace PLCopen sdruˇzuj´ıc´ı pˇredn´ı svˇetov´e v´ yrobce ˇr´ıdic´ıch syst´em˚ u normu Motion Control, kter´ a m´ a usnadnit programov´an´ı a zajistit kompatibilitu mezi jednotliv´ ymi v´ yrobci hardwaru [33]. Tato norma definuje podobu funkˇcn´ıch blok˚ u pro ˇr´ızen´ı pohybu, jejich vstupy, v´ ystupy a poˇzadovan´e chov´an´ı pˇri pouˇzit´ı z pohledu uˇzivatele. Zab´ yv´a se pouze ˇc´ast´ı pro generov´ an´ı trajektorie, vrstva regul´atoru pohybu je jiˇz z´avisl´a na konkr´etn´ım hardwaru a pouˇzit´ ych aktu´atorech. Samostatnou oblast´ı jsou v tomto smˇeru CNC obr´abˇec´ı stroje, jejichˇz programov´an´ı je jiˇz delˇs´ı dobu standardizov´ ano pouˇzit´ım normovan´eho jazyka, tzv. G-k´odu, pro popis jednotliv´ ych operac´ı pˇri obr´ abˇen´ı. T´ımto je zajiˇstˇena kompatibilita se softwarem pro poˇc´ıtaˇcovou konstrukci a modelov´ an´ı a vznik´a koncept poˇc´ıtaˇcov´e podpory v´ yroby (CAM - Computer aided manufacturing) [39, 40, 43, 44]. Probl´emu generov´ an´ı trajektorie je vˇenov´ana tˇret´ı kapitola t´eto pr´ace, kde jsou detailnˇeji rozebr´ any jednotliv´e v praxi typick´e u ´lohy.
20
2.5 Automatick´ a regulace v syst´ emech ˇ r´ızen´ı pohybu
GENERÁTOR TRAJEKTORIE Žádaná trajektorie pohybu v čase
Regulátor polohy
v*
Regulátor otáček
i*
buzení motoru Regulátor proudu/momentu
M
proud - i otáčky - v poloha - s
REGULÁTOR POHYBU Obr´ azek 2.5: Sch´ ema regul´ atoru pohybu jedn´ e osy - kask´adn´ı struktura
2.5.2
Regul´ ator pohybu
Modul regul´ atoru pohybu zajiˇst’uje spr´avn´e sledov´an´ı referenˇcn´ıch trajektori´ı vypoˇcten´ ych v gener´ atoru. Tato ˇc´ ast pracuje vˇzdy se zpˇetnou vazbou od aktu´aln´ıch hodnot polohy, rychlosti pˇr´ıpadnˇe s´ıly nebo momentu. Zpˇetn´a vazba je nutn´a pro zajiˇstˇen´ı minim´ aln´ı odchylky od poˇzadovan´ ych hodnot i pˇri p˚ usoben´ı vnˇejˇs´ıch poruch, nepˇresnostem v modelu syst´emu atd. V oblasti regulace elektrick´ ych pohon˚ u se ust´alila kask´adn´ı struktura, kter´ a je vyuˇz´ıv´ ana ve vˇetˇsinˇe pˇr´ıpad˚ u. Skl´ad´a se ze tˇr´ı hierarchicky ˇrazen´ ych regulaˇcn´ıch smyˇcek proudu, ot´aˇcek a polohy (Obr. 2.5). Nejvnitˇrnˇejˇs´ı smyˇcka ˇr´ıd´ı proud motoru ovl´ ad´ an´ım v´ ykonov´ ych ˇclen˚ u pohonu. Proud je pˇri konstantn´ım buzen´ı magnetick´eho pole pˇr´ımo u ´mˇern´ y kroutic´ımu momentu a t´ım i zrychlen´ı. Dalˇs´ı smyˇcka v poˇrad´ı jiˇz ˇr´ıd´ı ot´ aˇcky pohonu a zad´av´a poˇzadovanou hodnotu regul´atoru proudu. Posledn´ı smyˇcka slouˇz´ı k regulaci polohy a v´ ystupem je tentokr´at ˇz´adan´a hodnota rychlosti. V´ yhoda takto zapojen´e kask´adn´ı struktury je v minimalizaci zpoˇzdˇen´ı mezi p˚ usob´ıc´ı poruchou a mˇeˇren´ ym v´ ystupem. Vnitˇrn´ı proudov´a smyˇcka zajiˇst’uje regulaci momentu a potlaˇcuje pˇredevˇs´ım poruchy p˚ usob´ıc´ı v elektrick´em obvodu motoru, tj. napˇr. zmˇeny odporu a indukˇcnosti vlivem oteplen´ı statorov´eho vinut´ı, zpˇetn´e elektromotorick´e napˇet´ı p˚ usob´ıc´ı proti zdroji nebo pokles napˇet´ı na zdroji nebo v meziobvodu v´ ykonov´eho ˇclenu. Rychlostn´ı smyˇcka kompenzuje poruchu zp˚ usobenou vnˇejˇs´ım z´atˇeˇzn´ ym momentem a zmˇeny v mechanick´ ych parametrech ˇr´ızen´eho syst´emu (napˇr.
21
ˇ´ ´ ´ 2. R IZEN´ I POHYBU V MECHATRONICKYCH SYSTEMECH
zmˇena hmotnosti/momentu setrvaˇcnosti syst´emu pˇri uchopen´ı bˇremene, neline´arn´ı pr˚ ubˇeh tˇren´ı v prac. mechanismu). Polohov´a smyˇcka pak zajiˇst’uje pˇresnost polohov´an´ı i pˇri p˚ usoben´ı vˇsech v´ yˇse zm´ınˇen´ ych poruch. Dalˇs´ı v´ yhodou kask´adn´ı struktury je moˇznost jednoduch´eho zahrnut´ı omezen´ı na maxim´aln´ı ot´aˇcky a proud/moment pohonu. Jednotliv´e regulaˇcn´ı smyˇcky je nav´ıc moˇzn´e ladit ruˇcnˇe na z´akladˇe prov´adˇen´ ych experiment˚ u a to postupnˇe od proudov´e, pˇres rychlostn´ı aˇz po polohovou. Mezi kl´ıˇcov´e probl´emy n´ avrhu regul´ atoru pohybu patˇr´ı: • V´ ybˇ er vhodn´ eho algoritmu pro zpˇ etnovazebn´ı ˇ r´ızen´ı - V pr˚ umyslu se u velk´ ych v´ yrobc˚ u servotechniky ust´ alilo pouˇz´ıv´an´ı line´arn´ıch regul´ator˚ u typu PID. D˚ uvodem je jejich masov´e rozˇs´ıˇren´ı v oblasti ˇr´ızen´ı proces˚ u a t´ım i lety provˇeˇren´e univerz´ aln´ı algoritmy ˇr´ızen´ı, relativnˇe snadn´e ladˇen´ı mal´eho poˇctu parametr˚ ua obecn´e povˇedom´ı o tˇechto regul´ atorech a jejich vlastnostech v inˇzen´ yrsk´e praxi. Ve speci´ aln´ıch pˇr´ıpadech jsou vˇsak nahrazov´any sloˇzitˇejˇs´ımi algoritmy neline´arn´ıho, prediktivn´ıho nebo adaptivn´ıho ˇr´ızen´ı [45, 46, 47, 48], do regulace pohybu pronikaj´ı i techniky umˇel´e inteligence ve formˇe neuronov´ ych s´ıt´ı nebo fuzzy logiky [49, 50]. Tyto pokroˇcil´e techniky ˇcasto pˇrin´aˇsej´ı zv´ yˇsen´ı v´ ykonu a kvality ˇr´ızen´ı pro konkr´etn´ı aplikaci, zat´ım se vˇsak nedoˇckaly vˇetˇs´ıho rozˇs´ıˇren´ı, protoˇze nenab´ızej´ı univerz´ aln´ı ˇreˇsen´ı pro ˇsirok´e spektrum u ´loh, kter´e se vyskytuj´ı v syst´emech ˇr´ızen´ı pohybu. • Sledov´ an´ı referenˇ cn´ı trajektorie - Modul regul´atoru mus´ı zajistit co nejpˇresnˇejˇs´ı sledov´ an´ı referenˇcn´ıch hodnot pˇred´avan´ ych gener´atorem trajektorie ve formˇe poˇzadovan´eho pr˚ ubˇehu polohy, rychlosti, zrychlen´ı nebo s´ıly. • Kompenzace nelinearit v mechanick´ eˇ c´ asti syst´ emu - Typick´ y mechatronick´ y syst´em obsahuje celou ˇradu neline´arn´ıch prvk˚ u. Pˇr´ıkladem m˚ uˇze b´ yt statick´a charakteristika (napˇr. pˇrevod napˇet´ı na s´ılu u elektromagnetick´eho nebo pneumatick´eho aktu´ atoru), neline´ arn´ı pr˚ ubˇeh tˇren´ı, v˚ ule v kloubech a pˇrevodovk´ach, hystereze v pracovn´ım mechanismu atd. Vˇsechny tyto faktory maj´ı negativn´ı vliv na kvalitu regulace a ˇr´ıd´ıc´ı syst´em je mus´ı vhodn´ ym zp˚ usobem eliminovat nebo alespoˇ n dostateˇcnˇe potlaˇcit. Uplatˇ nuj´ı se metody dopˇredn´e kompenzace, ditheringu, neline´ arn´ıho ˇr´ızen´ı s klouzav´ ym reˇzimem nebo adaptivn´ı metody ˇr´ızen´ı [45, 51].
22
2.6 Robotick´ e manipul´ atory
• Zajiˇ stˇ en´ı robustnosti v˚ uˇ ci p˚ usob´ıc´ım poruch´ am a zmˇ en´ am v parametˇ ıdic´ı syst´em ˇcasto pracuje v podm´ınk´ach neurˇcitosti - na sysrech syst´ emu - R´ t´em p˚ usob´ı vnˇejˇs´ı poruchy (napˇr. reakˇcn´ı s´ıly pˇri obr´abˇen´ı nebo interakci s prostˇred´ım, p˚ usoben´ı gravitace), ˇcasto se mˇen´ı parametry syst´emu (zmˇena hmotnosti/momentu setrvaˇcnosti pˇri manipulac´ıch s bˇremenem, obecnˇe neline´arn´ı dynamika u sloˇzit´ ych v´ıceos´ ych syst´em˚ u). Na vˇsechny tyto zmˇeny mus´ı regul´ator pohybu adekv´ atnˇe reagovat a zajistit pˇrijatelnou kvalitu ˇr´ızen´ı a sledov´an´ı referenˇcn´ıho pohybu. • Tlumen´ı mechanick´ ych vibrac´ı - Pˇri n´avrhu mechatronick´ ych syst´em˚ u vznikaj´ı protich˚ udn´e poˇzadavky na vlastnosti mechanick´e konstrukce. Na jedn´e stranˇe je vyˇzadov´ ana dostateˇcn´a robustnost a tuhost s c´ılem omezit vibrace, na druh´e stranˇe snaha minimalizovat rozmˇery a n´aklady na materi´al, sn´ıˇzit setrvaˇcn´e hmoty atd. Odlehˇcov´ an´ı konstrukce pˇrin´aˇs´ı probl´emy se vznikem vibrac´ı vybuzen´ ych pohybem stroje. Mechanickou tuhost je moˇzn´e zv´ yˇsit implementac´ı vhodn´eho algoritmu ˇr´ızen´ı. Rozliˇsuje se pasivn´ı tlumen´ı bez zpˇetn´e vazby, kdy regul´ator je doplnˇen o vhodn´ y frekvenˇcn´ı filtr typu p´asmov´e z´adrˇze, kter´ y zabraˇ nuje vybuzen´ı mechanick´e konstrukce na kritick´ ych slabˇe tlumen´ ych frekvenc´ıch [29, 30, 31, 32, 52, 53]. Do syst´emu nen´ı dod´av´ana ˇz´adn´a dodateˇcn´a energie pro potlaˇcen´ı vibrac´ı. Pˇredpokladem je znalost modelu syst´emu a jeho rezonanˇcn´ıch frekvenc´ı. Pokud se dynamika syst´emu mˇen´ı v ˇcase je ˇcasto nutn´e vyuˇz´ıt technik pˇrep´ın´an´ı mezi v´ıce regul´ atory (gain-scheduling) nebo adaptivn´ıho ˇr´ızen´ı [54]. Pˇri aktivn´ım tlumen´ı je pˇr´ımo vyuˇz´ıv´ ana zpˇetn´a vazba bud’to z fyzick´ ych senzor˚ u nebo z rekonstruktoru stavu a je navrˇzen vhodn´ y kompenz´ator, kter´ y ovlivˇ nuje dynamiku uzavˇren´e smyˇcky s c´ılem stabilizovat syst´em a zatlumit vibrace v cel´em urˇcit´em rozsahu frekvenc´ı [46, 54, 55, 56]. Kompenz´ator dod´av´a do syst´emu energii z vnˇejˇs´ıho zdroje a pˇri ˇspatn´e funkci hroz´ı nestabilita.
2.6
Robotick´ e manipul´ atory
V oblasti pr˚ umyslov´e v´ yroby doch´az´ı k neust´al´emu zvyˇsov´an´ı poˇzadavk˚ u na produktivitu, pˇresnost a bezpeˇcnost cel´eho technologick´eho procesu. Tento trend vede ke st´ ale ˇcastˇejˇs´ımu nasazov´ an´ı automatizovan´ ych v´ yrobn´ıch zaˇr´ızen´ı, jejichˇz d˚ uleˇzitou souˇc´ast´ı se st´ avaj´ı roboty a manipul´ atory. Robotizovan´a pracoviˇstˇe nahrazuj´ı lidskou s´ılu pˇri
23
ˇ´ ´ ´ 2. R IZEN´ I POHYBU V MECHATRONICKYCH SYSTEMECH
opakovan´em prov´ adˇen´ı monot´ onn´ıch u ´kol˚ u s d˚ urazem na rychlost, pˇresnost a efektivnost. D˚ uleˇzit´ ym aspektem je tak´e moˇznost pr´ace ve ˇskodliv´em nebo ˇzivotu nebezpeˇcn´em prostˇred´ı. Problematice robotiky je vˇenov´ ana dlouh´a ˇrada publikac´ı, kter´a pokr´ yv´a vˇsechny moˇzn´e oblasti t´eto discipl´ıny od n´ avrhu a optimalizace mechanick´e konstrukce, pˇres modelov´an´ı a identifikaci po ˇr´ızen´ı a programov´an´ı [19, 20, 41, 42, 57]. Tato pr´ace je zamˇeˇrena na n´ avrh algoritm˚ u pro ˇr´ızen´ı pohybu, proto je d´ale uveden pouze velmi struˇcn´ y pˇrehled z´ akladn´ıch pojm˚ u. Podle vykon´ avan´e ˇcinnosti lze roboty rozdˇelit do nˇekolika z´akladn´ıch skupin: 1. Teleoper´ atory - Do t´eto skupiny patˇr´ı manipul´atory d´alkovˇe ovl´adan´e ˇclovˇekem. Jejich funkc´ı je pˇren´ aˇset, pˇr´ıpadnˇe zesilovat pohyb lidsk´eho oper´atora. Uplatˇ nuj´ı se pˇri nebezpeˇcn´ ych manipulac´ıch ve stavaˇrstv´ı, v medic´ınsk´ ych operac´ıch, pˇri pr´aci ve vesm´ıru nebo ˇskodliv´em prostˇred´ı. 2. Jedno´ uˇ celov´ e manipul´ atory s pevn´ ym programem - Jednoduch´e automatizovan´e manipul´ atory ve formˇe dopravn´ık˚ u, podavaˇc˚ u, bal´ıc´ıch stroj˚ u apod. Zmˇena jejich funkce zpravidla vyˇzaduje podstatn´ y z´asah do konstrukce cel´eho syst´emu. V´ yhodou je jednoduchost, spolehlivost a n´ızk´e poˇrizovac´ı n´aklady. 3. Univerz´ aln´ı pr˚ umyslov´ e manipul´ atory - Tyto stroje se vyznaˇcuj´ı univerz´aln´ı pouˇzitelnost´ı pro celou ˇradu u ´kon˚ u pˇri pr˚ umyslov´e v´ yrobˇe, napˇr´ıklad manipulaci s materi´ alem, obr´ abˇen´ı, obloukov´em a bodov´em svaˇrov´an´ı, brouˇsen´ı, lakov´an´ı, osazov´ an´ı a kompletaci stroj˚ u, paletizaci atd. Funkci robotu lze snadno zmˇenit u ´pravou jeho programu. 4. Adaptivn´ı roboty - Tyto stroje se vyznaˇcuj´ı vyˇsˇs´ı u ´rovn´ı ˇr´ızen´ı, kter´a umoˇzn ˇuje automatickou zmˇenu programu na z´akladˇe aktu´aln´ıho stavu okol´ı z´ıskan´eho z informac´ı od senzor˚ u s c´ılem splnit poˇzadovanou kvalitu ˇcinnosti podle nˇejak´eho krit´eria. Pˇr´ıkladem mohou b´ yt mobiln´ı roboty pro pr˚ uzkum vesm´ırn´ ych tˇeles. 5. Kognitivn´ı roboty - Tyto stroje se vyznaˇcuj´ı schopnost´ı autonomn´ı interakce s prostˇred´ım a projevem vysok´eho stupnˇe umˇel´e inteligence. Z´akladn´ımi znaky jsou schopnosti nalezen´ı optim´ aln´ı varianty pro splnˇen´ı urˇcit´eho c´ıle, uˇcen´ı se ze zkuˇsenost´ı, pˇrizp˚ usobivost okoln´ım podm´ınk´am, schopnost predikce ad. Pˇr´ıkladem m˚ uˇze b´ yt humanoidn´ı robot ASIMO nebo arm´adn´ı n´akladn´ı robot Big Dog.
24
2.6 Robotick´ e manipul´ atory
Obr´ azek 2.6: S´ eriov´ a a paraleln´ı struktura - manipul´atory KUKA, ABB
Tato pr´ ace je zamˇeˇrena pˇredevˇs´ım na pr˚ umyslov´e manipul´atory, kde je ˇr´ızen´ı pohybu kl´ıˇcov´ ym probl´emem n´ avrhu cel´eho zaˇr´ızen´ı a jsou zde kladeny nejvyˇsˇs´ı n´aroky na pˇresnost, rychlost a vlastn´ı pr˚ ubˇeh pohybu. U adaptivn´ıch a kognitivn´ıch robot˚ u pˇrevaˇzuj´ı metody umˇel´e inteligence, strojov´eho vn´ım´an´ı, autonomn´ıho rozhodov´an´ı atd. Prov´ adˇen´ı pohybu je zde pouze druhotn´ ym projevem vyˇsˇs´ıho rozhodovac´ıho syst´emu, kter´ y slouˇz´ı ke splnˇen´ı urˇcit´eho c´ıle. Pˇresto jsou syst´emy ˇr´ızen´ı pohybu nezbytnou souˇc´ast´ı ve vˇsech aplikac´ıch robot˚ u jakoˇzto z´akladn´ı prvek pˇri interakci s okoln´ım prostˇred´ım.
2.6.1
Struktura robotick´ ych manipul´ ator˚ u
Robotick´ y manipul´ ator se zpravidla skl´ad´a z nˇekolika z´akladn´ıch ˇc´ast´ı. Prvn´ı je pevn´ a nepohybliv´ a ˇc´ ast, z´ akladna manipul´ atoru, kter´a urˇcuje z´akladn´ı souˇradn´ y syst´em stroje. Na tu jsou postupnˇe upevnˇena ramena manipul´ atoru vykon´avaj´ıc´ı vlastn´ı pohyb cel´eho robotu. Ramena jsou navz´ajem propojena pomoc´ı kloub˚ u, jejichˇz druh a poˇcet urˇcuje rozsah prov´ adˇen´eho pohybu a poˇcet stupˇ n˚ u volnosti (napˇr. prismatick´ y, rotaˇcn´ı, sf´erick´ y atd.). Klouby mohou b´ yt poh´anˇen´e aktu´atory ve formˇe line´arn´ıch nebo rotaˇcn´ıch pohon˚ u, potom se mluv´ı o kloubech aktivn´ıch. Opakem jsou pasivn´ı klouby, kter´e pouze vymezuj´ı rozsah pohybu dvou sousedn´ıch ramen. Spojen´ı vˇsech ramen a
25
ˇ´ ´ ´ 2. R IZEN´ I POHYBU V MECHATRONICKYCH SYSTEMECH
kloub˚ u vytv´ aˇr´ı kinematickou strukturu manipul´atoru. Posledn´ı ˇc´ast´ı je koncov´y efektor ve formˇe vhodn´eho pracovn´ıho n´ astroje, kter´ y pˇrich´az´ı do kontaktu s okoln´ım prostˇred´ım. D˚ uleˇzit´ ym parametrem manipul´ atoru je tvar a velikost pracovn´ıho prostoru, kter´ y je definov´an jako mnoˇzina vˇsech dosaˇziteln´ ych pozic koncov´eho efektoru, pˇri omezen´ıch dan´ ych konstrukc´ı robotu. Natoˇcen´ı nebo vysunut´ı jednotliv´ ych aktu´ator˚ u urˇcuje tzv. vektor kloubov´ych souˇradnic Θ, v´ ysledn´a poloha a orientace koncov´eho efektoru potom vektor zobecnˇen´ych souˇradnic X. Jednou z kl´ıˇcov´ ych u ´loh robotiky je potom nalezen´ı kinematick´ ych transformac´ı mezi kloubov´ ymi a zobecnˇen´ ymi souˇradnicemi. ´ Uloha pˇr´ım´e kinematiky spoˇc´ıv´ a v nalezen´ı transformace X = G(Θ), pˇrev´adˇej´ıc´ı natoˇcen´ı jednotliv´ ych kloub˚ u na souˇradnice efektoru. K n´ı opaˇcn´a u ´loha inverzn´ı kinematiky, Θ = G−1 (X), hled´ a opaˇcnou transformaci ze zobecnˇen´ ych souˇradnic do kloubov´ ych. V z´avislosti na kinematick´e struktuˇre je ˇcasto nemoˇzn´e naj´ıt poˇzadovanou transformaci analyticky z d˚ uvodu sloˇzit´ ych neline´ arn´ıch vztah˚ u a je nutn´e pˇristoupit napˇr. k nˇekter´e z numerick´ ych metod. Dalˇs´ım stupnˇem je pak ˇreˇsen´ı u ´loh diferenci´aln´ı kinematiky, kter´a ˇreˇs´ı pˇrevod rychlost´ı a zrychlen´ı mezi kloubov´ ymi a zobecnˇen´ ymi souˇradnicemi. Pˇri znaˇcn´e sloˇzitosti kinematick´ ych transformac´ı, kter´e je nutn´e derivovat podle ˇcasu, vznikaj´ı velmi komplikovan´e v´ yrazy. Pro implementaci tˇechto transformac´ı do ˇr´ıdic´ıho syst´emu je ˇcasto nutn´e kv˚ uli vysok´ ym v´ ypoˇcetn´ım n´arok˚ um pˇristoupit k vhodn´e aproximaci nebo pouˇzit´ı numerick´ ych metod. Podle tvaru kinematick´e struktury lze manipul´atory dˇelit do dvou z´akladn´ıch skupin (viz obr. 2.6). S´ eriov´ e manipul´ atory vznikaj´ı postupn´ ym napojov´an´ım ramen a kloub˚ u. Kinematick´ a struktura m´ a tvar otevˇren´eho ˇretˇezce. V´ yhodou tohoto uspoˇr´ad´an´ı je relativnˇe jednoduˇsˇs´ı ˇr´ızen´ı pohybu a jednoduchost samotn´e mechanick´e konstrukce. D´ıky tomu jsou dnes tyto manipul´ atory v praxi nejrozˇs´ıˇrenˇejˇs´ı. Nev´ yhodou je niˇzˇs´ı pˇresnost polohov´ an´ı koncov´eho efektoru vlivem superpozice vˇsech mechanick´ ych odchylek jednotliv´ ych pohybov´ ych prvk˚ u a celkovˇe niˇzˇs´ı tuhost konstrukce pˇri zat´ıˇzen´ı. Paraleln´ı manipul´ atory maj´ı uzavˇrenou kinematickou strukturu, kdy koncov´ y efektor je souˇcasnˇe pˇripojen na v´ıce neˇz jedno rameno. V´ yhodou paraleln´ı struktury je vyˇsˇs´ı pˇresnost d´ıky pr˚ umˇerov´ an´ı mechanick´ ych chyb jednotliv´ ych prvk˚ u a vyˇsˇs´ı tuhost vlivem rozloˇzen´ı z´ atˇeˇze na vˇetˇs´ı poˇcet ramen. Vhodn´ ym n´avrhem konstrukce lze sn´ıˇzit celkovou hmotnost oproti s´eriov´emu uspoˇr´ad´an´ı pˇri stejn´em uˇziteˇcn´em zat´ıˇzen´ı. Nev´ yhodou paraleln´ıch robot˚ u je sloˇzitˇejˇs´ı ˇr´ızen´ı vlivem komplikovan´ ych kinematick´ ych transformac´ı mezi pohyby koncov´eho efektoru a jednotliv´ ych aktu´ator˚ u. Konstrukˇcn´ı
26
2.6 Robotick´ e manipul´ atory
probl´em m˚ uˇze pˇredstavovat geometricky omezen´ y rozsah pouˇziteln´ ych kloub˚ u, coˇz vede k horˇs´ımu pomˇeru velikosti pracovn´ıho prostoru vzhledem k celkov´emu objemu stroje. Tak jako v ostatn´ıch oblastech mechatroniky je d˚ uleˇzit´ ym aspektem pˇri konstrukci robot˚ u d˚ usledn´e uplatˇ nov´ an´ı princip˚ u simult´ann´ıho n´avrhu jednotliv´ ych ˇc´ast´ı stroje. Zvolen´ a kinematick´ a struktura, rozmˇery jednotliv´ ych ramen a typy pouˇzit´ ych kloub˚ u ovlivˇ nuj´ı velikost a tvar pracovn´ıho prostoru, nosnost robotu a moˇznosti jeho ˇr´ızen´ı. D˚ uleˇzit´e jsou pˇredevˇs´ım silov´e a pohybov´e z´avislosti mezi efektorem a jednotliv´ ymi pohony robotu a moˇznost v´ yskytu tzv. singul´arn´ıch poloh, kdy pˇri urˇcit´e konfiguraci jednotliv´ ych kloub˚ u m˚ uˇze doch´ azet ke ztr´atˇe ˇriditelnosti cel´eho syst´emu. Ke slovu pˇrich´azej´ı metody tzv. optim´ aln´ıho n´ avrhu manipul´atoru, kdy v prostoru parametr˚ u (d´elky ramen, jejich vz´ ajemn´e rozm´ıstˇen´ı atd.) je hled´ana konfigurace maximalizuj´ıc´ı urˇcit´e krit´erium optimality (napˇr. velikost pracovn´ıho prostoru). Z podstaty probl´emu toto vede obecnˇe na neline´ arn´ı optimalizaci s neline´arn´ımi omezen´ımi, kterou nen´ı moˇzn´e ˇreˇsit analyticky a ˇcasto se pˇristupuje na nejr˚ uznˇejˇs´ı numerick´e metody zaruˇcuj´ıc´ı pouze suboptim´aln´ı ˇreˇsen´ı ve formˇe lok´ aln´ıho maxima/minima dan´e kriteri´aln´ı funkce.
2.6.2
ˇ ızen´ı pohybu robotick´ R´ ych manipul´ ator˚ u
Struktura ˇr´ıdic´ıho syst´emu robotu odpov´ıd´a obecn´emu sch´ematu na obr. 2.4 a naznaˇcen´emu rozdˇelen´ı do tˇr´ı z´ akladn´ıch vrstev: Regul´ ator pohybu zajiˇst’uje spr´avn´e prov´adˇen´ı pohybu zad´avan´eho gener´atorem trajektorie. U jednoduch´ ych manipul´ator˚ u se m˚ uˇze jednat o pohyb v jedn´e translaˇcn´ı ˇci rotaˇcn´ı ose, univerz´ aln´ı manipul´atory jsou vˇsak zpravidla konstruov´any jako v´ıceos´e a je nutn´e ˇreˇsit spr´ avnou koordinaci vˇsech pohon˚ u robotu. Podle zp˚ usobu ˇr´ızen´ı se rozliˇsuj´ı dvˇe odliˇsn´e strategie. Pˇri decentralizovan´em ˇr´ızen´ı je poˇzadovan´ y pohyb efektoru inverzn´ı kinematickou transformac´ı pˇreveden na d´ılˇc´ı pˇr´ıspˇevky jednotliv´ ych aktu´ator˚ u a kaˇzd´ y z nich je potom ˇr´ızen jako nez´avisl´a jednotka. Vz´ajemn´e interakce jednotliv´ ych smyˇcek jsou u ´plnˇe zanedb´ any a uvaˇzov´any jako p˚ usob´ıc´ı porucha, pˇr´ıpadnˇe mohou b´ yt kompenzov´ any dopˇrednou nebo zpˇetnou vazbou ze znalosti dynamick´eho modelu syst´emu. V´ yhodou je jednoduˇsˇs´ı struktura regul´atoru, kter´a vˇsak pˇri zanedb´an´ı ˇc´asti dynamiky syst´emu vyˇzaduje vyuˇzit´ı technik robustn´ıho nebo adaptivn´ıho ˇr´ızen´ı pro zajiˇstˇen´ı kvalitn´ıho sledov´ an´ı referenˇcn´ıho pohybu [45, 46, 47]. V´ ysledkem je n nez´avisle
27
ˇ´ ´ ´ 2. R IZEN´ I POHYBU V MECHATRONICKYCH SYSTEMECH
Generátor trajektorie
požadovaná trajektorie koncového efektoru
Inverzní kinematická úloha
požadovaná trajektorie aktuátorǔ
Regulátor
síly (momenty) aktuátorǔ
Manipulátor
měření kloubových souřadnic
Generátor trajektorie
požadovaná trajektorie koncového efektoru
síly (momenty) aktuátorǔ
Regulátor
Přímá kinematická úloha
Manipulátor měření kloubových souřadnic
ˇ ızen´ı robotick´ Obr´ azek 2.7: R´ ych manipul´ ator˚ u - v prostoru kloubov´ ych a zobecnˇen´ ych souˇradnic
ˇr´ızen´ ych polohov´ ych smyˇcek jednotliv´ ych aktu´ator˚ u. Druh´ ym pˇr´ıstupem je centralizovan´e ˇr´ızen´ı, pˇri kter´em je uvaˇzov´ an kompletn´ı dynamick´ y model syst´emu vˇcetnˇe vazeb mezi jednotliv´ ymi aktu´ atory. V´ ystupem regul´atoru jsou pak poˇzadovan´e hodnoty s´ıly nebo momentu jednotliv´ ych pohon˚ u stroje. Bohuˇzel, i v´ yrazn´e zjednoduˇsen´ı popisu syst´emu (uvaˇzov´ an´ı soustavy v´ azan´ ych dokonale tuh´ ych tˇeles, zanedb´an´ı nelinearit jako tˇren´ı a v˚ ule v jednotliv´ ych kloubech) vede na velmi komplikovan´e neline´arn´ı dynamick´e modely a t´ım komplexn´ı a v´ ypoˇcetnˇe n´ aroˇcn´e nelie´arn´ı algoritmy ˇr´ızen´ı. V praxi se pak ukazuje, ˇze vzhledem k omezen´e platnosti pouˇzit´eho dynamick´eho modelu nemus´ı b´ yt obecnˇe v´ ysledek lepˇs´ı neˇz u decentralizovan´eho pˇr´ıstupu [19, 57]. Gener´ ator trajektorie (interpol´ ator) vytv´aˇr´ı ˇcasov´ y pr˚ ubˇeh poˇzadovan´eho pohybu na z´akladˇe pokyn˚ u oper´ atora nebo programu stroje a prov´ad´ı kinematick´e transformace mezi prostory kloubov´ ych a zobecnˇen´ ych souˇradnic. Rozliˇsuj´ı se dva z´akladn´ı pˇr´ıstupy. Pˇri ˇr´ızen´ı v prostoru kloubov´ych souˇradnic je poˇzadovan´ y pohyb efektoru pˇrev´adˇen inverzn´ı kinematickou transformac´ı na pohyby jednotliv´ ych aktu´ator˚ u, kter´e jsou ve formˇe referenˇcn´ıch hodnot pˇred´ any modulu regul´atoru. V´ yhodou je, ˇze regul´ator pohybu nemus´ı ˇreˇsit v´ ypoˇcetnˇe n´ aroˇcn´e kinematick´e transformace, regulaˇcn´ı smyˇcky mohou bˇeˇzet s menˇs´ı periodou vzorkov´an´ı, coˇz vede na pˇresnˇejˇs´ı sledov´an´ı pohybu. Nev´ yhodou je nemoˇznost kompenzace mechanick´ ych nepˇresnost´ı v konstrukci manipul´atoru, kter´e mohou zp˚ usobit odchylku koncov´eho efektoru od poˇzadovan´e trajektorie. Tento probl´em ˇreˇs´ı metody ˇr´ızen´ı v prostoru zobecnˇen´ych souˇradnic, kdy regul´atoru pohybu jsou pˇred´ av´ any pˇr´ımo referenˇcn´ı souˇradnice efektoru. Regul´ator potom vyuˇz´ıv´a pro sledov´an´ı zpˇetnou vazbu od skuteˇcn´e polohy efektoru mˇeˇren´e doplˇ nkov´ ymi ˇcidly
28
2.7 CNC obr´ abˇ ec´ı syst´ emy
Obr´ azek 2.8: CNC obr´ abˇ ec´ı syst´ emy - tˇr´ıos´a fr´ezka, soustruh
a t´ım dok´ aˇze kompenzovat pˇr´ıpadn´e mechanick´e odchylky. Instalace dodateˇcn´e instrumentace vˇsak v praxi ˇcasto nen´ı moˇzn´a. Senzory efektoru je moˇzn´e nahradit blokem pˇr´ım´e kinematick´e transformace, ˇc´ımˇz ovˇsem odpad´a hlavn´ı v´ yhoda t´eto metody, nav´ıc se do zpˇetnovazebn´ı smyˇcky regul´atoru zan´aˇs´ı v´ ypoˇcetnˇe n´aroˇcn´a u ´loha transformace souˇradnic, coˇz vede ke zhorˇsen´ı kvality ˇr´ızen´ı. Typick´e u ´lohy generov´an´ı trajektorie jsou uvedeny v dalˇs´ı kapitole pr´ ace. Pro u ´roveˇ n monitorov´ an´ı a diagnostiky plat´ı vˇsechny principy uveden´e pro obecn´ y pˇr´ıpad na zaˇc´ atku kapitoly.
2.7
CNC obr´ abˇ ec´ı syst´ emy
Stejnˇe jako v ostatn´ıch technick´ ych discipl´ın´ach pronikaj´ı principy mechatroniky tak´e do oblasti strojn´ı v´ yroby. Rozvoj poˇc´ıtaˇcov´e techniky, informaˇcn´ıch syst´em˚ u, elektroniky a nov´ ych metod automatick´eho ˇr´ızen´ı vedl k zaˇclenˇen´ı ˇc´ıslicov´eho ˇr´ıdic´ıho syst´emu do obr´ abˇec´ıch stroj˚ u [39, 40, 43, 44]. Tato inovace pˇrinesla z´asadn´ı zv´ yˇsen´ı produktivity v´ yroby a zcela nov´e funkˇcn´ı vlastnosti, kter´e byly pomoc´ı syst´em˚ u pˇredchoz´ı generace nedosaˇziteln´e. Pevn´ a logika dan´a zapojen´ım v´ ykonov´ ych obvod˚ u byla nahrazena poˇc´ıtaˇcov´ ym ˇr´ıdic´ım syst´emem se snadnou zmˇenou programu, coˇz umoˇznilo vyr´abˇet r˚ uzn´e d´ıly stejnou technologi´ı (tzv. pruˇzn´a automatizace), zkr´atily se v´ yrobn´ı ˇcasy, zlepˇsila se pˇresnost a opakovatelnost, zjednoduˇsila se obsluha stroje atd. D˚ uleˇzit´ ym aspektem je v´ yvoj CAD/CAM softwaru s integrovanou podporou konstrukce a v´ yroby, kter´ y v dneˇsn´ı dobˇe umoˇzn ˇuje ze strojn´ıho v´ ykresu automaticky generovat program pro vlastn´ı obr´abˇen´ı.
29
ˇ´ ´ ´ 2. R IZEN´ I POHYBU V MECHATRONICKYCH SYSTEMECH
Neust´ale se zvyˇsuj´ıc´ı n´ aroky na kvalitu a rychlost v´ yroby vedou kromˇe v´ yvoje potˇrebn´eho hardware tak´e k dokonalejˇs´ım a komplikovanˇejˇs´ım algoritm˚ um ˇr´ızen´ı. Probl´emy spojen´e s generov´ an´ım a sledov´ an´ım trajektorie pohybu nab´ yvaj´ı na d˚ uleˇzitosti pˇredevˇs´ım d´ıky poˇzadavku vysok´e pˇresnosti pohybu (dnes ˇr´adovˇe des´ıtky aˇz jednotky mikrometr˚ u), kter´ a je v´ yraznˇe vyˇsˇs´ı oproti ostatn´ım pr˚ umyslov´ ym aplikac´ım mechatronick´ ych syst´em˚ u. Poˇzadavky na rychlost a kvalitu jsou do znaˇcn´e m´ıry protich˚ udn´e. Zkracov´an´ı pracovn´ıch cykl˚ u stroje vol´a po metod´ach ˇcasovˇe optim´aln´ıho ˇr´ızen´ı, rychlejˇs´ı pohyb ovˇsem znamen´ a vyˇsˇs´ı poˇzadovanou ˇs´ıˇrku p´asma aktu´ator˚ u. Ta m˚ uˇze b´ yt obt´ıˇznˇe dosaˇziteln´ a vzhledem k omezen´emu rozliˇsen´ı zpˇetnovazebn´ıch ˇcidel, neust´ale p˚ usob´ıc´ım poruch´ am ve formˇe ˇrezn´e s´ıly pˇri obr´abˇen´ı nebo pot´ıˇzemi s buzen´ım vibrac´ı v konstrukci stroje. Stejnˇe jako v pˇr´ıpadˇe robot˚ u jsou nasazov´any robustn´ı nebo adaptivn´ı regul´ atory [21, 22, 45, 46]. Zvyˇsov´an´ı pˇresnosti tak´e vede na nutnost pouˇz´ıv´an´ı dokonalejˇs´ıch metod k popisu pr˚ ubˇehu pohybu. Dˇr´ıve uˇz´ıvan´e line´arn´ı a kruhov´e interpolace jsou dnes nahrazov´ any sloˇzitˇejˇs´ımi prostorov´ ymi kˇrivkami jako napˇr. Bezi´erovy, B-spline nebo nejobecnˇejˇs´ı NURBS funkce [58]. Tyto polynomi´aln´ı a racion´aln´ı interpolace vyˇsˇs´ıch ˇr´ ad˚ u vedou na hladk´ y pr˚ ubˇeh pohybu bez neˇz´adouc´ıch nespojitost´ı, ˇc´ımˇz pˇrisp´ıvaj´ı k dosaˇzen´ı vysok´e kvality obr´abˇen´ı bez nutnosti dodateˇcn´e povrchov´e u ´pravy. Mechanickou strukturu lze form´ alnˇe popsat stejn´ ym zp˚ usobem jako u robotick´ ych manipul´ator˚ u, liˇs´ı se pouze terminologie uˇz´ıvan´a pro pojmenov´an´ı jednotliv´ ych prvk˚ u stroje. Nam´ısto z´ akladny se tedy napˇr´ıklad mluv´ı o suportu, koncov´ y efektor je naz´ yv´an n´astrojem. Stejnˇe jako u robot˚ u se stroj skl´ad´a z nˇekolika ramen spojen´ ych klouby, kter´e urˇcuj´ı rozsah pohybu, velikost a tvar pracovn´ıho prostoru. Podle poˇctu os, kter´e mohou b´ yt pˇri obr´abˇen´ı souˇcasnˇe v ˇcinnosti lze rozdˇelit obr´abˇec´ı stroje do nˇekolika skupin [44]: • Jednoos´ e - napˇr. jedno´ uˇcelov´e vrtaˇcky • Dvouos´ e - fr´ezky, soustruhy, ˇrez´an´ı laserem, plazmou nebo vodn´ım paprskem • Tˇ r´ıos´ e - tˇr´ıos´ a kulov´ a fr´ezka, souˇradnicov´a vrtaˇcka • Pˇ etios´ e - tˇr´ıos´ a fr´ezka s moˇznost´ı volby orientace n´astroje • V´ıceos´ e - obr´ abˇec´ı centra
30
2.7 CNC obr´ abˇ ec´ı syst´ emy
Poˇcet ˇr´ızen´ ych os vyjadˇruje konstrukˇcn´ı vyspˇelost stroje, kter´a urˇcuje rovnˇeˇz sloˇzitost tvar˚ u, kter´e je moˇzn´e vyrobit. Obecn´ ym trendem posledn´ı doby je v´ yvoj v´ıceos´ ych v´ıceprofesn´ıch obr´ abˇec´ıch center, kter´e umoˇzn ˇuj´ı napˇr´ıklad postupnˇe soustruˇzit, fr´ezovat, tepelnˇe opracov´ avat pomoc´ı laseru, mˇeˇrit, brousit a to na jedno upnut´ı obrobku. ˇ ıdic´ı syst´em lze opˇet rozdˇelit do tˇr´ı z´akladn´ıch u R´ ´rovn´ı: Regul´ ator pohybu zajiˇst’uje spr´avn´e prov´adˇen´ı pohybu zad´avan´eho vyˇsˇs´ı vrstvou ˇr´ızen´ı, podle typu stroje v jedn´e nebo v´ıce os´ach. U draˇzˇs´ıch syst´em˚ u je ˇcasto pouˇz´ıv´ano pˇr´ım´e odmˇeˇrov´ an´ı n´ astroje pro zajiˇstˇen´ı vyˇsˇs´ı pˇresnosti polohov´an´ı. Jako aktu´atory jsou dnes ve vˇetˇsinˇe pˇr´ıpad˚ u pouˇzity elektrick´e synchronn´ı servomotory, u levn´ ych zaˇr´ızen´ı krokov´e motory. Zat´ımco u vˇetˇsiny robotick´ ych manipul´ator˚ u vypl´ yv´a sloˇzitost ˇr´ızen´ı z jejich silnˇe neline´ arn´ı dynamiky z´avisl´e na aktu´aln´ı konfiguraci, u obr´abˇec´ıch stroj˚ u je to ˇrezn´ a s´ıla, kter´ a zpˇetnˇe p˚ usob´ı na jednotliv´e aktu´atory pˇri kontaktu n´astroje s materi´ alem. C´ılem je tedy n´avrh robustn´ıch regul´ator˚ u schopn´ ych preciznˇe sledovat zadan´ y pohyb. Gener´ ator trajektorie lze rozdˇelit na dva menˇs´ı subsyst´emy. CNC j´ adro vyhodnocuje program stroje zadan´ y v normalizovan´em jazyku, tzv. G-k´odu, kter´ y se stal celosvˇetovˇe uzn´ avan´ ym standardem pro programov´an´ı CNC stroj˚ u a respektuj´ı ho vˇsichni pˇredn´ı v´ yrobci tˇechto syst´em˚ u. Program stroje m´a strukturovanou formu posloupnosti pˇr´ıkaz˚ u, kter´e slouˇz´ı k urˇcen´ı pr˚ ubˇehu procesu obr´abˇen´ı. Zaˇc´ın´a obvykle definic´ı souˇradn´eho syst´emu stroje a d˚ uleˇzit´ ych vztaˇzn´ ych bod˚ u, od kter´ ych je v dalˇs´ım pr˚ ubˇehu obr´ abˇen´ı odvozov´ an vlastn´ı pohyb n´astroje (nulov´ y bod obrobku, n´astrojov´eho drˇz´aku, n´astroje atd.). N´ asleduje popis geometrick´ ych drah n´astroje, kter´ y definuje ˇz´adan´ y pr˚ ubˇeh pohybu (pracovn´ı posuv napˇr. line´arn´ı, kruhovou nebo spline interpolac´ı nebo rychloposuv pˇri naj´ıˇzdˇen´ı do nov´e polohy bez kontaktu s obrobkem). Souˇc´ast´ı programu jsou tak´e servisn´ı pˇr´ıkazy napˇr´ıklad pro v´ ymˇenu n´astroje, zapnut´ı chlazen´ı apod. Pˇr´ıkazy interpretovan´e v j´ adru syst´emu jsou pˇred´av´any modulu interpol´ atoru, jehoˇz u ´kolem je generovat ˇcasov´ y pr˚ ubˇeh trajektorie pohybu n´astroje pro regul´ator. Z´akladn´ı u ´lohou interpol´ atoru je dodrˇzet plynulost pohybu pˇri pojezdu n´astroje po zadan´e dr´aze (v literatuˇre oznaˇcov´ ano jako feedrate control - viz [21, 22, 37]. Jak´ekoliv v´ yrazn´e fluktuace nebo nespojitosti v pr˚ ubˇehu rychlosti, zrychlen´ı nebo derivaci zrychlen´ı (jerku) mohou zp˚ usobit viditelnou ztr´atu kvality obr´abˇen´e souˇc´asti. Dalˇs´ım probl´emem nespojit´eho pohybu je moˇznost saturace aktu´ator˚ u a vybuzen´ı kmitav´ ych struktur´aln´ıch
31
ˇ´ ´ ´ 2. R IZEN´ I POHYBU V MECHATRONICKYCH SYSTEMECH
m´od˚ u konstrukce stroje. Tento probl´em nast´av´a zejm´ena u sloˇzitˇejˇs´ıch prostorov´ ych kˇrivek vyˇsˇs´ıch ˇr´ ad˚ u, kter´e nejsou pˇrirozenˇe parametrizov´any vzhledem k d´elce vytyˇcen´eho oblouku (parametr kˇrivky nez´ avis´ı line´arnˇe na d´elce ujet´e dr´ahy v prostoru). Dalˇs´ı d˚ uleˇzitou u ´lohou je ˇcasov´ a optimalizace pr˚ ujezdu zadan´ ymi drahami (v literatuˇre pojem feedrate optimization). Form´ alnˇe se tato u ´loha neliˇs´ı od ˇcasovˇe optim´aln´ıho ˇr´ızen´ı robotick´ ych manipul´ ator˚ u, c´ılem je opˇet v minim´aln´ım ˇcase vykonat zadan´ y pohyb pˇri respektov´an´ı vˇsech kinematick´ ych a dynamick´ ych omezen´ı dan´ ych konstrukc´ı stroje. Vzhledem ke komplexnosti a nelinearitˇe u ´lohy neexistuje obecn´e analytick´e ˇreˇsen´ı a je nutn´e hledat suboptim´ aln´ı ˇreˇsen´ı, ˇcasto s pomoc´ı numerick´ ych metod nebo nejr˚ uznˇejˇs´ıch heuristik. Interpol´ ator d´ ale ˇreˇs´ı kinematick´e transformace mezi zobecnˇen´ ymi souˇradnicemi n´astroje a kloubov´ ymi souˇradnicemi aktu´ator˚ u, oproti robotick´ ym manipul´ator˚ um pˇrib´ yv´a nutnost poˇc´ıtat dr´ ahov´e korekce n´astroje (skuteˇcn´ y pohyb koncov´eho bodu se liˇs´ı od trajektorie napl´ anovan´e v CNC programu o offset zp˚ usoben´ y aktu´aln´ım n´astrojem v n´astavci). Vrstva monitorov´ an´ı a diagnostiky opˇet zajiˇst’uje bezpeˇcnostn´ı funkce, nav´ıc vykon´av´a u ´lohy logick´eho ˇr´ızen´ı, komunikace s obsluhou atd.
Shrnut´ı Syst´emy ˇr´ızen´ı pohybu se objevuj´ı v cel´e ˇradˇe technick´ ych aplikac´ı mechatroniky. Kl´ıˇcovou roli zde hraje n´ avrh ˇr´ıdic´ıho syst´emu a v nˇem implementovan´ ych algoritm˚ u, kter´e ˇ z´asadn´ım zp˚ usobem ovlivˇ nuj´ı funkˇcnost zaˇr´ızen´ı jako celku. R´ıdic´ı syst´em je ˇcasto dˇelen do dvou u ´rovn´ı. Gener´ ator trajektorie pl´anuje pohyb v prostoru s c´ılem minimalizovat vhodn´e krit´erium kvality (napˇr. celkov´ y ˇcas pohybu) pˇri splnˇen´ı vˇsech kladen´ ych omezen´ı. Regul´ator pohybu zajiˇst’uje spr´ avn´e sledov´an´ı napl´anovan´e trajektorie pˇri p˚ usoben´ı vnˇejˇs´ıch poruch a neurˇcitostech v modelu syst´emu.
32
3
Generov´ an´ı trajektorie pohybu V pˇredchoz´ı kapitole byla pops´ana obecn´a struktura syst´emu ˇr´ızen´ı pohybu, u ´loha jeho ˇ ıdic´ı syst´em byl podle vykojednotliv´ ych ˇc´ ast´ı a vz´ ajemn´e vazby d´ılˇc´ıch subsyst´em˚ u. R´ n´avan´ ych funkc´ı rozˇclenˇen do tˇr´ı z´akladn´ıch u ´rovn´ı. Tato kapitola se detailnˇe vˇenuje vrstvˇe gener´ atoru trajektorie, uvedeny jsou metody n´avrhu pro typick´e u ´lohy vyskytuj´ıc´ı se v praxi.
3.1
ˇ ızen´ı pohybu v jedn´ R´ e ose
Jednoos´e ˇr´ızen´ı pohybu se uplatˇ nuje u jednoduˇsˇs´ıch zaˇr´ızen´ı (dopravn´ıky, zakladaˇce, balic´ı stroje, jednoduch´e manipul´atory a obr´abˇec´ı stroje) pˇr´ıpadnˇe u pomocn´ ych pohon˚ u sloˇzitˇejˇs´ıch stroj˚ u (vˇretena obr´abˇec´ıch stroj˚ u, ventil´atory, ˇcerpadla, odstˇredivky...). Vyuˇzit´ı najde tak´e u v´ıceos´ ych syst´em˚ u pro pohyby z bodu do bodu, kdy nez´aleˇz´ı na tvaru trajektorie efektoru/n´ astroje a jednotliv´e osy je moˇzn´e ˇr´ıdit nez´avisle na sobˇe (napˇr. rychloposuvy u obr´ abˇec´ıch stroj˚ u). Podle vykon´avan´eho pohybu lze tyto u ´lohy rozdˇelit do dvou z´ akladn´ıch skupin: • Diskr´ etn´ı pohyb z bodu do bodu - V tomto pˇr´ıpadˇe je ˇr´ızena poloha jedn´e osy stroje z poˇc´ ateˇcn´ıho bodu do koncov´eho. Pˇri prov´adˇen´ı pohybu nen´ı d˚ uleˇzit´ y vlastn´ı pr˚ ubˇeh trajektorie, ale pouze dosaˇzen´ı koncov´eho bodu v prostoru. Pˇrirozen´ ym praktick´ ym poˇzadavkem je pak proveden´ı pohybu v co nejkratˇs´ım ˇcase pˇri respektov´ an´ı fyzik´ aln´ıch omezen´ı dan´ ych konstrukc´ı stroje, konkr´etnˇe omezen´ı na maxim´ aln´ı rychlost (d´ano pouˇzit´ ymi aktu´atory a pˇrevodovkami) a zrychlen´ı (maxim´ aln´ı moment/s´ıla aktu´ator˚ u a setrvaˇcn´e hmoty pohybuj´ıc´ıch se ˇc´ast´ı). To
33
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
vede na formulaci u ´lohy ˇcasovˇe optim´aln´ıho ˇr´ızen´ı syst´emu s dvˇema integr´atory pˇri omezen´ı na stav a ˇr´ızen´ı. V´ ysledkem je obd´eln´ıkov´ y profil zrychlen´ı, lichobˇeˇzn´ıkov´ y pr˚ ubˇeh rychlosti a profil polohy ve tvaru S-kˇrivky s jedn´ım inflexn´ım bodem (Obr. 3.1). V praxi ovˇsem m˚ uˇze nastat probl´em s vybuzen´ım kmitav´ ych struktur´aln´ıch m´ od˚ u mechanick´e konstrukce stroje vlivem prudk´ ych zmˇen s´ıly nebo momentu aplikovan´eho aktu´ atorem v d˚ usledku nespojitosti zrychlen´ı v generovan´e trajektorii. Pro hladˇs´ı pr˚ ubˇeh pohybu je tedy vhodn´e omezit rovnˇeˇz velikost derivace zrychlen´ı (jerku). Nalezen´ı ˇcasovˇe optim´aln´ıho ˇreˇsen´ı pro tuto u ´lohu je ovˇsem netrivi´ aln´ı probl´em a v praxi se ˇcasto pˇristupuje k suboptim´aln´ım aproximac´ım pomoc´ı vhodn´ ych u ´prav trajektorie pohybu. Jednoduˇsˇs´ı variantou u ´lohy je pak ˇr´ızen´ı ot´ aˇcek, kdy gener´ ator zajiˇst’uje plynul´e naj´ıˇzdˇen´ı na poˇzadovanou hodnotu po ramp´ ach nebo S-kˇrivk´ach s definovanou strmost´ı. • Spojit´ y pohyb po zadan´ e trajektorii - V urˇcit´ ych pˇr´ıpadech m˚ uˇze vzniknout poˇzadavek na proveden´ı pohybu podle pˇresnˇe definovan´e trajektorie v ˇcase. Podle aplikace je uˇzivatelem zad´ ana mnoˇzina koincidenˇcn´ıch bod˚ u, tedy poˇzado´ van´e hodnoty polohy, rychlosti nebo zrychlen´ı v ˇcase. Ukolem ˇr´ıdic´ıho syst´emu je tyto body proloˇzit vhodnou interpolaˇcn´ı funkc´ı umoˇzn ˇuj´ıc´ı generov´an´ı trajektorie v re´ aln´em ˇcase. Nejjednoduˇsˇs´ım ˇreˇsen´ım je pouˇzit´ı line´arn´ı nebo polynomi´aln´ı interpolace. Poˇzadavek hladk´eho pr˚ ubˇehu polohy, rychlosti, zrychlen´ı a tak´e derivace zrychlen´ı vede na nutnost pouˇzit´ı polynom˚ u vyˇsˇs´ıch ˇr´ad˚ u, kter´e vˇsak pˇri vˇetˇs´ım poˇctu bod˚ u koincidence vykazuj´ı nepˇrirozen´e zvlnˇen´ı s velk´ ym mnoˇzst´ım ˇ sen´ım je pouˇzit´ı po ˇc´ inflex´ı. Reˇ astech polynomi´aln´ı interpolace spline funkcemi. V tomto pˇr´ıpadˇe je v´ ysledn´ a trajektorie tvoˇrena nˇekolika segmenty, z nichˇz kaˇzd´ y lze popsat polynomem urˇcit´eho stupnˇe, nav´ıc je zaruˇcena spojitost derivac´ı v navazovac´ıch bodech mezi segmenty. Dalˇs´ı moˇznost´ı je vyuˇzit´ı aproximaˇcn´ıch spline kˇrivek [34, 35, 37], kter´e sice vedou na chybu interpolace v zadan´ ych bodech, umoˇzn ˇuj´ı ovˇsem v´ yrazn´ ym zp˚ usobem redukovat poˇcet potˇrebn´ ych parametr˚ u, coˇz m˚ uˇze b´ yt v´ yhodn´e pˇri velk´em mnoˇzstv´ı koincidenˇcn´ıch bod˚ u.
3.1.1
ˇ Casovˇ e optim´ aln´ı pohyb z bodu do bodu
´ Ulohu t-optim´ aln´ıho ˇr´ızen´ı pohybu v jedn´e ose pˇri omezen´ı na maxim´aln´ı rychlost a zrychlen´ı je formulov´ an n´ asleduj´ıc´ım zp˚ usobem. Pohyb mechanick´eho syst´emu lze po-
34
ˇ ızen´ı pohybu v jedn´ 3.1 R´ e ose
psat line´ arn´ım dynamick´ ym modelem druh´eho ˇr´adu ve stavov´e reprezentaci zaveden´ım vektoru stavu obsahuj´ıc´ım postupnˇe polohu a rychlost (x(t) = [s(t) v(t)]T ):
x(t) ˙ = y(t) = x0 = xf
=
s(t) ˙ v(t) ˙
0 1 = 0 0 s(t) 1 0 v(t) s(0) x(0) = v(0) s(tf ) x(tf ) = v(tf )
s(t) v(t)
+
0 1
u(t)
(3.1) (3.2) (3.3) (3.4)
kde ˇr´ızen´ı u(t) je aktu´ aln´ı zrychlen´ı, x0 je vektor poˇc´ateˇcn´ıch podm´ınek, xf poˇzadovan´ y koncov´ y stav syst´emu. C´ılem je nal´ezt takov´e ˇr´ızen´ı, kter´e pˇrevede syst´em z libovoln´ ych poˇc´ateˇcn´ıch podm´ınek x0 do stavu xf v minim´ aln´ım ˇcase tf . Jedn´a se tedy o u ´lohu ˇcasovˇe optim´aln´ıho ˇr´ızen´ı s voln´ ym ˇcasem a pevn´ ym koncem, kter´e minimalizuje funkcion´al Z tf J= 1 dt = tf
(3.5)
t0
pˇri souˇcasn´em respektov´ an´ı omezen´ı na ˇr´ızen´ı a stav syst´emu − vmax < v(t) < vmax
(3.6)
−dmin < u(t) < amax
(3.7)
kde dmin a amax je maxim´ aln´ı pˇr´ıpustn´a decelerace a zrychlen´ı syst´emu, vmax je maxim´aln´ı rychlost. Speci´ aln´ım pˇr´ıpadem u ´lohy je pak pohyb z klidu do klidu, kdy okrajov´e podm´ınky maj´ı tvar x0 = [s0 0]T , xf = [sf 0]T . Pˇrestoˇze pohyb z klidu do klidu je v praxi nejˇcastˇejˇs´ı u ´lohou, gener´ator mus´ı umˇet ˇreˇsit tak´e obecn´ y pˇr´ıpad kv˚ uli implementaci funkc´ı m´ıch´ an´ı pohyb˚ u (blending), kdy od oper´atora nebo z programu stroje pˇrijde poˇzadavek na nov´ y pohyb jeˇstˇe pˇred dokonˇcen´ım toho pˇredchoz´ıho. V tomto pˇr´ıpadˇe je nutn´e pˇrepl´ anovat trajektorii z nenulov´ ych poˇc´ateˇcn´ıch podm´ınek. Podle Pontrjaginova principu minima [59] mus´ı ˇcasovˇe optim´aln´ı ˇr´ızen´ı nab´ yvat zadan´ ych mezn´ıch hodnot. Podle um´ıstˇen´ı okrajov´ ych podm´ınek ve stavov´em prostoru mohou nastat dva pˇr´ıpady:
35
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
1. Optim´ aln´ı trajektorie syst´emu nenaruˇsuje omezen´ı na maxim´aln´ı rychlost, ˇr´ızen´ı u(t) pak nab´ yv´ a na ˇcasov´em intervalu tf pouze dvou mezn´ıch hodnot −dmin a amax . 2. Trajektorie dosahuje maxim´ aln´ı dovolen´e rychlosti, ˇr´ızen´ı u(t) oproti pˇredchoz´ımu pˇr´ıpadu obsahuje nav´ıc interval nulov´e akcelerace. V obou pˇr´ıpadech je nav´ıc nutn´e podle okrajov´ ych podm´ınek rozliˇsit, zda ˇr´ızen´ı bude zaˇc´ınat f´az´ı zrychlen´ı nebo zpomalen´ı. Nalezen´ı optim´aln´ıho ˇr´ızen´ı potom spoˇc´ıv´a ve v´ ypoˇctu pˇrep´ınac´ıch ˇcas˚ u, ve kter´ ych akcelerace mˇen´ı hodnotu. Tyto ˇcasy lze z´ıskat ˇreˇsen´ım soustavy polynomi´ aln´ıch rovnic [60, 61]. Pˇri modifikaci u ´lohy zaveden´ım omezen´ı na derivaci zrychlen´ı je nutn´e model sysˇ ızen´ı syst´emu u(t) pak odpov´ıd´a jerku, jehoˇz t´emu (3.1) rozˇs´ıˇrit o dalˇs´ı integr´ ator. R´ mezn´ı hodnoty je tˇreba dodefinovat k omezen´ım (3.6) a (3.7). Takto formulovan´a u ´loha je jiˇz velmi komplikovan´ a a z literatury nen´ı zn´amo jej´ı analytick´e ˇreˇsen´ı. V praxi se pˇri implementaci gener´ ator˚ u trajektorie obvykle pˇristupuje k suboptim´aln´ımu ˇreˇsen´ı, kter´e spoˇc´ıv´a ve v´ ypoˇctu jednoduˇsˇs´ı varianty pro syst´em s dvˇema integr´atory a dodateˇcnou u ´pravou v´ ysledn´e trajektorie. Takovou u ´pravou m˚ uˇze b´ yt napˇr´ıklad zaˇrazen´ı dynamick´eho filtru prvn´ıho ˇr´ adu s pˇrenosem F (s) =
1 τ s+1
s vhodnˇe volenou ˇcasovou konstantou
τ , kter´a omezuje velikost jerku a urˇcuje rychlost n´abˇehu na maxim´aln´ı hodnoty zrychlen´ı a zpomalen´ı. To m´ a pˇr´ızniv´ y vliv na hladk´ y pr˚ ubˇeh pohybu, omezuje se nebezpeˇc´ı saturace aktu´ ator˚ u a vybuzen´ı mechanick´ ych vibrac´ı v pracovn´ım mechanismu. Nev´ yhodou je promˇenn´ a hodnota jerku v z´ avislosti na tvaru trajektorie, jeho exponenci´aln´ı pr˚ ubˇeh s poˇc´ ateˇcn´ı ˇspiˇckou a asymptotick´ y pr˚ ubˇeh trajektorie vlivem nekoneˇcn´e impulsn´ı funkce filtru, z ˇcehoˇz vypl´ yv´ a nutnost vyhodnocen´ı konce pohybu. Dalˇs´ı moˇznost´ı je u ´prava pr˚ ubˇehu akcelerace omezen´ım strmosti n´abˇehu v pˇrep´ınac´ıch ˇcasech, pˇr´ıpadnˇe n´ahrada schodovit´eho pr˚ ubˇehu zrychlen´ı vhodnou hladkou funkc´ı (napˇr. harmonickou). Trajektorii je potom obvykle nutn´e pˇr´ısluˇsn´ ym zp˚ usobem pˇrepoˇc´ıtat tak, aby koncov´ y stav odpov´ıdal zadan´e poˇzadovan´e hodnotˇe. Alternativn´ı pˇr´ıstup je prezentov´ an v publikaci [60]. Autoˇri zde pˇredkl´adaj´ı hypot´ezu, ˇze optim´ aln´ı ˇr´ızen´ı pro u ´lohu se tˇremi integr´atory s omezen´ım na stav a vstup nab´ yv´a tvaru bang-null-bang“, kdy se postupnˇe na sedmi intervalech stˇr´ıdaj´ı mezn´ı a ” nulov´e hodnoty odpov´ıdaj´ıc´ı derivaci zrychlen´ı. Tato hypot´eza umoˇzn ˇuje pˇrev´est optimalizaˇcn´ı probl´em na ˇreˇsen´ı soustavy polynomi´aln´ıch rovnic. Podle hodnot okrajov´ ych
36
ˇ ızen´ı pohybu v jedn´ 3.1 R´ e ose
podm´ınek mohou b´ yt opˇet ve v´ ysledn´e trajektorii aktivov´ana tˇri r˚ uzn´a omezen´ı na maxim´ aln´ı rychlost, zrychlen´ı a zpomalen´ı, nav´ıc je tˇreba ˇreˇsit dvˇe varianty ˇr´ızen´ı s poˇc´ateˇcn´ı akcelerac´ı nebo zpomalen´ım. Proto je v´ ysledkem ˇsestn´act soustav rovnic o sedmi nezn´ am´ ych (pˇrep´ınac´ı ˇcasy), kter´e jsou ˇreˇseny pouˇzit´ım metody Gr¨obnerov´ ych b´az´ı [62]. Za v´ ysledek je pak povaˇzov´ ano kladn´e re´aln´e ˇreˇsen´ı s nejmenˇs´ı hodnotou celkov´eho ˇcasu tf . Pro praktickou implementaci je tˇreba algoritmus vhodnˇe upravit pro zajiˇstˇen´ı numerick´e stability a oˇsetˇren´ı mezn´ıch stav˚ u kdy pro zadan´e okrajov´e podm´ınky neexistuje pˇri dan´ ych omezen´ıch ˇz´ adn´e ˇreˇsen´ı u ´lohy. Tyto probl´emy jsou diskutov´any v publikaci [63]. ´ Ulohu lze snadno modifikovat pro ˇr´ızen´ı rychlosti/ot´aˇcek, kdy v modelu (3.1) odpad´ a jeden integr´ ator odpov´ıdaj´ıc´ı stavu polohy. Rozˇs´ıˇren´ y probl´em s omezen´ım na jerk pak form´ alnˇe odpov´ıd´ au ´loze s dvˇema integr´atory (3.1) pˇri pouh´em pˇredefinov´an´ı vektoru stavu (x(t) = [v(t) a(t)]T ). Obr´ azek 3.1 ukazuje v´ ysledky simulaˇcn´ıho experimentu, kter´ y srovn´av´a ˇctyˇri r˚ uzn´e ´ strategie pro pl´ anov´ an´ı trajektorie pˇri ˇr´ızen´ı pohybu v jedn´e ose. Ukolem gener´ atoru bylo napl´ anovat ˇcasovˇe optim´aln´ı trajektorii pohybu z klidu do klidu z poˇc´ ateˇcn´ıch podm´ınek x0 = [0 0 0]T do koncov´eho stavu xf = [1 0 0]T pˇri omezen´ıch vmax = 0.5, amax = dmax = 1, jmax = 5, kde jmax je maximum derivace zrychlen´ı. V kaˇzd´em sloupci je postupnˇe shora zobrazen pr˚ ubˇeh polohy, rychlosti, zrychlen´ı a jerku. Prvn´ı sloupec odpov´ıd´ a algoritmu (3.1) bez omezen´ı na jerk, druh´ y sloupec suboptim´ aln´ımu ˇreˇsen´ı s pouˇzit´ım tvarovac´ıho filtru prvn´ıho ˇr´adu, tˇret´ı gener´atoru s harmonick´ ym pr˚ ubˇehem zrychlen´ı a posledn´ı sloupec optim´aln´ımu ˇreˇsen´ı pro rozˇs´ıˇren´ y model s omezen´ım na jerk podle [60]. Z v´ ysledk˚ u je vidˇet, ˇze prvn´ı strategie dosahuje nejkratˇs´ıho ˇcasu pohybu (tf = 2.5s), vzhledem k neomezen´emu jerku v pˇrep´ınac´ıch bodech (pro schodovit´ y pr˚ ubˇeh zrychlen´ı nekoneˇcn´a hodnota) ovˇsem vznik´a nebezpeˇc´ı vybuzen´ı mechanick´ ych vibrac´ı a saturace aktu´ator˚ u pˇri sledov´an´ı takto napl´anovan´e trajektorie. Ostatn´ı strategie dosahuj´ı m´ırnˇe delˇs´ıch ˇcas˚ u pˇri hladˇs´ım pr˚ ubˇehu pohybu s omezenou derivac´ı zrychlen´ı.
37
Zrychleni[m/s2]
Rychlost[m/s]
Poloha[m]
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
1
1
1
1
0.8
0.8
0.8
0.8
0.6
0.6
0.6
0.6
0.4
0.4
0.4
0.4
0.2
0.2
0.2
0.2
0
0
0
0
1
2
3
0
1
2
3
0
1
2
0
3
0.5
0.5
0.5
0.5
0.4
0.4
0.4
0.4
0.3
0.3
0.3
0.3
0.2
0.2
0.2
0.2
0.1
0.1
0.1
0.1
0
0
0
0
1
2
3
0
1
2
3
0
1
2
0
3
1
1
1
1
0.5
0.5
0.5
0.5
0
0
0
0
−0.5
−0.5
−0.5
−0.5
Jerk[m/s3]
−1
0
1
2
3
−1
0
1
2
−1
3
0
1
2
3
−1
5
5
5
5
0
0
0
0
−5
0
1
2
3
−5
0
1
2
−5
3
0
1
2
3
−5
0
1
2
3
0
1
2
3
0
1
2
3
0
1
2
3
t[s]
ˇ Obr´ azek 3.1: Casovˇ e optim´ aln´ı pohyb v jedn´ e ose - srovn´an´ı ˇctyˇr strategi´ı ˇr´ızen´ı
3.1.2
Spojit´ y pohyb po zadan´ e trajektorii
V tomto pˇr´ıpadˇe je uˇzivatelem zad´ ana mnoˇzina uzlov´ ych (koincidenˇcn´ıch) bod˚ u ve formˇe uspoˇr´ adan´ ych dvojic: {ti , di } ; i = 0..n.
(3.8)
kde prvky ti maj´ı v´ yznam ˇcasov´e znaˇcky, prvky di jsou poˇzadovan´e okamˇzit´e hodnoty polohy, rychlosti nebo zrychlen´ı v pˇr´ısluˇsn´ ych ˇcasov´ ych okamˇzic´ıch. C´ılem je nal´ezt vhodnou interpolaˇcn´ı funkci ϕ (t), kter´a m´a n´asleduj´ıc´ı vlastnosti: • Funkce splˇ nuje interpolaˇcn´ı podm´ınky dan´e vstupn´ımi daty : ϕ (ti ) = di ; i = 0..n
38
(3.9)
ˇ ızen´ı pohybu v jedn´ 3.1 R´ e ose
s, v, a
Position
Pos2 Pos3 Pos4
Pos1
0
Acceleration Time
Velocity
dT1
dT2 deltaTime dT1 dT2 dT3 dT4
dT3
dT4 absPos Pos1 Pos2 Pos3 Pos4
Obr´ azek 3.2: Pohyb po spojit´ e trajektorii - pˇr´ıklad interpolace uˇzivatelsk´ ych dat zadan´ ych tabulkou
• Funkce vyhovuje okrajov´ ym podm´ınk´am ve tvaru d d d2 d2 ϕ (0) = v0 , ϕ (tn ) = v1 , 2 ϕ (0) = a0 , 2 ϕ (tn ) = a1 dt dt dt dt
(3.10)
Tedy prvn´ı dvˇe derivace odpov´ıdaj´ıc´ı funkc´ım pro rychlost a zrychlen´ı mus´ı b´ yt m´ıt na zaˇc´ atku a konci profilu pohybu pˇresnˇe definovanou hodnotu. Tento poˇzadavek vych´ az´ı z praktick´e potˇreby pˇri zad´ av´ an´ı vykon´avan´eho pohybu, napˇr´ıklad jestliˇze uˇzivatel zad´ a poˇzadovan´ y ˇcasov´ y pr˚ ubˇeh polohy, pak na zaˇc´atku a konci trajektorie mus´ı m´ıt interpolaˇcn´ı funkce nulovou rychlost a zrychlen´ı, ˇc´ımˇz se doc´ıl´ı fyzik´alnˇe realizovateln´eho pohybu osy z klidu do klidu. Jestliˇze je zad´av´an profil rychlosti, pak je rozumn´e poˇzadovat nulov´e zrychlen´ı na poˇc´atku a konci trajektorie. Nenulov´a poˇc´ateˇcn´ı rychlost nebo zrychlen´ı m˚ uˇze m´ıt za n´ asledek vznik regulaˇcn´ı odchylky pˇri vykon´av´an´ı pohybu a vybuzen´ı velk´ ych silov´ ych r´az˚ u na mechanickou ˇc´ast ˇr´ızen´eho syst´emu. Pˇri nenu-
39
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
lov´e koncov´e rychlosti nebo zrychlen´ı pak vznik´a probl´em s nutnost´ı uv´est syst´em do klidov´eho stavu po dokonˇcen´ı pl´ anovan´eho pohybu. Z praktick´ ych d˚ uvod˚ u je vhodn´e v´ yˇse uveden´e matematick´e podm´ınky doplnit dalˇs´ımi poˇzadavky •
Rozumn´e“ chov´ an´ı interpolaˇcn´ı kˇrivky mezi zadan´ ymi uzlov´ ymi body. Silnˇe zvl” nˇen´a kˇrivka pˇredepsan´ a polynomem vysok´eho ˇr´adu m˚ uˇze pˇresnˇe proch´azet zadan´ ymi body, avˇsak pro popis poˇzadovan´eho pohybu nen´ı pouˇziteln´a.
• Mal´ y poˇcet parametr˚ u kˇrivky, v ide´aln´ım pˇr´ıpadˇe menˇs´ı neˇz poˇcet vstupn´ıch dat. • Tvar funkce vhodn´ y pro generov´an´ı trajektorie v re´aln´em ˇcase. Tento bod u ´zce souvis´ı s pˇredchoz´ım poˇzadavkem. Kˇrivka mus´ı b´ yt pops´ana jednoduch´ ym pˇredpisem s mal´ ym poˇctem parametr˚ u tak, aby okamˇzit´e hodnoty mohly b´ yt snadno a rychle vyˇc´ısleny v re´ aln´em ˇcase. • Moˇznost dodateˇcnˇe ovlivnit pr˚ ubˇehy derivac´ı funkce ϕ (t), pˇri zachov´an´ı platnosti interpolaˇcn´ıch podm´ınek. To je d˚ uleˇzit´e napˇr´ıklad v pˇr´ıpadech, kdy implicitnˇe vypoˇcten´ a kˇrivka pro dr´ ahu pˇrekraˇcuje maxim´aln´ı dovolen´e hodnoty v rychlosti nebo zrychlen´ı a uˇzivatel m˚ uˇze poˇzadovat modifikaci tvaru kˇrivky v tˇechto probl´emov´ ych ˇc´ astech. Z v´ yˇse uveden´ ych praktick´ ych poˇzadavk˚ u vypl´ yv´a, ˇze nejvhodnˇejˇs´ım zp˚ usobem proloˇzen´ı zadan´ ych dat pro generov´ an´ı trajektori´ı pohybu je pouˇzit´ı po ˇc´astech polynomi´aln´ı interpolaˇcn´ı funkce. Volba dostateˇcnˇe vysok´eho stupnˇe polynomu zajiˇst’uje hladk´ y pr˚ ubˇeh vyˇsˇs´ıch derivac´ı polohy (vˇcetnˇe jerku), coˇz je d˚ uleˇzit´e z hlediska buzen´ı mechanick´ ych vibrac´ı. Pˇr´ıliˇs vysok´ y ˇr´ ad polynomu ovˇsem vede na pˇreparametrizovan´e kˇrivky s nevhodn´ ym tvarem vlivem velk´eho poˇctu inflex´ı a lok´aln´ıch extr´em˚ u. Jako ide´aln´ı se jev´ı polynom p´ at´eho stupnˇe, kter´ y vykazuje hladk´ y pr˚ ubˇeh tˇr´ı derivac´ı (spojit´ y pr˚ ubˇeh ˇctyˇr derivac´ı) a z tˇechto d˚ uvod˚ u je velmi ˇcasto uˇz´ıvan´ y pro popis trajektori´ı pohybu [34, 36, 37]. V dalˇs´ım textu jsou struˇcnˇe pops´any tˇri z´akladn´ı pˇr´ıstupy synt´ezy interpolaˇcn´ıch funkc´ı pro generov´ an´ı pohybu. Detailn´ı odvozen´ı metod vˇcetnˇe implementace je moˇzn´e nal´ezt ve v´ yzkumn´e zpr´ avˇe [64] nebo publikaci [65].
40
ˇ ızen´ı pohybu v jedn´ 3.1 R´ e ose
Interpolace kvintick´ ym polynomem V tomto pˇr´ıstupu je pro mnoˇzinu n + 1 uˇzivatelem zadan´ ych koincidenˇcn´ıch bod˚ u (3.8) vypoˇctena kˇrivka s n vnitˇrn´ımi segmenty, z nichˇz kaˇzd´ y je pops´an polynomem p´at´eho stupnˇe ve formˇe: ϕk (u) = a5k u5 + a4k u4 + a3k u3 + a2k u2 + a1k u + a0k ; k = 1..n, u ∈< 0, 1 >
(3.11)
kde parametr u je zaveden z d˚ uvod˚ u numerick´e stability pˇri hled´an´ı hodnot koeficient˚ u polynom˚ u pro jednotliv´e segmenty. Hodnota u = 0 odpov´ıd´a poˇc´ateˇcn´ımu bodu segmentu kˇrivky, u = 1 potom koncov´emu. ych segmentech, C´ılem je nal´ezt takov´e hodnoty koeficient˚ u aik pro polynomy v jednotliv´ aby v´ ysledn´ a kˇrivka pˇresnˇe interpolovala zadan´a data. Uzlov´e body zadan´e uˇzivatelem definuj´ı okrajov´e body kaˇzd´eho segmentu, coˇz lze zapsat ve zvolen´e nez´avisl´e promˇenn´e u takto: ϕk (0) = dk−1 = dk0 ; ϕk (1) = dk = dk1
(3.12)
Ze vztahu (3.12) dost´ av´ ame 2n podm´ınek. Pro kompletn´ı parametrizaci interpolaˇcn´ı funkce je vˇsak tˇreba urˇcit celkem 6n koeficient˚ u polynom˚ u v jednotliv´ ych segmentech, zb´ yv´ a tedy doplnit 4n vztah˚ u, aby v´ ysledn´a kˇrivka byla d´ana jednoznaˇcnˇe. Tyto dopln ˇuj´ıc´ı podm´ınky lze z´ıskat definov´an´ım pr˚ ubˇehu rychlosti a zrychlen´ı v ˇcasov´ ych okamˇzic´ıch zadan´ ych uzlov´ ymi body. Derivov´an´ım polynomu (3.11) podle ˇcasu dost´av´ame vztah pro funkci rychlosti vk (t) uvnitˇr k-t´eho segmentu kˇrivky (t ∈< tk−1 , tk >): 5a5k u4 + 4a4k u3 + 3a3k u2 + 2a2k u + a1k dϕk (u (t)) vk (t) = = dt tk − tk−1 1 , ϕ0k (u) . ; k = 1..n, u ∈< 0, 1 > fnk
(3.13)
kde fnk je faktor vznikaj´ıc´ı v d˚ usledku normalizace nez´avisl´e promˇenn´e (ˇcasu) na interval < 0, 1 > v promˇenn´e u uvnitˇr kaˇzd´eho segmentu kˇrivky. Podobn´ ym zp˚ usobem lze odvodit vztah pro zrychlen´ı uvnitˇr segmentu ak (t): 20a5k u3 + 12a4k u2 + 6a3k u + 2a2k d2 ϕk (u) du 2 ak (t) = = . du2 dt (tk − tk−1 )2 1 , ϕ00k (u) . 2 ; k = 1..n, u ∈< 0, 1 > fnk
41
(3.14)
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
Ze vztah˚ u (3.13) a (3.14) dostaneme 4 doplˇ nkov´e podm´ınky pro kaˇzd´ y segment na poˇc´ateˇcn´ı a koncovou rychlost a zrychlen´ı:
ϕ0k (0) = v(tk−1 )fnk = vk0
(3.15)
ϕ0k (1) = v(tk )fnk = vk1 2 ϕ00k (0) = a(tk−1 )fnk = ak0 2 ϕ00k (1) = a(tk )fnk = ak1
kde v(tk−1 ), v(tk ) jsou uˇzivatelem zadan´e poˇzadovan´e rychlosti, a(tk−1 ), a(tk ) poˇzadovan´a zrychlen´ı v krajn´ıch bodech segment˚ u kˇrivky. Celkem tedy dost´av´ame 6 rovnic pro 6 koeficient˚ u polynomu 5.ˇr´ adu v kaˇzd´em segmentu kˇrivky, kter´a je jiˇz jednoznaˇcnˇe urˇcena. Dosazen´ım interpolaˇcn´ıch podm´ınek (3.12) a podm´ınek na vyˇsˇs´ı derivace (3.15) do vztah˚ u pro polynomy (3.11),(3.13),(3.14) dost´av´ame pro kaˇzd´ y k − t´ y segment soustavu line´arn´ıch algebraick´ ych rovnic, jejichˇz ˇreˇsen´ım jsou hodnoty koeficient˚ u polynomu popisuj´ıc´ıho kˇrivku v dan´em segmentu. Tento zp˚ usob v´ ypoˇctu interpolaˇcn´ı funkce umoˇzn ˇuje uˇzivateli kromˇe samotn´ ych uzlov´ ych bod˚ u zad´avat tak´e pr˚ ubˇeh dvou derivac´ı, ˇc´ımˇz je moˇzn´e interaktivnˇe tvarovat v´ yslednou trajektorii pohybu. Tato metoda vˇsak nen´ı vhodn´a pˇri velk´em poˇctu zadan´ ych koincidenˇcn´ıch bod˚ u, kdy pˇresn´ y pr˚ ubˇeh dvou derivac´ı m˚ uˇze b´ yt apriori jen velmi tˇeˇzko urˇcen. Pro tato pˇr´ıpady se hod´ı dalˇs´ı dvˇe metody interpolace. Kvintick´ a spline interpolace Podobnˇe jako v pˇredchoz´ım pˇr´ıpadˇe se v´ ysledn´a kˇrivka skl´ad´a z n segment˚ u mezi jednotliv´ ymi koincidenˇcn´ımi body zadan´ ymi uˇzivatelem, kde kaˇzd´ y segment je pops´an polynomem p´ at´eho stupnˇe ve tvaru (3.11). Na rozd´ıl od pˇredchoz´ı metody nezad´av´a uˇzivatel pr˚ ubˇeh vyˇsˇs´ıch derivac´ı kˇrivky v uzlov´ ych bodech, ale koeficienty polynom˚ u jsou urˇceny z tzv. spline podm´ınek na spojitost prvn´ıch ˇctyˇr derivac´ı v navazovac´ıch bodech. Zadan´e uzlov´e body urˇcuj´ı 2n interpolaˇcn´ıch podm´ınek pro pr˚ ubˇeh polohy v kaˇzd´em k-t´em segmentu: ϕk (0) = dk−1 = dk0 , ϕk (1) = dk = dk1 ; k = 1, ..., n
42
(3.16)
ˇ ızen´ı pohybu v jedn´ 3.1 R´ e ose
Z pˇredpisu (3.16) lze pˇr´ımo vypoˇc´ıtat hodnoty koeficient˚ u a0k = dk ; k = 1, ..., n. Poˇzadavek na rovnost prvn´ıch ˇctyˇr derivac´ı polohy ve vnitˇrn´ıch navazovac´ıch bodech kˇrivky d´ av´ a dalˇs´ıch 4(n − 1) podm´ınek. Opˇet je nutn´e n´asoben´ı faktorem fnk u vyˇsˇs´ıch derivac´ı kv˚ uli normalizaci segmentu na interval < 0, 1 > v promˇenn´e u:
1 fnk 1 ϕ00k (1) 2 fnk 1 ϕ000 k (1) 3 fnk 1 ϕIV k 4 (1) fnk ϕ0k (1)
k
1 fnk+1 1 = ϕ00k+1 (0) 2 fnk+1 1 = ϕ000 k+1 (0) 3 fnk+1 1 = ϕIV k+1 (0) 4 fnk+1 = ϕ0k+1 (0)
(3.17)
= 1, ..., n − 1
(3.18)
Celkem dost´ av´ ame 6n−4 rovnic pro 6n nezn´am´ ych koeficient˚ u polynom˚ u 5. ˇr´adu v jednotliv´ ych segmentech. Zb´ yv´ a tedy urˇcit dalˇs´ı 4 okrajov´e podm´ınky v krajn´ıch bodech kˇrivky. Pro interpolaci ˇcasov´eho profilu polohy vypl´ yvaj´ı tyto podm´ınky z poˇzadavku na fyzik´ alnˇe realizovateln´ y pohyb z klidu do klidu, tedy na nulovou rychlost a zrychlen´ı v poˇc´ ateˇcn´ım a koncov´em bodu kˇrivky, coˇz lze zapsat:
ϕ01 (0) = 0, ϕ001 (0) = 0
(3.19)
ϕ0n (1) = 0, ϕ00n (1) = 0 Vztahy (3.16)-(3.19) jiˇz jednoznaˇcnˇe urˇcuj´ı hledanou interpolaˇcn´ı kˇrivku, jej´ıˇz parametry lze z´ıskat ˇreˇsen´ım uveden´e soustavy rovnic. V´ ysledkem v´ ypoˇctu je hladk´a trajektorie pohybu se spojit´ ymi ˇctyˇrmi derivacemi polohy s pˇrirozen´ ym pr˚ ubˇehem a minim´aln´ı kˇrivost´ı. Pˇri vˇetˇs´ım mnoˇzstv´ı uzlov´ ych bod˚ u z˚ ust´ av´ a ovˇsem probl´em s velk´ ym poˇctem parametr˚ u (pro n + 1 zadan´ ych bod˚ u je kˇrivka urˇcena 6n koeficienty polynom˚ u v jednotliv´ ych segmentech). Pro tyto pˇr´ıpady je vhodn´e pouˇz´ıt metody zaloˇzen´e na aproximaˇcn´ıch spline kˇrivk´ach.
43
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
Aproximaˇ cn´ı B-spline funkce Z d˚ uvod˚ u uveden´ ych v pˇredchoz´ı ˇc´ asti je moˇzn´e vyuˇz´ıt dalˇs´ı metodu popisu trajektorie pohybu, tentokr´ at ve formˇe jednorozmˇern´e uniformn´ı kvintick´e aproximaˇcn´ı B-spline kˇrivky, kter´a je plnˇe urˇcena n´ asleduj´ıc´ımi parametry: • Poˇctem segment˚ u kˇrivky N, na kter´e je rovnomˇernˇe rozdˇelen ˇcasov´ y interval < 0, tn > ze zadan´ ych vstupn´ıch dat (3.8). • Vektorem ˇr´ıd´ıc´ıch bod˚ u P , kter´ y obsahuje (N + 5) prvk˚ u. Protoˇze poˇzadujeme jednorozmˇernou funkci, ˇr´ıd´ıc´ı body jsou re´aln´e konstanty. V kaˇzd´em d´ılˇc´ım subintervalu o d´elce
tn N
je potom v´ ysledn´a funkce generov´ana jin´ ym
polynomem 5.ˇr´ adu, pˇriˇcemˇz v hraniˇcn´ıch bodech mezi segmenty je zaruˇcena spojitost aˇz do ˇctvrt´e derivace (derivace jerku). Funkˇcn´ı hodnotu v k − tm segmentu (pro k = 1, 2, .., N ) vymezuj´ıc´ım oblast kˇrivky pro ˇcas < tk−1 = (k − 1) tNn , tk = k tNn > lze vyj´adˇrit n´asleduj´ıc´ım vztahem:
ϕk {t (u)} =
6 X
Ni (u) pi+k−1
(3.20)
tn ,0≤u≤1 N
(3.21)
i=1
t (u) = tk−1 + u.
Vztah (3.20 ) tedy ˇr´ık´ a, ˇze v kaˇzd´em segmentu je funkˇcn´ı hodnota kˇrivky urˇcena line´arn´ı kombinac´ı ˇsesti ˇr´ıd´ıc´ıch bod˚ u pi a zat´ım neurˇcen´ ych b´azov´ ych funkc´ı Ni . Kaˇzd´ y segment je tedy ovlivˇ nov´ an pouze lok´ alnˇe ˇsesti ˇr´ıd´ıc´ımi body a dva sousedn´ı segmenty maj´ı vˇzdy 5 ˇr´ıd´ıc´ıch bod˚ u spoleˇcn´ ych, coˇz spolu s tvarem b´azov´ ych funkc´ı zaruˇcuje poˇzadovanou spojitost mezi ˇc´ astmi kˇrivky. Lze dok´ azat, ˇze pro uniformn´ı parametrizaci jsou b´azov´e funkce stejn´e pro vˇsechny segmenty, pouze posunut´e o interval B-spline je lze zapsat v polynomi´ aln´ım tvaru (obr. 3.3):
44
tn N
[35, 38]. Pro kvintick´ y
ˇ ızen´ı pohybu v jedn´ 3.1 R´ e ose
0.7 N1 N2 N3 N4 N5 N6
0.6
0.5
N(u)
0.4
0.3
0.2
0.1
0
0
0.1
0.2
0.3
0.4
0.5 u
0.6
0.7
0.8
0.9
1
Obr´ azek 3.3: B´azov´e funkce pro kvintick´ y uniformn´ı B-spline
N1 (u) = (1 − u)5 /120 N2 (u) = (5u5 − 20u4 + 20u3 + 20u2 − 50u + 26)/120 N3 (u) = (−10u5 + 30u4 − 60u2 + 66)/120 N4 (u) = (10u5 − 20u4 − 20u3 + 20u2 + 50u + 26)/120 N5 (u) = (−5u5 + 5u4 + 10u3 + 10u2 + 5u + 1)/120 N6 (u) = u5 /120
(3.22)
kde parametr polynom˚ u u prob´ıh´a v kaˇzd´em segmentu v intervalu < 0, 1 >, jak je pops´ ano ve vztahu (3.21). Rovnice (3.20) a (3.22) lze pˇrepsat do maticov´eho tvaru. Pro kaˇzd´ y k − t´ y segment potom plat´ı:
45
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
T u5 −1 5 −10 10 −5 1 pk 5 pk+1 u4 −20 30 −20 5 0 3 u 1 −10 20 0 −20 10 0 . pk+2 ϕk {t (u)} = 2 . . 20 −60 20 10 0 u 120 10 pk+3 u −5 −50 0 50 5 0 pk+4 1 26 66 26 1 0 1 pk+5 = Uk .M.Pk ; u < 0, 1 >
= (3.23)
kde Uk je parametrick´ y vektor, M je b´azov´a matice a Pk je vektor ˇr´ıd´ıc´ıch bod˚ u pˇr´ısluˇsn´ ych dan´emu segmentu. Tvar (3.23) je v´ yhodn´ y pro generov´an´ı trajektorie pohybu v re´aln´em ˇcase, protoˇze vektor vznikl´ y souˇcinem M.Pk lze pˇredpoˇc´ıtat pro vˇsechny segmenty kˇrivky a v kaˇzd´em kroku algoritmu je pron´asobit prvn´ımi pˇeti mocninami parametru u nam´ısto sloˇzitˇejˇs´ıho vyˇc´ıslov´an´ı b´azov´ ych polynom˚ u podle vztah˚ u (3.22). Pˇredpis pro funkce rychlosti, zrychlen´ı a jerku lze potom z´ıskat jednoduˇse derivov´an´ım vztahu (3.23) podle ˇcasu:
sk (t) = ϕk {t (u)} = Uk .M.Pk dUk dϕk {t (u)} N= M.Pk .N vk (t) = du du d2 ϕk {t (u)} 2 d2 Uk ak (t) = N = M.Pk .N 2 du2 du2 d3 ϕk {t (u)} 3 d3 Uk jk (t) = N = M.Pk .N 3 du3 du3
(3.24) (3.25) (3.26) (3.27)
N´asoben´ı mocninami poˇctu segment˚ u kˇrivky N vypl´ yv´a z pˇredpisu (3.21) a vˇety o derivaci sloˇzen´e funkce. Synt´eza aproximaˇcn´ı kˇrivky pak vede na optimalizaˇcn´ı probl´em nalezen´ı hodnot (N +5) ˇr´ıdic´ıch bod˚ u pi kˇrivky ϕ (t) sloˇzen´e z N segment˚ u takov´ ym zp˚ usobem, aby byla n P minimalizov´ ana suma kvadr´ at˚ u rezidu´ı (ϕ (ti ) − di )2 → min. Volba poˇctu segment˚ u i=0
N je narozd´ıl od pˇredchoz´ıch metod nez´avisl´a na poˇctu uˇzivatelem zadan´ ych uzlov´ ych bod˚ u. To umoˇzn ˇuje nal´ezt optim´ aln´ı kompromis mezi sloˇzitost´ı kˇrivky a pˇresnost´ı interpolace. Ze vztahu (3.20) je vidˇet, ˇze pr˚ ubˇeh kˇrivky z´avis´ı line´arnˇe na ˇr´ıd´ıc´ıch bodech, lze ´ tedy pouˇz´ıt metodu line´ arn´ıch nejmenˇs´ıch ˇctverc˚ u. Ulohu je ovˇsem nutn´e upravit pro pˇresn´e splnˇen´ı okrajov´ ych podm´ınek na zaˇc´atku a konci trajektorie (3.10), coˇz lze ˇreˇsit
46
3.2 Synchronizovan´ y pohyb - elektronick´ e vaˇ cky a pˇ revodovky
1
3
0.8
2.5
0.6
Slave poloha
Poloha slave
2
1.5
p (u) 3
0.4
0.2
1
pn(u)
p2(u)
0
p1(u)
0
0.5
0
0.1
0.2
0.3
0.4
0.5 Poloha master
0.6
0.7
0.8
0.9
−0.2
1
0
0.1
0.2
0.3
0.4
0.5 Master poloha
0.6
0.7
0.8
0.9
1
Obr´ azek 3.4: Zdvihov´ a z´ avislost - elektronick´a pˇrevodovka a vaˇcka
metodou Lagrangeov´ ych multiplik´ator˚ u. Tento probl´em je v´ıce rozebr´an v dalˇs´ı sekci vˇenovan´e synchronizovan´emu pohybu a elektronick´ ym vaˇck´am. Hlavn´ı v´ yhoda pouˇzit´ı aproximaˇcn´ı B-spline kˇrivky je v moˇznosti volby poˇctu segment˚ u. D´ıky optimalizaˇcn´ımu algoritmu je pak moˇzn´e napˇr´ıklad pro zadanou maxim´aln´ı nebo stˇredn´ı kvadratickou chybu interpolace v uzlov´ ych bodech naj´ıt kˇrivku s minim´aln´ım poˇctem parametr˚ u, coˇz je v´ yhodn´e pˇri velk´em mnoˇzstv´ı zadan´ ych koincidenˇcn´ıch bod˚ u. Vˇsechny uveden´e metody je moˇzn´e modifikovat pro uˇzivatelem zadan´ y poˇzadovan´ y profil rychlosti nebo zrychlen´ı. Princip ˇreˇsen´ı z˚ ust´ av´a totoˇzn´ y a mˇen´ı se pouze ˇreˇsen´e soustavy rovnic.
3.2
Synchronizovan´ y pohyb - elektronick´ e vaˇ cky a pˇ revodovky
Synchronizovan´e ˇr´ızen´ı pohybu je vyˇzadov´ano ve specifick´ ych aplikac´ıch s velk´ ym poˇctem pomocn´ ych pohon˚ u, jejichˇz pohyb je nutn´e sjednotit pro spr´avnou funkci zaˇr´ızen´ı (textiln´ı, tiskaˇrsk´e, znaˇckovac´ı, balic´ı, ˇrezac´ı stroje, nav´ıjeˇcky), pˇr´ıpadnˇe pro prov´ adˇen´ı sloˇzit´eho vz´ ajemnˇe z´ avisl´eho pohybu dvou os (manipul´atory). Dˇr´ıve byl probl´em synchronizace ˇreˇsen mechanicky pomoc´ı centr´aln´ı (kr´alovsk´e) hˇr´ıdele, jej´ıˇz pohyb byl pomoc´ı pˇrevod˚ u a vaˇcek rozv´ adˇen v r´amci stroje. Dnes jsou mechanick´e vaˇcky nahrazoˇ ıdic´ı syst´em trvale udrˇzuje synchronizaci mezi v´any jejich elektronick´ ym protˇejˇskem. R´ nez´ avisle ˇr´ızenou (master) osou, kter´a ˇcasto urˇcuje celkovou rychlost pr´ace stroje, a z´avisl´ ymi pohyby podˇr´ızen´ ych (slave) os. Pohyb podˇr´ızen´ ych os je definov´an tzv. zdvihovou z´ avislost´ı, kter´ a vyjadˇruje vztah mezi polohami master-slave (3.4). Master osa m˚ uˇze b´ yt definov´ ana jako fyzick´ y pohon stroje nebo virtu´aln´ı gener´ator bˇeˇz´ıc´ı uvnitˇr
47
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
ˇr´ıdic´ıho syst´emu. Speci´ aln´ım pˇr´ıpadem vaˇcky je elektronick´a pˇrevodovka, kdy zdvihov´a z´avislost m´a podobu pˇr´ımky, jej´ıˇz smˇernice urˇcuje poˇzadovan´ y pˇrevodov´ y pomˇer. V´ yhodou elektronick´ ych vaˇcek je snadn´a zmˇena programu stroje, ladˇen´ı parametr˚ u za bˇehu, spolehliv´e a pˇresn´e ˇr´ızen´ı pohybu. D´ale je odstranˇena nev´ yhoda mechanick´ ych vaˇcek, kter´e mˇen´ı svoji zdvihovou z´ avislost vlivem opotˇreben´ı materi´alu.
3.2.1
Synt´ eza profilu elektronick´ e vaˇ cky
Form´alnˇe se u ´loha synt´ezy vaˇckov´eho profilu neliˇs´ı od generov´an´ı ˇcasov´eho profilu pro pohyb v jedn´e ose zm´ınˇen´eho v pˇredchoz´ı sekci. Uˇzivatel opˇet zad´av´a mnoˇzinu uzlov´ ych bod˚ u ve formˇe uspoˇr´ adan´ ych dvojic (3.8), kter´e maj´ı ovˇsem v tomto pˇr´ıpadˇe v´yznam zdvihov´e z´ avislosti, tedy vztahu mezi polohou/natoˇcen´ım master a slave osy. Opˇet je hled´ana vhodn´ a interpolaˇcn´ı kˇrivka odpov´ıdaj´ıc´ı vstupn´ım dat˚ um, na kterou jsou kladeny stejn´e poˇzadavky z hlediska hladkosti trajektorie. Odliˇsn´ y je pouze poˇzadavek na okrajov´e podm´ınky profilu, kter´ y mus´ı b´ yt uzavˇren´y. Hodnoty vyˇsˇs´ıch derivac´ı interpolaˇcn´ı kˇrivky mus´ı b´ yt shodn´e na zaˇc´atku a konci profilu pro moˇznost periodick´eho vykon´av´an´ı pohybu (master osa se pohybuje konstantn´ı rychlost´ı urˇcuj´ıc´ı rychlost stroje a podˇr´ızen´e slave osy vykon´ avaj´ı periodick´ y pohyb podle zadan´e z´avislosti), coˇz lze pro prvn´ı dvˇe derivace zapsat:
d d d2 d2 ϕ (0) = ϕ (tn ) = v0 , 2 ϕ (0) = 2 ϕ (tn ) = a0 dt dt dt dt
(3.28)
Samotn´ y v´ ypoˇcet interpolaˇcn´ı kˇrivky potom prob´ıh´a stejn´ ym zp˚ usobem jako u ˇcasov´eho profilu pro jednu osu a lze uˇz´ıt vˇsechny metody zm´ınˇen´e v pˇredchoz´ı sekci. Oˇ setˇ ren´ı okrajov´ ych podm´ınek u aproximaˇ cn´ı kˇ rivky Okrajov´e podm´ınky (3.28) lze pro prvn´ı dvˇe zm´ınˇen´e metody interpolace (kvintick´ y polynom a spline kˇrivka) snadno implementovat u ´pravou algoritmu popsan´eho v pˇredchoz´ı sekci. Pro aproximaˇcn´ı B-spline kˇrivku je vˇsak nutn´e modifikovat postup v´ ypoˇctu. Jednoduch´e ˇreˇsen´ı optimalizaˇcn´ıho probl´emu nalezen´ı polohy ˇr´ıdic´ıch bod˚ u s pouˇzit´ım metody nejmenˇs´ıch ˇctverc˚ u totiˇz obecnˇe vede na chybu interpolace v okrajov´ ych bodech trajektorie. To m˚ uˇze b´ yt neˇz´ adouc´ı, protoˇze rozd´ıln´e hodnoty vyˇsˇs´ıch derivac´ı na
48
3.2 Synchronizovan´ y pohyb - elektronick´ e vaˇ cky a pˇ revodovky
zaˇc´ atku a konci kˇrivky vedou na nespojitosti v periodicky vykon´avan´em pohybu. Algoritmus je nutn´e modifikovat pro zajiˇstˇen´ı pˇresn´eho splnˇen´ı okrajov´ ych podm´ınek, coˇz lze zajistit napˇr´ıklad uˇzit´ım metody Lagrangeov´ ych multiplik´ator˚ u. Ze zadan´ ych uzlov´ ych bod˚ u (3.8) lze sestavit maticovou soustavu rovnic ve tvaru:
Y = Φ.P
(3.29)
kde Y je vektor dat di o dimenzi (n + 1) × 1, Φ je matice regresor˚ u dimenze (n + 1) × (N + 5), kterou lze sestavit z pˇredpisu pro B-spline, P je vektor o d´elce (N + 5) odpov´ıdaj´ıc´ı hledan´ ym ˇr´ıdic´ım bod˚ um kˇrivky. Okrajov´e podm´ınky na kˇrivku lze zapsat maticovˇe: ξ = L.P
(3.30)
kde ξ je vektor poˇzadovan´ ych hodnot polohy, rychlosti a zrychlen´ı (celkem 6 podm´ınek), matice L urˇcuje vliv ˇr´ıdic´ıch bod˚ u P na pr˚ ubˇeh trajektorie v krajn´ıch bodech. C´ılem je tedy ˇreˇsit optimalizaˇcn´ı probl´em nalezen´ı hodnot ˇr´ıdic´ıch bod˚ u kˇrivky, kter´e minimalizuj´ı souˇcet ˇctverc˚ u odchylek v zadan´ ych uzlov´ ych bodech pˇri souˇcasn´em splnˇen´ı okrajov´ ych podm´ınek (3.30), coˇz vede na modifikovan´e krit´erium ve tvaru
J=
1 (Y − ΦP )T (Y − ΦP ) + ΛT (LP − ξ) 2
(3.31)
kde Λ je sloupcov´ y vektor Lagrangeov´ ych multiplik´ator˚ u o d´elce 6 (poˇcet okrajoˇ sen´ı optimalizaˇcn´ıho probl´emu pak spoˇc´ıv´a v nalezen´ı extr´emu v´ ych podm´ınek). Reˇ funkce (3.31), tedy poloˇzen´ım parci´aln´ıch derivac´ı podle vektoru ˇr´ıd´ıc´ıch bod˚ u a vektoru Lagrangeov´ ych multiplik´ ator˚ u rovn´ ych nule. Po u ´pravˇe dostaneme:
∂J ∂P T ∂J ∂ΛT
= ΦT ΦP + ΦT Y + LT Λ = 0
(3.32)
= LP − ξ = 0
(3.33)
Pˇredchoz´ı dvˇe rovnice lze spojit do jedn´e maticov´e soustavy:
49
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
ΦT Φ LT L 0
P Λ
=A
P Λ
=
ΦT Y ξ
(3.34)
ˇ sen´ım soustavy line´ Reˇ arn´ıch rovnic (3.34) je vektor ˇr´ıdic´ıch bod˚ u pro v´ ysledn´ y vaˇc´ kov´ y profil. Uloha m˚ uˇze b´ yt v nˇekter´ ych pˇr´ıpadech ˇspatnˇe podm´ınˇen´a a pro v´ ypoˇcet je nutn´e pouˇz´ıt vhodnou numerickou metodu (napˇr. v´ ypoˇcet s pouˇzit´ım SVD rozkladu a pseudoinverze, viz [64]). Generov´ an´ı trajektorie pohybu podle vaˇ ckov´ eho profilu Interpolaˇcn´ı/aproximaˇcn´ı kˇrivka z´ıskan´a nˇekterou z popsan´ ych metod vyjadˇruje z´avislost os master-slave, kterou lze zapsat: 4
ss (t)=ϕ (sm (t)) = fs (sm (t))
(3.35)
kde ss (t) je ˇcasov´ y pr˚ ubˇeh dr´ ahy pro slave osu, sm (t) je dr´aha ˇr´ıd´ıc´ı master osy. Tento vztah jednoznaˇcnˇe pˇriˇrazuje polohu slave osy v z´avislosti na poloze master osy v ˇcase. Pro udrˇzen´ı synchronizace pohybu je vˇsak nutn´e vypoˇc´ıtat rovnˇeˇz funkˇcn´ı z´avislosti pro rychlost a zrychlen´ı. Pouˇzit´ım vˇety o derivaci sloˇzen´e funkce dostaneme: vs (t) =
d d d d 4 ss (t) = f (sm (t)) = f (sm (t)) sm (t) = fv (sm (t)) vm (t) dt dt dsm dt
(3.36)
kde funkce vs (t) je ˇcasov´ y pr˚ ubˇeh rychlosti slave osy, vm (t) je rychlost master osy a funkce fv (sm (t)) je form´ alnˇe shodn´a s profilem rychlosti z´ıskan´ ym v pˇredchoz´ım pˇr´ıpadˇe pˇri v´ ypoˇctu ˇcasov´e trajektorie. Pro funkci zrychlen´ı plat´ı: 2
as (t) =
d d d2 2 ss (t) = 2 f (sm (t)) vm (t) + f (sm (t)) am (t) 2 dt dsm dsm
4
2 = fa (sm (t)) vm (t) + fv (sm (t)) am (t)
(3.37) (3.38)
kde as (t) je ˇcasov´ y pr˚ ubˇeh zrychlen´ı slave osy, am (t) je zrychlen´ı master osy a funkce fa (sm (t)) odpov´ıd´ a profilu zrychlen´ı. Ze vztah˚ u (3.36) a (3.38) je vidˇet, ˇze pro udrˇzen´ı synchronizace mus´ı b´yt zn´ am´ a okamˇzit´ a rychlost a zrychlen´ı master osy. Pokud je cel´ y syst´em ˇr´ızen centr´alnˇe, pak jsou
50
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
Obr´ azek 3.5: CamEdit - editor vaˇckov´ ych profil˚ u
tyto hodnoty k dispozici a nen´ı probl´em je z´ıskat z blok˚ u gener´atoru pohybu aktu´alnˇe pouˇz´ıvan´eho pro pohyb master osy. Jakmile je ovˇsem pohyb master osy ˇr´ızen zvenˇc´ı a dostupn´ a je napˇr´ıklad pouze jej´ı poloha, je nutn´e hodnoty rychlosti a zrychlen´ı vhodn´ ym zp˚ usobem odhadovat. V praxi b´ yv´a synt´eza vaˇckov´ ych nebo ˇcasov´ ych profil˚ u podpoˇrena vhodn´ ym uˇzivatelsk´ ym n´ astrojem s grafick´ ym rozhran´ım (GUI), kter´e je integrov´ano do v´ yvojov´eho prostˇred´ı k dan´e HW platformˇe ˇr´ıdic´ıho syst´emu. Takov´ y n´astroj umoˇzn ˇuje interaktivn´ı zad´ av´ an´ı, u ´pravy a vyhodnocov´an´ı pr˚ ubˇeh˚ u definovan´ ych trajektori´ı a jejich automatick´ y export do programu stroje. Na obr. 3.5 je uk´azka takov´eho GUI vyvinut´eho v r´ amci v´ yzkumn´eho projektu MPO [64].
3.3
Koordinovan´ y pohyb ve v´ıce os´ ach
Koordinovan´ y pohyb je vyˇzadov´an ve sloˇzitˇejˇs´ıch aplikac´ıch u zaˇr´ızen´ı s vˇetˇs´ım poˇctem os, typicky u robotick´ ych manipul´ator˚ u a CNC obr´abˇec´ıch stroj˚ u, kde vyvst´avaj´ı
51
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
poˇzadavky na komplikovanˇejˇs´ı prov´ adˇen´e u ´kony. Tˇemto dvˇema hlavn´ım oblastem je vˇenov´ano velk´e mnoˇzstv´ı literatury, coˇz vypl´ yv´a z jejich d˚ uleˇzitosti pro technickou praxi [19, 20, 39, 40, 41, 42, 43, 44]. Zaj´ımav´ ym fenom´enem je, ˇze pˇrestoˇze je problematika ˇr´ızen´ı pohybu v obou pˇr´ıpadech velmi podobn´a, dˇel´ı se dostupn´e materi´aly do dvou smˇer˚ u robotiky a CNC ˇr´ızen´ı. Aˇckoliv se ˇcasto ˇreˇs´ı stejn´e probl´emy, je pouˇz´ıv´ana odliˇsn´a terminologie a liˇs´ı se tak´e pˇr´ıstupy k ˇreˇsen´ı. Metody robotiky vych´azej´ı pˇrev´aˇznˇe z matematicky formulovan´ ych probl´em˚ u, kter´e ˇcasto vedou na optimalizace podle nejr˚ uznˇejˇs´ıch krit´eri´ı. Dosaˇzen´ y optim´ aln´ı“ v´ ysledek je ovˇsem nˇekdy v praxi naprosto ” nepouˇziteln´ y (napˇr. v´ ypoˇcetn´ı n´ aroˇcnost algoritmu, kter´ y nelze nasadit v re´aln´em ˇcase, nevhodn´ y pr˚ ubˇeh trajektorie pohybu pro proveden´ı robotem nebo pˇredpoklad u ´pln´e ´ znalosti dynamick´eho modelu syst´emu, kter´ y nikdy nen´ı splnˇen). Ulohy CNC ˇr´ızen´ı jsou naopak motivov´ any technickou prax´ı a vych´az´ı z re´aln´ ych probl´em˚ u vznikaj´ıc´ıch v pr˚ umyslov´ ych aplikac´ıch. Sp´ıˇse neˇz optimalita ˇreˇsen´ı v matematick´em slova smyslu je d˚ uleˇzit´ y dosaˇzen´ y v´ ysledek (kvalita obr´abˇen´eho povrchu, rozumn´ y celkov´ y ˇcas cyklu stroje, jeho opotˇreben´ı...). Nev´ yhodou tohoto stroj´ırensk´eho“ pˇr´ıstupu m˚ uˇze b´ yt pˇr´ı” liˇsn´a konzervativnost nˇekter´ ych ˇreˇsen´ı, kter´e lze s pouˇzit´ım vhodn´eho matematick´eho apar´atu vylepˇsit. Obˇe tyto oblasti se pak liˇs´ı i z pohledu uˇzivatele v´ ysledn´eho zaˇr´ızen´ı. Zat´ımco v CNC ˇr´ızen´ı existuje pomˇernˇe standardizovan´a forma programov´an´ı ve formˇe normalizovan´eho G-k´ odu [39], pak u robotick´ ych manipul´ator˚ u se vyskytuje cel´a ˇrada programovac´ıch jazyk˚ u, kter´e se liˇs´ı podle jednotliv´ ych v´ yrobc˚ u [41]. Ve snaze sjednotit oba tyto svˇety vydala organizace PLCOpen sdruˇzuj´ıc´ı pˇredn´ı v´ yrobce v oblasti automatizaˇcn´ı techniky normu pro syst´emy ˇr´ızen´ı pohybu Motion Control [33]. Jej´ı souˇc´ ast´ı je i sekce vˇenovan´a koordinovan´emu pohybu, kter´a popisuje jednotn´ y zp˚ usob programov´ an´ı a funkˇcn´ı vlastnosti, kter´e by mˇel v´ıceos´ y syst´em splˇ novat. Vlastn´ı implementace tˇechto funkc´ı je pak jiˇz ponech´ana na v´ yrobci konkr´etn´ıho zaˇr´ızen´ı. Tato norma je jiˇz za p´ ar let sv´e existence v praxi respektov´ana a vˇetˇsina velk´ ych firem jiˇz zahrnuje definovan´e funkce do sv´ ych v´ yvojov´ ych prostˇred´ı. Pr´avˇe kv˚ uli ˇsirok´emu uplatnˇen´ı v praxi poslouˇzila tak´e jako pˇredloha pro definici z´akladn´ıch okruh˚ u probl´em˚ u spojen´ ych s ˇr´ızen´ım pohybu, kter´e jsou popsan´e v t´eto pr´aci.
52
2.1 Coordinate System and kinematic transformation
The essence of a trajectory is the coordinated motion of two or more axes from a starting point to a target point via defined path with a specified path velocity. As path one can think of a straight line, a circular movement, or via a splin function. The definition of a path– or any position information - in space requires a coordinate system. Within thi specification three coordinate systems are defined: ACS MCS PCS
Axis related Machine related 3.3 Koordinovan´ y pohyb ve v´ıce os´ ach Product or Workpiece related
Y'
Z'
y´
PCS
Souřadnice produktu/programu
X'
kartézská transformace
Z MCS
X
Y ACS
1
Souřadnice stroje kinematické transformace
Souřadnice aktuátorů
Pohony stroje 3 2
Figure 2: Overview of the coordinate systems and transformations Obr´ azek 3.6: Souˇ radn´ e syst´ emy v´ıceos´ eho stroje - definice podle [33]
ACS: Axes Coordinate System – actual position of the physical axis (after homing). 3.3.1 Souˇ radn´ e syst´ emy a jejich transformace MCS: Machine Coordinate System – Cartesian coordinate system with the origin is a fixed position relative to th Ve v´ıceos´ em syst´ emu Coordinate jsou obvykleSystem” definov´aor ny “Base tˇri r˚ uzn´ e souˇradn´e System”). syst´emy (viz obr. with 3.6). Cartesian buil machine. (Sometimes called “World Coordinate (Note: machines, MCS may be identical to ACS, or mapped via a trivial transformation). The coordinate system from th radn´ y is syst´ em to produktu/programu (PCS) je kart´ezsk´ a soustava a conversion). physical multipleSouˇ axes ACS linked the MCS via a kinematic transformation (forward anddefinovan´ backward vzhledem k prostˇred´ı, se kter´ ym pˇrich´az´ı stroj do kontaktu (napˇr. obr´abˇen´a nebo mon-
PCS: The real work piece can have rotationjeho or shift MCS ecoordinate even might be moving relativ tovan´ a souˇ c´ ast). D˚ uavodem pouˇzto it´ıthe je snadnˇ jˇs´ı zp˚ usob system popisuorpoˇ zadovan´ e trato the MCS coordinate system, and often one wants to describe the trajectory independent from the machine situation jektorie nez´ avisle aktu´ aln´ımua stavu (konfiguraci) stroje a jeho souˇradn´ eho To map these two worldspohybu (MCS to PCS andnavice versa), cartesian or cylindrical transformation is normally done. Th syst´ Ten m˚ uˇzcan e b´ ybe t vzhledem k PCS definov´ an staticky pomoc´ ı operac´ ı translace a coordinate system ofemu. the product called PCS – Product Coordinate System (or “Program Coordinate System” in CNC world). There cannebo be more than one PCS transformation applicable at the same time. In this case the ENUM t rotace dynamicky, jestliˇ ze jsou tyto dvˇe soustavy souˇ radnic v pohybu. specify the coordinate system (CS) has to be extended. A PCS can be a static or a dynamic transformation. Souˇ radn´ y syst´ em stroje (MCS) nˇekdy naz´ yvan´ y z´akladn´ı nebo svˇetov´ y syst´em
In order to specify aradnic point je orsv´ orientation in space a positionˇcalways has(z´ toakladna/suport be related to a manipul´ coordinate system. By mean souˇ az´ an s pevnou nepohyblivou ´ast´ı stroje atoru of transformations this position can be transformed to other coordinate systems. Within this specification, functio nebo V tomto souˇradn´ em syst´ emu je definov´aof n pohyb koncov´eho efektoru nebo blocks are defined forrobotu). these transformations, hiding the complexity these transformations to the programmer in it n´astroje, tedymotion jeho pozice a pˇr´ıpadnˇ e orientace v prostoru robotice prostor day to day use. All multi axes commands are related to only one of the(vcoordinate systemszobecat the same time. nˇen´ych souˇradnic). Vzhledem k PCS je v´az´an obvykle kart´ezskou transformac´ı pˇres
The example below demonstrates how a point P, which is situated on a 2D workpiece (red trapezoid), can be describe TC2 - Task Force Motion Control Part 4 – Coordinated Motion
December 3, 2008 V 1.0 53
© PLCopen – 2007 - 2008 page 13/ 119
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
operace translace a rotace, se soustavou souˇradnic aktu´ator˚ u potom pomoc´ı kinematick´e transformace. V nˇekter´ ych pˇr´ıpadech m˚ uˇze b´ yt souˇradn´ y syst´em stroje ztotoˇznˇen s jedn´ım ze dvou zb´ yvaj´ıc´ıch, pˇr´ıpadnˇe je v´az´an pouze trivi´aln´ı line´arn´ı transformac´ı (napˇr. odliˇsn´e poˇrad´ı os). Souˇ radn´ y syst´ em aktu´ ator˚ u (ACS) odpov´ıd´a poloh´am/natoˇcen´ı jednotliv´ ych pohon˚ u stroje (uzlov´e souˇradnice v robotice), kter´e vz´ajemnˇe koordinovan´ ym pohybem vytv´aˇr´ı poˇzadovanou trajektorii efektoru. Mezi ACS a MCS jsou definov´any kinematick´e transformace (pˇr´ım´ a a inverzn´ı, viz. kapitola 2). Pro jednoduch´e konfigurace stroj˚ u mohou b´ yt tyto transformace trivi´ aln´ı, napˇr´ıklad pro tˇr´ıos´ y kart´ezsk´ y robot ACS a MCS spl´ yvaj´ı. Pro sloˇzitˇejˇs´ı kinematick´e struktury vˇsak ˇcasto vych´azej´ı velmi komplikovan´e vztahy, kter´e nemus´ı b´ yt analyticky ˇreˇsiteln´e, coˇz b´ yv´a ˇcast´ ym probl´emem u inverzn´ı kinematiky pro s´eriov´e manipul´ atory a pˇr´ım´e kinematiky u paraleln´ıch. V takov´ ych pˇr´ıpadech je nutn´e pˇristoupit k numerick´emu v´ ypoˇctu, coˇz pˇrin´aˇs´ı probl´emy s konvergenc´ı, v´ ypoˇcetn´ı n´aroˇcnost´ı atd. Kart´ ezsk´ e transformace P CS ↔ M CS Pˇrechod mezi dvˇema kart´ezsk´ ymi syst´emy souˇradnic je moˇzn´e plnˇe popsat pomoc´ı operac´ı translace a rotace [57]. Uvaˇzujeme-li syst´em souˇradnic CS0 definovan´ y poˇc´atkem a kart´ezsk´ ym syst´emem os O0 − x0 y0 z0 , pak pozici libovoln´eho bodu p1 v jin´em syst´emu souˇradnic CS1 : O1 − x1 y1 z1 lze pˇrev´est pomoc´ı transformace: p0 = o01 + R01 p1
(3.39)
kde o01 jsou souˇradnice poˇc´ atku v´ ychoz´ı soustavy souˇradnic CS1 v soustavˇe CS0 a R01 je rotaˇcn´ı matice definuj´ıc´ı orientaci CS1 v˚ uˇci CS0 . Rotaˇcn´ı matice R je definov´ ana jako posloupnost tˇr´ı element´arn´ıch operac´ı rotace kolem jedn´e osy souˇradn´eho syst´emu:
r11 r12 r13 R = R(α, β, γ) = r21 r22 r23 r31 r32 r33
(3.40)
kde α, β, γ jsou u ´hly natoˇcen´ı kolem zvolen´ ych os. Existuj´ı r˚ uzn´e zp˚ usoby v´ ybˇeru os, podle kter´ ych je postupnˇe prov´ adˇena rotace, v praxi se ust´alil syst´em RP Y (Roll-PitchYaw), kter´ y vych´ az´ı z popisu pohybu uˇz´ıvan´eho v aeronautice a m´a v´ yznam postupn´e
54
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
rotace kolem os z, y, x. V robotice je rovnˇeˇz uˇz´ıv´an syst´em Eulerov´ ych u ´hl˚ u, kter´e jsou definov´ any poˇrad´ım rotac´ı z, y 0 , z 00 , tedy rotac´ı kolem osy z, dalˇs´ı rotac´ı kolem novˇe vznikl´e osy y 0 a tˇret´ı rotac´ı kolem n´aslednˇe vznikl´e osy z 00 . Z vlastnosti ortogonality rotaˇcn´ı matice plyne inverzn´ı transformace k (3.39) mezi souˇradn´ ymi syst´emy: p1 = (R01 )T p0 − (R01 )T o01
(3.41)
Z d˚ uvodu kompaktnˇejˇs´ıho z´ apisu se ˇcasto zav´ad´ı tzv. homogenn´ı souˇradnice ve tvaru ¯ = [p 1]T a k nim pˇr´ısluˇsn´ p a homogenn´ı transformaˇcn´ı matice 0 R1 o01 0 A1 = 0T 1
(3.42)
Pomoc´ı homogenn´ıch souˇradnic a matice (3.42) lze pak transformaci souˇradnic zapsat jednoduˇse vztahem p¯0 = A01 p¯1
(3.43)
−1 p¯1 = (A01 ) p¯0
(3.44)
Kinematick´ e transformace M CS ↔ ACS Kinematick´e transformace slouˇz´ı k pˇrechodu mezi souˇradnicemi aktu´ator˚ u, kter´e lze popsat jako vektor natoˇcen´ı nebo vysunut´ı v jednotliv´ ych kloubech stroje θ1 Θ = ...
(3.45)
θn a vektorem souˇradnic stroje, kter´e definuj´ı polohu p a orientaci φ koncov´eho efektoru/n´ astroje: X=
p φ
(3.46)
´ Uloha pˇr´ım´e kinematiky pak spoˇc´ıv´a v nalezen´ı transformace X = G(Θ), opaˇcn´a u ´loha inverzn´ı kinematiky ˇreˇs´ı pˇrechod Θ = G−1 (X). Kromˇe jednoduch´ ych pˇr´ıpad˚ u b´ yv´a nalezen´ı tˇechto transformac´ı velmi komplikovan´e, kv˚ uli nelinearitˇe vypl´ yvaj´ıc´ı z podstaty ˇ probl´emu. Casto nen´ı moˇzn´e nal´ezt analytick´e ˇreˇsen´ı, coˇz komplikuje pˇr´ıpadnou imˇ sen´ı kinematick´ plementaci tˇechto v´ ypoˇct˚ u do ˇr´ıdic´ıho syst´emu. Reˇ ych transformac´ı je jednou ze z´ akladn´ıch u ´loh robotiky a tomuto probl´emu je vˇenov´ana cel´a ˇrada publikac´ı [19, 20, 41, 42, 57].
55
a circular movement, or a spline function. The path via which the new commanded position is reached is important. For example, this is essential if a workpiece is being processed. Further, the path velocity of the TCP can be controlled directly. Contrary to joint interpolated movements the process of the position of each axis is determined by the desired path and the inverse kinematic transformation. The applicable Function Blocks as specified herein are: • MC_MoveLinearAbsolute • MC_MoveLinearRelative • MC_MoveCircularAbsolute • MC_MoveCircularRelative ´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU • MC_MovePath The figure below illustrates the differences between different types of movement by means of a theoretical machine. y
poloha jedné osy
poloha efektoru v rovině
E
S x
t
Figure 5: Different types of movements Obr´ azek 3.7: Pohyby v´ıceos´ ych syst´ em˚ u - rozdˇelen´ı podle tvaru trajektorie, modˇre MC_MoveDirect (black), MC_MoveLinear and MC_MoveCircular (blue) pˇr´ıkruhov´ a interpolace, zelenˇe line´ arn´ı, ˇcernˇe pohyb(green) PTP. Vlevo pohyb efektoru, vpravo and typical positions of one of the axis of the machine participating in the movement klad odpov´ıdaj´ıc´ıho pohybu jedn´e osy [33]
Uˇzivatel syst´emu obvykle definuje poˇzadovan´e pohyby v souˇradnic´ıch stroje nebo produktu a jednotliv´e transformace jsou pˇred n´ım skryty. Tento pˇr´ıstup umoˇzn ˇuje jednoduˇsˇs´ı programov´ an´ı, rychlejˇs´ı uveden´ı do provozu a pˇrin´aˇs´ı abstrakci vlastn´ıch funkc´ı vykon´avan´ ych zaˇr´ızen´ım od matematick´eho apar´atu, kter´ y je potˇrebn´ y pro jejich reali-
TC2 - Task Force Motion Control Part 4 – zaci. Coordinated Motion
3.3.2
December 3, 2008 V 1.0
© PLCopen – 2007 - 2008 page 16/ 119
Druhy pohybu u v´ıceos´ ych syst´ em˚ u
Podle tvaru trajektorie v prostoru lze prov´adˇen´e pohyby rozdˇelit do ˇctyˇr z´akladn´ıch skupin: Pohyb z bodu do bodu (PTP - Point-to-point) - Podobnˇe jako u diskr´etn´ıho pohybu v jedn´e ose nez´ aleˇz´ı v tomto pˇr´ıpadˇe na tvaru trajektorie pohybu v prostoru, d˚ uleˇzit´e je pouze dosaˇzen´ı spr´ avn´e koncov´e polohy v co nejkratˇs´ım ˇcase (rychloposuvy u obr´abˇec´ıch stroj˚ u, manipul´ atory typu pick-and-place, osazovac´ı stroje, souˇradnicov´a vrtaˇcka apod.). Tento pˇr´ıpad odpov´ıd´ a ˇcern´ ym kˇrivk´am na obr. (3.7). Pl´anov´an´ı pohybu lze ˇreˇsit oddˇelenˇe pro kaˇzd´ y aktu´ ator syst´emu pomoc´ı metod zm´ınˇen´ ych v pˇredchoz´ı sekci. Pokud uˇzivatel poˇzaduje synchronn´ı zastaven´ı ve vˇsech os´ach stroje souˇcasnˇe, je nutn´e pˇrizp˚ usobit vˇsechny pohyby nejpomalejˇs´ı ose (aktu´atoru s nejdelˇs´ım ˇcasem pˇresunu z klidu do klidu pˇri dan´ ych omezen´ıch), coˇz lze snadno realizovat zmˇenou ˇcasov´eho mˇeˇr´ıtka. Pˇri tomto druhu pohybu je tˇreba m´ıt na pamˇeti, ˇze pohyb efektoru v prostoru nen´ı pˇresnˇe definov´ an, hroz´ı tedy kolize s pˇr´ıpadn´ ymi pˇrek´aˇzkami.
56
axes. With single axes the commanded position is always reached. Just the velocity at the time when the commanded position is reached (or passed) can be changed according to the input parameter BufferMode. With interpolated motion control several types of blending can be thought of, depending on the application and process. Therefore new types of blending have to be introduced for interpolated motion control. The input parameter for blending might vary due to the kind of interpolation method applied. So this input is supplier specific. The type of inserted curve that modifies the original path (the ‘contour curve’) is not part of this specification and can be defined by the supplier specific input parameter for blending.
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach Buffered without Blending p2 p3
Aborting p2
p3
Blending p2
p3
p4
Trajectory of TCP p1
p1
p1
Speed of TCP t
t
t
Figure Trajectories process of Velocity in principlenahoˇ of two Obr´ azek 3.8:6:Pohyby typuand MP - pˇr´ıklady m´ıch´an´ ı pohybu, re trajektorie efektoru consecutive motion commands in three modes v prostoru, dole rychlost posuvu v ˇcase
2.4.2
Overview of Buffer Modes
Pohyb po the spojit´ e dr´ aze (CP Continuous V tomto pˇr´ıpadˇ e je pˇ redeFor axes group motions same buffer modes - are used as forpath) single- axis motions (ENUM of type MC_BUFFER_MODE). ps´ana spojit´ a trajektorie v kart´ezsk´ ych souˇradnic´ıch stroje nebo produktu, v pˇr´ıpadˇe sloˇzitˇejˇs´ıch kinematick´ yDescription ch struktur se nav´ıc pˇrid´avaj´ı souˇradnice pro popis orientace MC_BUFFER_MODE Aborting Start FBdanou immediately (default efektoru. Stroj mus´ı sledovat trajektorii v mode) prostoru a nav´ıc ˇr´ıdit rychlost posuvu Buffered Start FB after current motion has finished koncov´eho bodu. Ta m˚ uˇze velocity b´ yt konstantn´ ı (´ ulohy aˇsen´ıoflaku, BlendingLow The is blended with the jako lowestnan´ velocity both brouˇ FBs sen´ı, svaˇroBlendingPrevious velocitymˇ iseblended with velocity ofethe first a FB v´an´ı) pˇr´ıpadnˇe se m˚ uˇzThe e plynule nit (napˇ r. u ´the loha ˇcasovˇ optim´ ln´ıho pˇrejezdu po BlendingNext The velocity is blended with velocity of the second FB dan´e trajektorii pˇri manipulaci s bˇ remenem). Pohyby spojit´ dr´aze nuj´ı naBlendingHigh The velocity is blended with highest po velocity ofeboth FBsse uplatˇ Table 2: Overview of Buffer Modes pˇr´ıklad v u ´loh´ ach s pˇrek´ aˇzkami v prostoru, kde se pohybuje efektor nebo u obr´abˇec´ıch For details refer to Chapter 77 - Details of Blending and Buffering of Movements. No. 0 1 2 3 4 5
stroj˚ u, kde zadan´ a trajektorie pˇr´ımo urˇcuje konturu obr´abˇen´e souˇc´asti. Pohyby po spoTC2 - Task Force © PLCopen 2008ˇreˇsit jit´eMotion dr´ azeControl jsou nejn´ aroˇcnˇejˇs´ı zDecember hlediska 3,n´a2008 vrhu algoritm˚ u ˇr´ızen´ ı, protoˇz–e2007 je tˇr- eba Part 4 – Coordinated Motion V 1.0 page 17/ 119
generov´ an´ı vhodn´ ych interpolaˇcn´ıch funkc´ı (nejˇcastˇeji line´arn´ı nebo kruhov´a interpolace, viz zelen´e a modr´e kˇrivky na obr. (3.7), pˇr´ıpadnˇe sloˇzitejˇs´ı spline funkce), v´ ypoˇcty kinematick´ ych transformac´ı souˇradnic a souˇcasnˇe zajistit plynul´ y pohyb efektoru pˇri omezen´ıch dan´ ych jednotliv´ ymi pohony stroje. Pohyb mezi v´ıce body (MP - Multipoint) - V souˇradnic´ıch stroje nebo produktu je definov´ ano v´ıcero bod˚ u, mezi nimiˇz se pohybuje koncov´ y efektor. Trajektorie mezi jednotliv´ ymi body mohou b´ yt typu PTP nebo CP, vykon´avan´ y pohyb m˚ uˇze b´ yt v jednotliv´ ych bodech zastaven nebo plynule nav´az´an, potom se opˇet mluv´ı o m´ıch´an´ı pohybu (blending), viz pˇr´ıklad pr˚ ujezdu po pˇr´ımce mezi tˇremi body (obr. 3.8 - vlevo pˇreruˇsen´ı pohybu, uprostˇred se zastaven´ım v kaˇzd´em bodˇe, vpravo plynul´ y pˇrechod). Tento zp˚ usob zad´ av´ an´ı pohybu se velmi ˇcasto pouˇz´ıv´a pˇri programov´an´ı robotick´ ych manipul´ ator˚ u. V programu je obvykle definov´ana mnoˇzina bod˚ u a zp˚ usob pr˚ ujezdu
57
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
mezi nimi (napˇr. line´ arn´ı nebo kruhov´a interpolace, se zastaven´ım nebo s plynul´ ym pˇrechodem). Kontaktn´ı pohyby pˇ ri interakci s prostˇ red´ım - Ve specifick´ ych u ´loh´ ach m˚ uˇze vyvstat poˇzadavek na pˇresn´e ˇr´ızen´ı s´ıly, kterou stroj p˚ usob´ı na sv´e okol´ı (mont´aˇzn´ı linky, osazovac´ı stroje, leˇstˇen´ı nebo brouˇsen´ı, ˇr´ızen´ı ˇrezn´e s´ıly pˇri obr´ abˇen´ı). Efektor je potom vˇetˇsinou osazen vhodn´ ym taktiln´ım ˇcidlem, kter´e poskytuje informace o s´ıle v m´ıstˇe kontaktu s prostˇred´ım. Na rozd´ıl od probl´emu sledov´ an´ı trajektorie v prostoru, kdy je vyˇzadov´ ana maxim´ aln´ı tuhost syst´emu (v ide´ aln´ım pˇr´ıpadˇe nekoneˇcn´ a penalizace odchylky v poloze) je pˇri ˇr´ızen´ı s´ıly ide´ aln´ı tuhost nulov´a
ˇ ızen´ı s´ıly Obr´ azek 3.9: R´
(efektor p˚ usob´ı zadanou silou nez´ avisle na jeho poloze). Je ovˇsem nutn´e si uvˇedomit, ˇze v re´aln´em svˇetˇe je vˇzdy s´ıla v m´ıstˇe dotyku doprov´ azen´ a deformac´ı jak ˇcidla (tenzometru) tak vlastn´ıho prostˇred´ı. V´ ysledn´ a zpˇetn´ a vazba pˇriveden´a z taktiln´ıho ˇcidla se tedy ve skuteˇcnosti uzav´ır´a opˇet od polohy efektoru a dynamick´e vlastnosti cel´e soustavy stroj-prostˇred´ı do znaˇcn´e m´ıry z´ avis´ı na mechanick´ ych charakteristik´ach obou ˇc´ast´ı. Pro tyto u ´ˇcely se pouˇz´ıvaj´ı r˚ uzn´e hybridn´ı zp˚ usoby ˇr´ızen´ı, kter´e kombinuj´ı zpˇetnou vazbu od s´ıly, polohy a rychlosti efektoru. Pˇr´ıkladem m˚ uˇze b´ yt tzv. impedanˇcn´ı ˇr´ızen´ı, kter´e se snaˇz´ı vynutit soustavˇe robot-koncov´ y bod-prostˇred´ı pˇredepsanou mechanickou impedanci s definovan´ ym koeficientem tuhosti a tlumen´ı [19, 46, 66, 67].
3.3.3
Metody interpolace pro popis trajektori´ı pohybu
Pro popis pohyb˚ u po spojit´e dr´ aze lze vyuˇz´ıt nejr˚ uznˇejˇs´ı interpolaˇcn´ı funkce. U jednoduˇsˇs´ıch manipul´ ator˚ u jsou to nejˇcastˇeji line´arn´ı a kruhov´e interpolace (pˇr´ıpadnˇe jejich kombinace), kv˚ uli jednoduch´e parametrizaci a menˇs´ı v´ ypoˇcetn´ı n´aroˇcnosti algoritm˚ u v gener´atoru. V n´ aroˇcn´ ych aplikac´ıch s velk´ ymi poˇzadavky na pˇresnost a hladkost pohybu (CNC obr´ abˇen´ı) pak pˇrich´ azej´ı ke slovu sloˇzitˇejˇs´ı interpolace vyˇsˇs´ıch ˇr´ad˚ u, v posledn´ı dobˇe pˇredevˇs´ım spline funkce. Interpolaˇcn´ı funkce b´ yv´a nejˇcastˇeji zad´ana
58
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
v parametrick´em tvaru ve formˇe: X = f (u) ; u ∈ hu0 , u1 i
(3.47)
kde X je vektor souˇradnic v operaˇcn´ım prostoru (souˇradnice stroje nebo produktu), pro definici pozice efektoru bude obsahovat kart´ezsk´e souˇradnice pozice p = [x y z]T , u sloˇzitejˇs´ıch kinematick´ ych konfigurac´ı stroje nav´ıc dalˇs´ı prvky pro urˇcen´ı orientace (viz rotaˇcn´ı matice v sekci vˇenovan´e souˇradn´ ym syst´em˚ um). Funkce f (u) je zad´ana takov´ ym zp˚ usobem, ˇze poˇc´ ateˇcn´ı bod trajektorie odpov´ıd´a parametru u = u0 a jeho postupn´ ym zvˇetˇsov´ an´ım je dosaˇzeno pohybu po dan´e trajektorii aˇz do koncov´eho bodu pro parametr u = u1 . Z hlediska generov´an´ı trajektorie pohybu lze rozdˇelit interpolaˇcn´ı funkce do dvou z´ akladn´ıch skupin. Pˇrirozenˇe parametrizovan´e kˇrivky jsou definov´any takov´ ym zp˚ usobem, ˇze hodnota parametru u z´avis´ı line´arnˇe na d´elce oblouku vytyˇcen´eho na intervalu < u0 , u >, coˇz je v´ yhodn´e kv˚ uli poˇzadavku na plynul´e generov´an´ı pohybu (napˇr. rovnomˇern´ y r˚ ust parametru u odpov´ıd´a pohybu po definovan´e trajektorii s konstantn´ı teˇcnou rychlost´ı). Do t´eto skupiny funkc´ı patˇr´ı line´arn´ı a kruhov´ a interpolace. Sloˇzitˇejˇs´ı kˇrivky b´ yvaj´ı ovˇsem nepˇrirozenˇe parametrizovan´e, tedy rychlost pohybu po dan´e trajektorii je v´az´ana na rychlost zmˇeny parametru u neline´arnˇe. Tento probl´em nast´ av´ a napˇr´ıklad u spline interpolac´ı, pro kter´e je nutn´e hledat vhodn´e transformaˇcn´ı funkce pˇrev´ adˇej´ıc´ı poˇzadovanou polohu (a pˇr´ıpadnˇe dalˇs´ı vyˇsˇs´ı derivace) na spr´ avnou odpov´ıdaj´ıc´ı hodnotu parametru kˇrivky u. Line´ arn´ı interpolace Nejjednoduˇsˇs´ım zp˚ usobem definice trajektorie je pohyb po pˇr´ımce v rovinˇe nebo prostoru. Uvaˇzujeme-li poˇc´ ateˇcn´ı pozici p0 = [x0 y0 z0 ]T a koncov´ y bod p1 = [x1 y1 z1 ]T , potom lze pohyb po pˇr´ımce zapsat v parametrick´em tvaru: p(u) = p0 + au; u ∈< 0, 1 >
(3.48)
kde a = [(x1 − x0 ) (y1 − y0 ) (z1 − z0 )]T je vektor s orientac´ı p0 → p1 a d´elkou p odpov´ıdaj´ıc´ı vzd´ alenosti mezi krajn´ımi body l = (x1 − x0 )2 + (y1 − y0 )2 + (z1 − z0 )2 , u je promˇenn´ y parametr, kter´ y pro u = 0 odpov´ıd´a poˇc´ateˇcn´ımu a u = 1 koncov´emu bodu. Pohyb je pˇrirozenˇe parametrizovan´ y, tzn. dr´aha ujet´a mezi poˇc´ateˇcn´ım a koncov´ ym bodem je line´ arnˇe z´ avisl´ a na parametru pˇr´ımky. Pro generov´an´ı pohybu v re´aln´em ˇcase
59
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
pak zb´ yv´a naj´ıt vhodn´ y algoritmus nebo funkci ft popisuj´ıc´ı pr˚ ubˇeh parametru u a t´ım i polohu na zvolen´e pˇr´ımce v ˇcase, tedy urˇcit z´avislost u(t) = ft (t)
(3.49)
K tomu lze vyuˇz´ıt algoritmy pro ˇr´ızen´ı pohybu v jedn´e ose popsan´e v pˇredchoz´ı ˇc´asti pˇri form´aln´ım pˇredefinov´ an´ı promˇenn´e pro polohu s(t) = u(t) a zaveden´ım okrajov´ ych podm´ınek s(t0 ) = 0, s(tf ) = 1. Pro okamˇzitou rychlost a zrychlen´ı v ˇcase potom plat´ı: dp (u (t)) dt d2 p (u (t)) dt2
du = av (t) dt d2 u = a(t) = a 2 = aa (t) dt = v(t) = a
(3.50) (3.51)
kde v(t) a a(t) jsou okamˇzit´e hodnoty rychlosti a zrychlen´ı v algoritmu pro pohyb v jedn´e ose, kter´e souvis´ı se skuteˇcnou rychlost´ı a zrychlen´ım ve smˇeru p0 → p1 podle vztahu vp0 →p1 (t) =k v(t) k= lv(t)
(3.52)
ap0 →p1 (t) =k a(t) k= la(t)
(3.53)
Vztahy (3.52) a (3.53) lze vyuˇz´ıt k nastaven´ı omezen´ı na maxim´aln´ı rychlost, zrychlen´ı a zpomalen´ı (vmax , amax , dmax ) v algoritmu gener´atoru. Kruhov´ a interpolace Dalˇs´ı moˇznost´ı pro popis trajektorie koncov´eho bodu v prostoru je pohyb po kruˇznici. Norma PLCOpen definuje tˇri moˇzn´e zp˚ usoby jej´ıho zad´an´ı: • Tˇ ri body v prostoru - uˇzivatel specifikuje poˇc´ateˇcn´ı a koncov´ y bod trajektorie plus nav´ıc bod, kter´ ym mus´ı stroj projet. T´ım je jednoznaˇcnˇe urˇcena kruˇznice v prostoru. • Dva body a stˇ red - uˇzivatel zad´a stˇred kruˇznice a okrajov´e body trajektorie. Dalˇs´ım voliteln´ ym parametrem je v´ ybˇer jednoho ze dvou pˇr´ıpustn´ ych smˇer˚ u pohybu. • Krajn´ı body a orientace - uˇzivatel zad´a okrajov´e body a nav´ıc vektor jehoˇz orientace je kolm´ a na rovinu poˇzadovan´e kruˇznice a jehoˇz d´elka odpov´ıd´a polomˇeru.
60
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
Obecn´ y pohyb po kruˇznici v prostoru d´av´a pomˇernˇe komplikovan´e vztahy, kter´e lze vˇsak v´ yraznˇe zjednoduˇsit vhodnou volbou souˇradn´eho syst´emu. Uvaˇzujeme-li obecnou kruˇznici v prostoru se zn´ amou polohou stˇredu, polomˇerem a orientac´ı (splnˇeno pro vˇsechny v´ yˇse popsan´e zp˚ usoby zad´an´ı) zadanou v souˇradnic´ıch stroje nebo produktu, pak m˚ uˇzeme definovat nov´ y kart´ezsk´ y syst´em s poˇc´atkem ve stˇredu kruˇznice, osou x orientovanou od stˇredu k poˇca´teˇcn´ımu bodu pohybu, osou z paralelnˇe s osou kruˇznice a orientac´ı podle ˇz´ adan´eho smˇeru pohybu a pravidla prav´e ruky. V tomto souˇradn´em syst´emu lze pohyb po kruˇznici vyj´adˇrit v parametrick´em tvaru: r cos(sf u/r) p(u) = r sin(sf u/r) ; u ∈< 0, 1 > 0
(3.54)
kde r je polomˇer kruˇznice a sf je d´elka oblouku kruˇznice vytyˇcen´a poˇc´ateˇcn´ım a koncov´ ym bodem. Pro generov´ an´ı trajektorie lze opˇet vyuˇz´ıt algoritmy pro jednu osu zadefinov´ an´ım s(t) = u(t). Pro okamˇzitou rychlost pohybu potom plat´ı: − sin(sf u/r) dp(u(t)) du = v(t) = cos(sf u/r) .sf = asf v(t) dt dt 0 k v(t) k = sf v(t)
(3.55) (3.56)
kde a je jednotkov´ y vektor s orientac´ı teˇcny ke kruˇznici v dan´em bodˇe. Teˇcn´e zrychlen´ı at m´ a stejn´ y smˇer a velikost danou vztahem k at (t) k= at (t) = sf
dv(t) = sf a(t) dt
(3.57)
Pohyb je pˇrirozenˇe parametrizovan´ y a dr´aha ujet´a v oblouku je line´arnˇe z´avisl´a na parametru u. Vztahy (3.54-3.57) lze opˇet snadno vyuˇz´ıt k nastaven´ı maxim´aln´ı rychlosti a teˇcn´eho zrychlen´ı/zpomalen´ı v algoritmu gener´atoru pro pohyb v jedn´e ose. Spojen´ı line´ arn´ı a kruhov´e interpolace se ˇcasto vyuˇz´ıv´a pro tzv. h´elickou interpolaci, pˇri kter´e dvˇe osy vykon´ avaj´ı pohyb po kruˇznici a tˇret´ı po pˇr´ımce. Superpozic´ı tˇechto dvou pohyb˚ u vznik´ a prostorov´ a ˇsroubovice, ˇcehoˇz lze vyuˇz´ıt napˇr´ıklad pˇri obr´abˇen´ı z´avit˚ u. NURBS interpolace Line´ arn´ı a kruhov´e interpolace umoˇzn ˇuj´ı vykon´avat pouze z´akladn´ı druhy pohyb˚ u, kter´e jsou pro sloˇzitˇejˇs´ı manipul´ atory nebo obr´abˇec´ı stroje nedostaˇcuj´ıc´ı. Z toho d˚ uvodu se
61
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
u modern´ıch zaˇr´ızen´ı uplatˇ nuj´ı interpolace spline funkcemi, z nichˇz nejobecnˇejˇs´ım druhem jsou neuniformn´ı racion´ aln´ı B-spline kˇrivky (NURBS). Za vznikem teorie NURBS funkc´ı st´ala potˇreba naj´ıt vhodn´ y n´ astroj pro popis sloˇzit´ ych prostorov´ ych kˇrivek a povrch˚ u uˇz´ıvan´ ych pˇri konstruov´ an´ı stroj˚ u, uplatnˇen´ı nalezly tak´e v oblastech poˇc´ıtaˇcov´e grafiky a animace [37, 38, 58]. V dneˇsn´ı dobˇe je jiˇz podpora NURBS mezi pˇredn´ımi v´ yrobci CAD/CAM softwaru pro poˇc´ıtaˇcovou podporu konstrukce a v´ yroby povaˇzov´ana za standard. Logick´ ym navazuj´ıc´ım krokem je pak implementace tˇechto funkc´ı do obr´abˇec´ıch stroj˚ u, kde slouˇz´ı k popisu drah n´astroj˚ u pˇri vlastn´ım procesu obr´abˇen´ı. Tento pˇr´ıstup vede na eliminaci ztr´ aty pˇresnosti oproti dˇr´ıve uˇz´ıvan´e technice n´ahrady vˇsech drah line´arn´ımi nebo kruhov´ ymi segmenty, dalˇs´ı d˚ uleˇzitou v´ yhodou je u ´spora v d´elce programu stroje. Pˇrestoˇze nezbytn´ a teorie je zn´am´a jiˇz zhruba od poloviny minul´eho stolet´ı, doch´ az´ı k pronik´ an´ı NURBS funkc´ı do syst´em˚ u ˇr´ızen´ı pohybu teprve v posledn´ıch letech. D˚ uvodem je vˇetˇs´ı v´ ypoˇcetn´ı n´aroˇcnost algoritm˚ u, kter´a br´anila nasazen´ı v re´aln´em ˇcase v ˇr´ıdic´ıch syst´emech pˇredchoz´ıch generac´ı a nedostatek potˇrebn´ ych metod z oblasti teorie ˇr´ızen´ı pro ˇreˇsen´ı probl´em˚ u spojen´ ych s uv´adˇen´ım tˇechto princip˚ u do praxe. NURBS kˇrivky jsou dvoj´ım zobecnˇen´ım uniformn´ıch B-spline funkc´ı popsan´ ych v pˇredchoz´ıch sekc´ıch. Prvn´ı zobecnˇen´ı spoˇc´ıv´a v moˇznosti neuniformn´ı parametrizace tzv. uzlov´eho vektoru, kter´ y urˇcuje oblast vlivu jednotliv´ ych ˇr´ıdic´ıch bod˚ u na tvar kˇrivky. Druh´e zobecnˇen´ı je v zaveden´ı v´ ahov´ych koeficient˚ u, kter´e pˇrid´avaj´ı dalˇs´ı stupeˇ n volnosti moˇznost´ı plynule ˇr´ıdit m´ıru pˇrimyk´an´ı kˇrivky k jednotliv´ ym ˇr´ıdic´ım bod˚ um bez nutnosti mˇenit jejich polohu. V´ yhody tohoto zobecnˇen´ı jsou v moˇznosti popisu kˇrivek, kter´e nelze vytv´ aˇret standardn´ımi metodami (napˇr. kuˇzeloseˇcky) a v intuitivn´ım tvarov´an´ı kˇrivky z uˇzivatelsk´eho pohledu. Kaˇzd´a NURBS kˇrivka je po ˇc´ astech racion´aln´ı funkce, kter´a je jednoznaˇcnˇe urˇcena: • Stupnˇem kˇrivky p, kter´ y ud´ av´ a ˇra´d b´azov´ ych funkc´ı, kter´e popisuj´ı pr˚ ubˇeh kˇrivky v jednotliv´ ych segmentech. Z d˚ uvodu poˇzadavk˚ u na hladk´ y pr˚ ubˇeh pohybu formulovan´ ych v pˇredchoz´ı sekci se pro generov´an´ı trajektorie pouˇz´ıvaj´ı nejˇcastˇeji kvintick´e polynomy. Se stupnˇem je spojen tak´e ˇr´ ad kˇrivky k = p + 1, kter´ y ud´av´a kolik ˇr´ıdic´ıch bod˚ u se aktivnˇe pod´ıl´ı na generov´an´ı libovoln´eho bodu kˇrivky. • Uzlov´ym vektorem - posloupnost´ı vzestupnˇe ˇrazen´ ych re´aln´ ych ˇc´ısel ve tvaru U = {u0 , u1 , ..., up+n } (kde n + 1 je poˇcet ˇr´ıdic´ıch bod˚ u), kter´a ud´av´a tvar b´azov´ ych
62
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
funkc´ı a t´ım urˇcuje oblast kˇrivky, kterou ovlivˇ nuj´ı jednotliv´e ˇr´ıd´ıc´ı body. Pokud je interval mezi jednotliv´ ymi prvky vektoru konstantn´ı (ui+1 − ui = konst., ∀i), jedn´ a se o uniformn´ı B-spline. Obecnˇe vˇsak m˚ uˇze b´ yt interval mezi prvky promˇenn´ y nebo i nulov´ y (v´ıcen´asobn´ y prvek v uzlov´em vektoru). Neuniformn´ı parametrizace m˚ uˇze b´ yt zvolena napˇr´ıklad na z´akladˇe vzd´alenost´ı jednotliv´ ych ˇr´ıdic´ıch bod˚ u, ˇc´ımˇz se lze pˇribl´ıˇzit k pˇrirozen´e parametrizaci d´elkou oblouku. N´asoben´ı prvk˚ u uzlov´eho vektoru m´a za n´asledek lok´aln´ı sn´ıˇzen´ı spojitosti kˇrivky, ˇcehoˇz se vyuˇz´ıv´ a napˇr´ıklad pˇri vytv´aˇren´ı ostr´ ych roh˚ u jinak hladk´e kˇrivky. Prvn´ı p + 1 prvky b´ yvaj´ı voleny jako nulov´e, d´ıky ˇcemuˇz kˇrivka vych´az´ı pˇr´ımo z prvn´ıho ˇr´ıdic´ıho bodu, podobnˇe posledn´ıch p + 1 ˇclen˚ u je voleno jednotkov´ ych pro pˇresnou shodu konce kˇrivky s posledn´ım ˇr´ıdic´ım bodem. • B´ azov´ymi funkcemi, kter´e z´avis´ı na stupni spline kˇrivky a hodnot´ach prvk˚ u uzlov´eho vektoru a jsou definov´any rekurzivnˇe jako ( 1, ui ≤ u < ui+1 Ni,1 (u) = ; i = 0..n 0 jinak ui+p+1 − u u − ui Ni,p (u) = Ni,p−1 (u) + Ni+1,p−1 (u) ui+p − ui ui+p+1 − ui+1
(3.58)
kde Ni,p je i − t b´ azov´ a funkce stupnˇe p. Jedn´a se v podstatˇe o rekurzivnˇe opakovanou operaci line´ arn´ı interpolace (viz obr. 3.10 pro b´aze do stupnˇe 2). • Vektorem ˇr´ıdic´ıch bod˚ u (ˇr´ıdic´ım polygonem) P = {P0 , P1 , ..., Pn } - ˇr´ıdic´ı body ovlivˇ nuj´ı tvar v´ ysledn´e funkce a jejich dimenze urˇcuje dimenzi kˇrivky. Pro popis trajektorie pohybu v prostoru m˚ uˇze b´ yt trojrozmˇern´a pro urˇcen´ı pozice a ˇsestirozmˇern´ a pro pln´ y popis orientace (tˇri u ´hly rotaˇcn´ı matice). • Vektorem v´ ahov´ych koeficient˚ u W = {w0 , w1 , ..., wn }, kter´e ud´avaj´ı m´ıru vlivu jednotliv´ ych ˇr´ıdic´ıch bod˚ u na tvar kˇrivky. Vlastn´ı funkci lze potom definovat v parametrick´em tvaru pˇredpisem: n P
Φ (u) =
Ni,p (u) Pi wi
i=0 n P
= Ni,p (u) wi
n X i=0
i=0
63
Ri,p (u) Pi ; u ∈ h0..1i
(3.59)
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
Ni,p
Ni,0
p=0
Ni+1,0(u*)
1
Ni,0(u*)
0 0
Ni,p 1
ui
1 ui+1 u* u, parametr krivky Ni+1,1
Ni,1
p=1
Ni,1(u*)
Ni+1,1(u*)
0 0
Ni,p 1
1
p=2 Ni,2 Ni,2(u*)
0 0
u*
ui
ui+p
ui+p+1 1
Obr´ azek 3.10: Konstrukce B-spline b´azov´ ych funkc´ı
Zmˇeny pr˚ ubˇehu funkce lze doc´ılit: 1. Zmˇenou polohy ˇr´ıdic´ıho bodu 2. Zmˇenou v´ ahy ˇr´ıdic´ıho bodu 3. Pouˇzit´ım v´ıcen´ asobn´eho ˇr´ıdic´ıho bodu - vˇetˇs´ı pˇrimyk´an´ı k n´asobn´emu bodu 4. Zmˇenou stupnˇe kˇrivky - vyˇsˇs´ı stupeˇ n znamen´a obecnˇe hladˇs´ı pr˚ ubˇeh a vzdalov´an´ı od vrchol˚ u ˇr´ıdic´ıho polygonu Obr´azek 3.11 ukazuje pˇr´ıklad dvourozmˇern´e NURBS kˇrivky a vliv zmˇeny polohy nebo v´ahy ˇr´ıdic´ıho bodu na jej´ı tvar.
64
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
Chapter 2. Literature Review
8 Změna váhy, wi
Změna polohy řídicího bodu, Pi
P6
P6 P7 w6 =
P5 P4 P1‘
P7 P5 P4
P8
P8
P3
P3
P2
P2
Y
Y P0 X
100 3 1 0.3 0
P1
P0 X
P1
Figure 2-3. Shape modification of a NURBS curve using control points and weights. [5] Obr´ azek 3.11: 2D NURBS kˇ rivka - pˇr´ıklad zmˇeny tvaru [58]
are equal, then the splines are G1 continuous. Segments are G 2 continuous if they share a Pro popis trajektorie pohybu vypl´ yv´a z vlastnost´ı b´azov´ ych funkc´ı definovan´ ych pˇred-
common center of curvature at the boundary. At the least, G 2 continuity is required to pisem (3.58) nˇekolik d˚ uleˇzit´ ych vlastnost´ı: achieve smooth motion in CNC machining. Parametric derivative continuity, denoted as C n • B´ azov´e funkce jsou spline funkcemi, v navazovac´ıch bodech mezi jednotliv´ ymi
continuity where (v n bodech is the order of the derivative, is a special of geometric continuity segmenty kdy parametr je roven prvku uzlov´ehocase vektoru) je tedy zajiˇ swhen the parameterization is with the distance traveled along toolpath, which is tˇena spojitost prvn´ıch p − 1respect derivac´to ı (kromˇ e pˇr´ıpad˚ u s n´asobn´ ymi the prvky uzlov´eho vektoru). also known as arc-length parameterization. In the case of arc-length parameterization for 2 toolpaths, continuity necessary, however when is not paraarc-length • KaˇzC d´ y ˇr´ıdic´ ı bod Pi isovlivˇ nuje tvar funkce pouze lok´athe lnˇe atoolpath to na intervalu
metru u G ∈ 2hucontinuity i , ui+p i parameterized, is sufficient and is much more flexible than parametric
continuity constraints [6] interpolaˇ [7]. • Funkˇ cn´ı hodnota cn´ı funkce leˇz´ı v konvexn´ım obalu p + 1 ˇr´ıdic´ıch bod˚ u pˇr´ısluˇsej´ıc´ıch dan´emu segmentu.
Formulating a NURBS curve involves obtaining a knot vector, weights, and control Pro generov´ an´ı for bod˚ u na kˇrivce libovoln´e hodnoty parametruthe u se v praxi ˇcasto vypoints, whereas power basispropolynomial representations, only unknowns are the uˇz´ıv´ a tzv. Cox-De Boor algoritmus, kter´ y je v´ ypoˇcetnˇ e efektivn´ a numericky ı algebraic coefficients. Cubic and quintic polynomial splines haveı been heavily stabiln´ investigated. variantou vztah˚ u (3.59) a (3.58). Opakovan´ ym dosazen´ım rekurentn´ıch vztah˚ u pro b´ a-
Wang and Yang [8] developed a spline curve fitting algorithm that starts by obtaining a zov´e funkce do pˇredpisu samotn´e kˇrivky a vyuˇzit´ı skuteˇcnosti o lok´aln´ım vlivu ˇr´ıdic´ıch
chordal-length parameterized cubic spline; then based on the cubic spline finds a nearly arcbod˚ u lze postupnˇe pˇrej´ıt na v´ ypoˇcet b´az´ı nult´eho stupnˇe, kter´e lze snadno vyˇc´ıslit [58].
C 2 continuity. Wang uet∈al.hu[9] furthered length parameterized quintic spline (NAPQS) with Pˇredpis pro libovoln´ y bod na kˇrivce ˇr´adu k pro parametr u na intervalu l , ul+1 ) lzetechnique potom zapsat ve tvaru: this to produce approximately arc-length parameterized C 3 quintic interpolatory
splines (AAP C 3 QIS) by utilizing an iteration to optimize the parameterization and adding a third derivative continuity condition. Later, Erkorkmaz and Altintas [10] formulated and 65
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
(k−1)
Φ (u) = Pl
(u); u ∈< ul , ul+1 )
(3.60)
kde j−1 1 − αj wi−1 P (j−1) (u) + αj wij−1 P (j−1) (u) (j) i−1 i wj i i wij Pi (u) = i Pi j−1 wij (u) = 1 − αij wi−1 + αij wij−1 αij
=
u − ui ui+k−j − ui
;j>0 ;j=0
(3.61) (3.62) (3.63)
Vztahy (3.60-3.63) popisuj´ı geometrick´ y zp˚ usob konstrukce B-spline funkce, kdy se vyuˇz´ıv´a vlastnost´ı uzlov´eho vektoru s n´ asobn´ ymi prvky. Lze dok´azat, ˇze pˇri k-n´ asobnˇe opakovan´em vloˇzen´ı parametru u, pro kter´ y je hled´an bod na kˇrivce, do uzlov´eho vektoru a dopoˇcten´ı pˇr´ısluˇsn´eho poˇctu ˇr´ıdic´ıch bod˚ u, kter´e generuj´ı stejnou p˚ uvodn´ı kˇrivku, doch´az´ı ke stavu, kdy vˇsechny b´ azov´e funkce kromˇe jedin´e maj´ı v zadan´em parametru u nulovou hodnotu a kˇrivka tedy mus´ı proch´azet pˇresnˇe posledn´ım vloˇzen´ ym ˇr´ıdic´ım bodem. Tento algoritmus tedy ukazuje vztah mezi analytick´ ym a geometrick´ ym vyj´adˇren´ım B-spline/NURBS kˇrivky. Interpolace uˇ zivatelsk´ ych dat NURBS kˇ rivkou V´ yˇse popsan´ a obecn´ a NURBS kˇrivka je aproximaˇcn´ı funkc´ı, kter´a kromˇe poˇc´ateˇcn´ıho a koncov´eho bodu (pˇri v´ yˇse popsan´em zp˚ usobu volby uzlov´eho vektoru) neproch´az´ı body ˇr´ıdic´ıho polygonu. V praxi je ovˇsem velmi ˇcast´a u ´loha nalezen´ı vhodn´e funkce, kter´a interpoluje nebo aproximuje uˇzivatelem zadan´a data. Tento probl´em byl jiˇz pops´an v pˇredchoz´ıch sekc´ıch vˇenovan´ ych pohybu v jedn´e ose a elektronick´ ym vaˇck´am. V pˇr´ıpadˇe koordinovan´ ych pohyb˚ u ve v´ıce os´ach se u ´loha zobecˇ nuje pro n-rozmˇern´ y prostor souˇradnic a sloˇzitˇejˇs´ı tvar NURBS funkce. Podle zp˚ usobu prokl´ ad´ an´ı datov´ ych bod˚ u lze rozdˇelit metody synt´ezy do dvou skupin. Na interpolaˇcn´ı funkci je kladen poˇzadavek pˇresn´e shody se zadan´ ymi datov´ ymi body. V oblasti ˇr´ızen´ı pohybu je vhodn´a pˇredevˇs´ım pro vytv´aˇren´ı trajektori´ı pohybu robotick´ ych manipul´ ator˚ u, protoˇze umoˇzn ˇuje generovat hladkou prostorovou kˇrivku z relativnˇe mal´eho poˇctu uˇzivatelem zadan´ ych bod˚ u. Ty mohou b´ yt vloˇzeny a vizualizov´any interaktivnˇe ve vhodn´em softwarov´em n´astroji nebo je lze zadat napˇr´ıklad v uˇc´ıc´ım se“ reˇzimu, kdy je manipul´ ator postupnˇe naveden oper´atorem do jednotliv´ ych ”
66
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
poloh, kter´ ymi m´ a bˇehem pohybu proj´ıt. Tyto polohy jsou uloˇzeny jako datov´e body reprezentuj´ıc´ı pozici a orientaci efektoru a slouˇz´ı k v´ ypoˇctu interpolaˇcn´ı funkce. Aproximaˇcn´ı funkce neproch´az´ı pˇresnˇe zadan´ ymi body, ale sleduje jejich pr˚ ubˇeh s urˇcitou chybou. Tento zp˚ usob tvorby trajektorie je vhodn´ y pˇri velk´em poˇctu datov´ ych bod˚ u, kter´e mohou obsahovat chybu mˇeˇren´ı (opakov´an´ı pohybu robotem podle trajektorie zachycen´e kamerou nebo jin´ ym sn´ımaˇcem polohy, uˇzivatelem zadan´ y pohyb z tabletu nebo nˇejak´eho vstupn´ıho zaˇr´ızen´ı). V tomto pˇr´ıpadˇe nen´ı ˇz´adouc´ı sledovat pˇresnˇe vˇsechny body ale zachytit z´akladn´ı tvar cel´e trajektorie. Dalˇs´ım zp˚ usobem dˇelen´ı interpolaˇcn´ıch a aproximaˇcn´ıch metod je na glob´aln´ı a lok´aln´ı algoritmy. Glob´ aln´ı metody hledaj´ı jednu funkci pro celou sadu vstupn´ıch dat, zat´ımco metody lok´ aln´ı dˇel´ı trajektorii na jednotliv´e segmenty s rozd´ıln´ ymi geometrick´ ymi charakteristikami (napˇr. v´ ykres strojn´ı souˇc´asti obsahuj´ıc´ı ˇc´asti kruˇznic a elips spojen´e pˇr´ımkov´ ymi u ´seky s ostr´ ymi rohy, kter´e nelze popsat jednou hladkou kˇrivkou). Pro kaˇzd´ y segment je hled´ ana odliˇsn´a interpolaˇcn´ı nebo aproximaˇcn´ı funkce a nav´ıc jsou vˇetˇsinou kladeny podm´ınky na geometrickou spojitost v navazovac´ıch bodech mezi jednotliv´ ymi ˇc´ astmi (nejˇcastˇeji spoleˇcn´ y navazovac´ı bod, teˇcna, pˇr´ıpadnˇe stˇred kˇrivosti v tomto bodu). Glob´ aln´ı metody jsou vhodn´e pro popis trajektori´ı manipul´ator˚ u typu pick-and-place, roboty pro kompletaci, osazov´an´ı, brouˇsen´ı a dalˇs´ı pomocn´e pr´ace, kde nez´ aleˇz´ı na pˇresn´em tvaru zadan´e trajektorie a pohyby m˚ uˇze zadat program´ator robotu nebo uˇzivatel. Lok´ aln´ı metody se hod´ı pro popis obr´abˇen´ı u CNC stroj˚ u, kde je ˇcasto vyˇzadov´ ano napojov´ an´ı pohyb˚ u s rozd´ıln´ ymi geometrick´ ymi charakteristikami, pˇriˇcemˇz z´aleˇz´ı na pˇresn´em pr˚ ubˇehu dan´e trajektorie. V tomto pˇr´ıpadˇe vykon´av´a aproximaˇcn´ı a interpolaˇcn´ı algoritmy vˇetˇsinou pˇr´ısluˇsn´ y CAD/CAM software pro poˇc´ıtaˇcovou podporu konstrukce a v´ yroby. Probl´emem vˇsech algoritm˚ u synt´ezy NURBS funkc´ı je neexistence jednoznaˇcn´eho ˇreˇsen´ı. Pro mnoˇzinu zadan´ ych vstupn´ıch dat existuje nekoneˇcnˇe mnoho interpolaˇcn´ıch nebo aproximaˇcn´ıch funkc´ı, z nichˇz je nutn´e vybrat prakticky pouˇzitelnou variantu. V literatuˇre existuje velk´e mnoˇzstv´ı glob´aln´ıch i lok´aln´ıch metod, z nichˇz kaˇzd´a m´a sv´ a specifika, v´ yhody a nev´ yhody. Pˇri nevhodn´e kombinaci vstupn´ıch dat a urˇcit´e metody m˚ uˇze doj´ıt k neoˇcek´ avan´ ym v´ ysledk˚ um, kter´e byt’ matematicky spr´avn´e nemus´ı odpov´ıdat pˇredstavˇe uˇzivatele. Pˇrehledn´ y popis z´akladn´ıch metod uˇz´ıvan´ ych v souˇcasnosti je pod´ an v publikaci [58].
67
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
Jako pˇr´ıklad uved’me variantu synt´ezy glob´ aln´ı interpolaˇcn´ı funkce, kter´a m´a praktick´ y v´ yznam pro popis trajektorie robotick´ ych manipul´ator˚ u a lze ji formulovat n´asledovnˇe. Pro uˇzivatelem zadanou mnoˇzinu datov´ ych bod˚ u bl´ıˇze nespecifikovan´e dimenze d (z´avis´ı na kinematice stroje) Q = {Qk }; k = 0, 1, .., n
(3.64)
je tˇreba nal´ezt uzlov´ y vektor U, ˇridic´ı polygon P, a vektor v´ahov´ ych koeficient˚ u W tak, aby byly splnˇeny podm´ınky interpolace Φ(uk ) = {Qk }; ∀k
(3.65)
Z pˇredpisu (3.59) je vidˇet, ˇze obecn´ a NURBS funkce je neline´arn´ı vzhledem k v´ahov´ ym koeficient˚ um vektoru W , coˇz vede na problematick´e ˇreˇsen´ı interpolaˇcn´ı u ´lohy. Vˇetˇsina metod tedy pouˇz´ıv´ a neracion´ aln´ı variantu B-spline funkce, kter´a odpov´ıd´a volbˇe shodn´ ych v´ahov´ ych koeficient˚ u wi = 1; ∀i. Z d˚ uvodu poˇzadavku na hladk´ y pohyb a spojitost vyˇsˇs´ıch derivac´ı generovan´e trajektorie je vhodn´e pouˇz´ıt kvintick´ y spline, tedy zvolit stupeˇ n p = 5. Dalˇs´ı nezn´ amou jsou ˇr´ıdic´ı body kˇrivky, kter´e lze z´ıskat ˇreˇsen´ım d soustav (n + 1) × (n + 1) nyn´ı jiˇz line´ arn´ıch rovnic z podm´ınky (3.65): Qk = Φ(uk ) =
n X
Ni,p (uk ) Pi ; k = 0, ..., n
(3.66)
i=0
kde ˇr´ıdic´ı body pˇredstavuj´ı n + 1 nezn´am´ ych promˇenn´ ych. K pln´emu urˇcen´ı soustavy (3.66) zb´ yv´a zav´est uzlov´ y vektor a hodnoty parametru uk , ve kter´ ych m´a funkce odpov´ıdat zadan´ ym dat˚ um. Nejˇcastˇejˇs´ım zp˚ usobem volby uk je tzv. chord´ alov´ a parametrizace, kter´ a rozdˇeluje pˇr´ıpustn´ y interval parametru u ∈< 0, 1 > na d´ılˇc´ı segmenty na z´akladˇe Euklidovsk´e vzd´ alenosti mezi jednotliv´ ymi datov´ ymi body, ˇc´ımˇz se snaˇz´ı pˇribl´ıˇzit ide´ alu pˇrirozen´e parametrizace d´elkou oblouku. Oznaˇc´ıme-li d jako celkovou d´elku od prvn´ıho k posledn´ımu datov´emu bodu d=
n X
kQk − Qk−1 k2
(3.67)
k=1
pak hodnoty uk lze volit podle pˇredpisu uk = uk−1 +
kQk − Qk−1 k2 ; k = 1, ..., n − 1 d
(3.68)
u0 = 0 un = 1 (3.69)
68
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
Kromˇe parametrizace (3.68) by mˇel pro pˇrirozen´ y tvar interpolaˇcn´ı funkce respektovat rozm´ıstˇen´ı datov´ ych bod˚ u a jejich vz´ajemnou vzd´alenost tak´e uzlov´ y vektor U, kter´ y urˇcuje oblasti vlivu jednotliv´ ych ˇr´ıdic´ıch bod˚ u na pr˚ ubˇeh funkce. K tomu lze pouˇz´ıt algoritmus pr˚ umˇerov´ an´ı popsan´ y v [58], podle kter´eho jsou prvky uzlov´eho vektoru voleny n´ asledovnˇe u0 = u1 = ... = up = 0; um−p = ... = um = 1; uj+p =
1 p
j+p−1 X
ui ; j = 1, ..., n − p
(3.70) (3.71)
i=j
tedy kaˇzd´ a hranice segmentu uzlov´eho vektoru je z´ısk´ana zpr˚ umˇerov´an´ım pˇr´ısluˇsn´ ych p y podle chord´alov´e parametrizace (3.68) odpov´ıd´a rozm´ıstˇen´ı hodnot parametru uk , kter´ datov´ ych bod˚ u. Jak jiˇz bylo ˇreˇceno v´ yˇse, vypoˇcten´a kˇrivka nemus´ı pˇresnˇe odpov´ıdat pˇredstavˇe uˇzivatele o tvaru v´ ysledn´e trajektorie. Zmˇeny lze dos´ahnout nˇekolika zp˚ usoby 1. Ruˇcn´ı u ´prava interpolaˇcn´ı funkce vypoˇcten´e algoritmem (3.66-3.71) zmˇenou polohy ˇr´ıdic´ıch bod˚ u nebo u ´pravou v´ahov´ ych funkc´ı. V tom pˇr´ıpadˇe ovˇsem doch´ az´ı lok´ alnˇe k poruˇsen´ı podm´ınek interpolace datov´ ych bod˚ u. Pro v´ıcerozmˇern´e souˇradnice je manu´ aln´ı u ´prava ˇr´ıdic´ıch bod˚ u obt´ıˇzn´a z hlediska pˇredstavy uˇzivatele o vlivu ˇr´ıdic´ıch bod˚ u a sloˇzitosti vizualizace pr˚ ubˇehu vlastn´ı interpolaˇcn´ı funkce. ´ ech t´eto metody tedy do znaˇcn´e m´ıry z´avis´ı na kvalitˇe a pˇrehlednosti vhodUspˇ n´eho softwarov´eho n´ astroje pro vytv´aˇren´ı a u ´pravu trajektori´ı. 2. Pˇrid´ an´ı dalˇs´ıch datov´ ych bod˚ u pro pˇresnˇejˇs´ı specifikaci ˇz´adan´e trajektorie. Tento pˇr´ıstup umoˇzn ˇuje dodateˇcnˇe tvarovat interpolaˇcn´ı funkci v problematick´ ych ˇc´ astech, kter´e neodpov´ıdaj´ı zam´ yˇslen´emu pr˚ ubˇehu. Pˇrid´an´ım nˇekolika dalˇs´ıch datov´ ych bod˚ u lze vˇetˇsinou u ´spˇeˇsnˇe upravit tvar v´ ysledn´e trajektorie do poˇzadovan´eho pr˚ ubˇehu. 3. Rozˇs´ıˇren´ım popsan´eho algoritmu o specifikaci derivace spline funkce (vektor teˇcen) v datov´ ych bodech, coˇz vede na podobnou soustavu line´arn´ıch rovnic nav´ıc s dodateˇcn´ ymi podm´ınkami na derivace. T´ımto zp˚ usobem lze ovlivnit chov´an´ı funkce v okol´ı vstupn´ıch dat. Zad´av´an´ı vektor˚ u derivac´ı v datov´ ych bodech je ovˇsem problematick´e uˇz v trojrozmˇern´em prostoru, natoˇz v obecn´ ych ˇsesti rozmˇerech ˇ pˇri pln´em popisu pozice a orientace. Casto se vyuˇz´ıv´a pˇredeps´an´ı hodnot derivace
69
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
3
3
2.5
2.5
2
2
1.5
1.5
1
1
0.5
0.5
0
0
−0.5
−0.5
−1 −1
−0.5
0
0.5
1
1.5
2
2.5
−1 −1
3
−0.5
0
0.5
1
1.5
2
2.5
3
−0.5
0
0.5
1
1.5
2
2.5
3
3 3 2.5 2.5 2 2 1.5 1.5 1
1
0.5
0.5
0
0
−0.5
−0.5
−1 −1
−0.5
0
0.5
1
1.5
2
2.5
−1 −1
3
Obr´ azek 3.12: 2D NURBS interpolace - pˇr´ıklady pouˇzit´ı, ˇcervenˇe zadan´e datov´e body, teˇckovanˇe s kˇr´ıˇzky naznaˇcen ˇr´ıdic´ı polygon kˇrivky
funkce pouze v poˇc´ ateˇcn´ım a koncov´em bodu, kde b´ yv´a pro uˇzivatele snadnˇejˇs´ı formulovat zam´ yˇslen´ y tvar trajektorie (poˇc´ateˇcn´ı a koncov´ y smˇer pohybu).
Pˇr´ıklad pouˇzit´ı popsan´e metody ilustruje obr´azek 3.12 pro pˇr´ıpad dvourozmˇern´e NURBS funkce interpoluj´ıc´ı zadan´e datov´e body. V´ ysledn´a kˇrivka m˚ uˇze pˇredstavovat napˇr´ıklad trajektorii pohybu manipul´ atoru v rovinˇe pˇri obj´ıˇzdˇen´ı pˇrek´aˇzek. Prvn´ı funkce vlevo nahoˇre splˇ nuje podm´ınky interpolace uˇzivatelsk´ ych dat, na zaˇc´atku a konci vˇsak obsahuje zvlnˇen´ı, kter´e je neˇz´ adouc´ı. Lze ho odstranit manu´aln´ı u ´pravou polohy ˇr´ıdic´ıch bod˚ u (nahoˇre vpravo), pˇrid´ an´ım dalˇs´ıch datov´ ych bod˚ u pro pˇresnˇejˇs´ı specifikaci trajektorie v problematick´ ych ˇc´ astech (vlevo dole) nebo pouˇzit´ım varianty algoritmu s pˇredepsan´ ymi okrajov´ ymi derivacemi (vpravo dole, vektory derivac´ı oznaˇceny ˇsipkou).
70
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
3.3.4
Omezen´ı fluktuac´ı zp˚ usoben´ ych nepˇ rirozenou parametrizac´ı
V´ yˇse zm´ınˇen´e metody d´ avaj´ı n´avod jak konstruovat parametricky vyj´adˇren´e funkce, kter´e popisuj´ı trajektorii pohybu v prostoru (trojrozmˇern´em pˇri popisu pozice a ˇsestirozmˇern´em vˇcetnˇe orientace). Pro ˇr´ızen´ı pohybu v re´aln´em ˇcase je vˇsak nav´ıc nutn´e urˇcit ˇcasov´ y pr˚ ubˇeh pohybu po zvolen´e trajektorii, tedy v kaˇzd´em okamˇziku vzorkov´an´ı vypoˇc´ıtat aktu´ aln´ı hodnoty polohy, pˇr´ıpadnˇe rychlosti a zrychlen´ı, kter´e jsou pˇred´any modulu regul´ atoru jako referenˇcn´ı veliˇciny pro sledov´an´ı. Pro danou trajektorii v parametrick´em tvaru je tedy tˇreba urˇcit z´avislost X(t) = Φ(u(t)) = Φ(f (t)); t = k.Ts
(3.72)
kde X jsou zobecnˇen´e souˇradnice stroje v operaˇcn´ım prostoru, Φ je obecn´a parametrick´ a funkce popisuj´ıc´ı danou trajektorii pohybu a Ts je perioda vzorkov´an´ı. Algortimy vytv´ aˇrej´ıc´ı ˇcasovou posloupnost funkce f (t) b´ yvaj´ı oznaˇcov´any jako gener´ ator posuvu (z anglick´eho feedrate control ), protoˇze generuj´ı hodnoty parametru u na z´akladˇe uˇzivatelem zadan´ ych pˇr´ıkaz˚ u posuvu po zvolen´e trajektorii. Pˇr´ıkladem m˚ uˇze b´ yt ˇcasovˇe optim´ aln´ı pˇrejezd pˇri manipulac´ıch s materi´alem nebo pr˚ ujezd zadanou konstantn´ı rychlost´ı v teˇcn´em smˇeru pˇri obr´ abˇen´ı. Pro pˇrirozenˇe parametrizovan´e kˇrivky (line´arn´ı, kruhov´a interpolace) plat´ı line´arn´ı z´avislost mezi pˇr´ırustkem parametru u a d´elkou opsan´eho oblouku (dr´ahy) v kaˇzd´em okamˇziku vzorkov´ an´ı. Gener´ ator posuvu lze pro tyto pˇr´ıpady snadno implementovat pomoc´ı algoritm˚ u pro pohyb v jedn´e ose popsan´ ych na zaˇc´atku kapitoly. U sloˇzitejˇs´ıch interpolac´ı vyˇsˇs´ıho ˇr´adu po ˇc´astech polynomi´aln´ımi funkcemi, mezi kter´e patˇr´ı obecn´e NURBS kˇrivky a vˇsechny jejich speci´aln´ı pˇr´ıpady, vˇsak tato line´arn´ı z´avislost neplat´ı. Pˇri konstantn´ım pˇr´ırustku parametru u doch´az´ı pod´el trajektorie pohybu k fluktuac´ım d´elky opsan´eho oblouku a vyˇsˇs´ıch derivac´ı v teˇcn´em smˇeru vlivem mˇen´ıc´ı se geometrie kˇrivky. To m˚ uˇze pˇri generov´an´ı trajektorie zp˚ usobovat nerovnomˇern´ y chod stroje, zv´ yˇsen´e opotˇreben´ı akˇcn´ıch ˇclen˚ u a mechanick´ ych komponent syst´emu a degradaci kvality ˇr´ızen´ı. V pˇr´ıpadˇe obr´abˇec´ıch stroj˚ u se tyto fluktuace viditelnˇe podepisuj´ı na povrchu obr´ abˇen´ ych souˇc´ast´ı, mohou v´est i k poˇskozen´ı n´astroje nebo obrobku. Pˇri implementaci algoritm˚ u interpol´atoru je tedy d˚ uleˇzit´e zachytit funkˇcn´ı z´avislost mezi dr´ ahou ujetou po kˇrivce a hodnotou jej´ıho parametru.
71
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
Metody zaloˇ zen´ e na aproximaci Taylorov´ ym rozvojem Pˇredpokl´adejme obecnou parametrickou funkci popisuj´ıc´ı pozici koncov´eho efektoru/n´astroje v trojrozmˇern´em kart´ezsk´em syst´emu souˇradnic
x(u) Φ(u) = y(u) ; u ∈< 0, 1 > z(u)
(3.73)
Oznaˇc´ıme li jako s(u(t)) funkci popisuj´ıc´ı dr´ahu opsanou obloukem z poˇc´ateˇcn´ıho bodu kˇrivky do aktu´ aln´ıho m´ısta trajektorie pˇr´ısluˇsn´e parametru u(t) a okamˇzitou rychlost pohybu V (t) v dan´em m´ıstˇe, potom plat´ı n´asleduj´ıc´ı vztahy ds du ds = . dt du dt V (t) V (t) V (t) =q = ds k Φ0 (u) k dy 2 dx 2 dz 2 du ( du ) + ( du ) + ( du )
V (t) = ⇒
du dt
=
(3.74) (3.75)
Z pˇredchoz´ıch vztah˚ u tedy plyne z´ avislost mezi parametrick´ ym pˇredpisem kˇrivky, zmˇenou parametru u v ˇcase a rychlost´ı pohybu po kˇrivce V (t). Jestliˇze V (t) bude obecn´a funkce poˇzadovan´e rychlosti, kter´ a je dan´a aktu´aln´ım programem stroje nebo pˇr´ıkazem uˇzivatele/oper´ atora, pak je teoreticky moˇzn´e z´ıskat pr˚ ubˇeh parametru kˇrivky v ˇcase u(t). Vztah (3.75) je ovˇsem pro obecn´e funkce V (t) a Φ(u) analyticky obt´ıˇznˇe ˇreˇsiteln´ y, proto se pˇristupuje k aproximac´ım, nejˇcastˇeji pomoc´ı Taylorova rozvoje, na jehoˇz principu je zaloˇzena cel´ a ˇrada metod pro interpolaci prostorov´ ych kˇrivek v re´aln´em ˇcase [68, 69, 70, 71]. V kaˇzd´em okamˇziku vzorkov´ an´ı tk je moˇzn´e funkci u(tk ) = uk rozloˇzit v ˇradu pomoc´ı Taylorova rozvoje do tvaru uk+1 = uk +
du d2 u T 2 Ts |u=uk + 2 s |u=uk +R(uk ) dt dt 2
(3.76)
kde Ts je perioda vzrorkov´ an´ı a v´ yraz R(uk ) pˇredstavuje zanedban´e ˇcleny vyˇsˇs´ıch ˇr´ad˚ u. Dosazen´ım vztahu (3.75) a jeho druh´e derivace podle ˇcasu do rozvoje (3.76) dost´av´ame pˇredpis pro v´ ypoˇcet parametru kˇrivky na z´akladˇe aktu´aln´ı poˇzadovan´e rychlosti V (tk ) a zrychlen´ı A(tk ) = uk+1
dV (tk ) dt
v teˇcn´em smˇeru
V (tk ) 1 = uk + Ts + k Φ0 (uk ) k k Φ0 (uk ) k
Φ0 (uk )T Φ00 (uk )V (tk )2 Ts2 A(tk ) − (3.77) k Φ0 (uk ) k3 2
72
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
Z pˇredpisu (3.77) je vidˇet, ˇze pˇri pouˇzit´ı Taylorova rozvoje 2.ˇr´adu je potˇreba v kaˇzd´em kroku algoritmu poˇc´ıtat prvn´ı dvˇe derivace interpolaˇcn´ı funkce Φ(u), coˇz je pˇri pouˇzit´ı obecn´ ych NURBS kˇrivek v´ ypoˇcetnˇe velmi n´aroˇcn´e vzhledem k nutnosti ˇreˇsit rekurzivn´ı vztahy pro vyˇc´ıslen´ı b´ azov´ ych funkc´ı a jejich derivac´ı. Proto se nˇekdy pˇristupuje k aproximaci rozvojem prvn´ıho ˇr´adu v oblastech s velk´ ym polomˇerem kˇrivosti, kde jsou vyˇsˇs´ı ˇcleny rozvoje zanedbateln´e. Kromˇe v´ ypoˇcetn´ı n´aroˇcnosti tˇechto metod je dalˇs´ı nev´ yhodou nevyhnuteln´ a akumulace numerick´ ych chyb vlivem rekurzivn´ıho sˇc´ıt´an´ı pˇri zanedb´ an´ı vyˇsˇs´ıch ˇclen˚ u. Metody zaloˇ zen´ e na polynomi´ aln´ı korekˇ cn´ı funkci Alternativn´ım ˇreˇsen´ım je zachycen´ı z´avislosti mezi d´elkou opsan´eho oblouku s a parametrem kˇrivky u pomoc´ı polynomi´aln´ı funkce vysok´eho ˇr´adu [21, 37]: u(s) = p(s) = a0 + a1 s + a2 s2 + a3 s3 + a4 s4 + a5 s5 + ...
(3.78)
Polynom p(s) by mˇel b´ yt opˇet alespoˇ n kvintick´ y, pro zajiˇstˇen´ı hladkosti pohybu aˇz do derivace zrychlen´ı (jerku). Probl´emem m˚ uˇze b´ yt zachytit z´avislost d´elky oblouku a parametru jedin´ ym polynomem pro celou kˇrivku se sloˇzitou geometri´ı. Zvyˇsov´an´ı ˇr´adu polynomu nepˇrin´ aˇs´ı dobr´e v´ ysledky, protoˇze vede podobnˇe jako pˇri interpolaci dat na nepˇrirozen´e zvlnˇen´ı s velk´ ym poˇctem lok´aln´ıch extr´em˚ u. Tento probl´em lze ˇreˇsit dˇelen´ım kˇrivky na menˇs´ı segmenty, z nichˇz kaˇzd´ y m´a vlastn´ı korekˇcn´ı polynom [37]. Mezi polynomy z jednotliv´ ych segment˚ u pak nav´ıc plat´ı spojitost jeho vyˇsˇs´ıch derivac´ı, pro bezr´ azov´ y pˇrechod mezi jednotliv´ ymi ˇc´astmi kˇrivky pˇri generov´an´ı trajektorie, podobnˇe jako u v´ ypoˇct˚ u interpolaˇcn´ıch spline kˇrivek. Pro synt´ezu korekˇcn´ı funkce je nutn´e poˇc´ıtat d´elku kˇrivky L1,2 na urˇcit´em intervalu jej´ıho parametru parametru podle vztahu Z u2 r Z u2 Z u2 dx dy dz ds du = ( )2 + ( )2 + ( )2 du = k Φ0 (u) k du; u1 ≤ u ≤ u2 L1,2 = du du du du u1 u1 u1 (3.79) Vztah (3.79) nelze bohuˇzel pro obecnou NURBS funkci ˇreˇsit analyticky a je nutn´e pˇristoupit k numerick´emu v´ ypoˇctu. V publikaci [71] je navrˇzen adaptivn´ı algoritmus pro numerick´ y v´ ypoˇcet s definovanou chybou. Oznaˇc´ıme-li integrand ze vztahu (3.79) jako f (u) =k Φ0 (u) k a zadefinujeme novou hodnotu parametru u3 leˇz´ıc´ı uprostˇred
73
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
intervalu u1 , u2 jako u3 =
u1 +u2 2 ,
pak aplikac´ı Simpsonova pravidla dostaneme pˇredpis
pro v´ ypoˇcet integr´ alu (3.79) ve formˇe L1,2 =
u2 − u1 (f (u1 ) + 4f (u3 ) + f (u2 )) 6
(3.80)
V dalˇs´ım kroku algoritmu je vyhodnoceno Simpsonovo pravidlo na dvou poloviˇcn´ıch subintervalech L1,3 a L3,2 a jejich souˇcet je porovn´an s hodnotou pro jeden interval L1,2 podle vztahu =k L1,3 + L3,2 − L1,2 k Lze dok´azat, ˇze maxim´ aln´ı chybu integrace lze vyˇc´ıslit podle vztahu max =
(3.81) 10 .
Uveden´e
dˇelen´ı intervalu se pak v dalˇs´ıch kroc´ıch opakuje tak dlouho, dokud dosaˇzen´a chyba nen´ı menˇs´ı neˇz pˇredem zadan´ a hodnota max . Pro integrand f (u) se spojitou ˇctvrtou derivac´ı na dan´em intervalu (splnˇeno pro kvintickou NURBS funkci), lze libovoln´e pˇresnosti doc´ılit v koneˇcn´em poˇctu dˇelen´ı [71]. Aplikujeme-li tento algoritmus na celou kˇrivku, tedy pro 0 ≤ u ≤ 1, pak postupn´ ym sˇc´ıt´an´ım d´elek oblouk˚ u jednotliv´ ych segment˚ u vznikl´ ych dˇelen´ım interval˚ u dostaneme mnoˇzinu uspoˇr´ adan´ ych dvojic {ui , si }; i = 1, .., ns , kter´a ud´av´a z´avislost parametru ui a d´elky oblouku opsan´eho z poˇc´ ateˇcn´ıho bodu kˇrivky si = L0,ui s definovanou pˇresnost´ı max . Posledn´ı dvojice pak oznaˇcuje celkovou d´elku kˇrivky. Takto z´ıskan´e dvojice {ui , si } lze pouˇz´ıt jako koincidenˇcn´ı body pro synt´ezu korekˇcn´ıho polynomu. Podobnˇe jako v sekci 3.1.2 vˇenovan´e synt´eze interpolaˇcn´ı kˇrivky pro popis trajektorie pohybu lze v tomto pˇr´ıpadˇe sestavit soustavu line´arn´ıch rovnic ve tvaru u = Φ.Θ kde u je vektor parametr˚ u ui odpov´ıdaj´ıc´ı koincidenˇcn´ım bod˚ um, Φ je matice regresor˚ u vyˇc´ıslen´ a z pˇredpisu korekˇcn´ıho polynomu (??) a hodnot d´elek oblouku si a Θ je hledan´ y vektor koeficient˚ u korekˇcn´ı polynomi´aln´ı funkce u(s). Tuto soustavu rovnic lze ˇreˇsit metodou line´ arn´ıch nejmenˇs´ıch ˇctverc˚ u. Po v´ ypoˇctu korekˇcn´ı funkce je moˇzn´e vyhodnotit pˇresnost ˇreˇsen´ı napˇr´ıklad v´ ypoˇctem stˇredn´ı kvadratick´e chyby M SE oproti vstupn´ım dat˚ um {ui , si } podle vztahu M SE =
ns X (ui − u bi )2 i=1
ns
(3.82)
kde u bi je predikce parametru z´ıskan´ a z korekˇcn´ı funkce. Jestliˇze dosaˇzen´a pˇresnost nevyhovuje, je zdvojn´ asoben poˇcet interval˚ u kˇrivky a pro kaˇzd´ y segment je navrˇzen jin´ y
74
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
Korekcni fce u(s)
1.6 1.4 1.2
1
0.5
0
1
du(s)/ds
y[m]
0
1
2
3
4
5
6
7
0
1
2
3
4
5
6
7
0
1
2
3 4 5 Delka opsaneho oblouku pro danou trajektorii s [m]
6
7
1.5
0.8 0.6 0.4
1 0.5 0
0.2 2
5 d u(s)/ds
0
2
−0.2 −0.4 −0.5
0
0.5
1 x[m]
1.5
2
0 −5 −10
2.5
V(t) pozadovana V(t) s korekci V(t) bez korekce
1.2
Rychlost posuvu po trajektorii v tecnem smeru V(t) [m/s]
Rychlost posuvu po trajektorii v tecnem smeru V(t) [m/s]
1.4
1
0.8
0.6
0.4
0.2
0
0
1
2
3
4 t[s]
5
6
7
8
V(t) pozadovana V(t) s korekci V(t) bez korekce
1.01
1.005
1
0.995
0.99
1
1.5
2
2.5
3
3.5 t[s]
4
4.5
5
5.5
6
Obr´ azek 3.13: Korekˇ cn´ı polynomi´ aln´ı funkce pro NURBS - kˇrivka v rovinˇe, korekˇcn´ı polynom a jeho dvˇe derivace, rychlost posuvu pˇri generov´an´ı trajektorie s korekˇcn´ı funkc´ı a bez n´ı
korekˇcn´ı polynom. Pro zajiˇstˇen´ı hladk´eho pˇrechodu mezi segmenty je nutn´e dodefinovat okrajov´e podm´ınky na rovnosti vyˇsˇs´ıch derivac´ı korekˇcn´ı funkce v navazovac´ıch bodech mezi jednotliv´ ymi polynomy. Ty lze z´ıskat v´ ypoˇctem diferenci´aln´ıch pˇr´ırustk˚ u parametru v˚ uˇci opsan´emu oblouku v aktu´aln´ım m´ıstˇe dan´e trajektorie podle podm´ınek: du(s) 1 d2 u(s) −Φ0 (u)T Φ00 (u) 00 = p0 (s) = , = p (s) = ds k Φ0 (u) k ds2 k Φ0 (u) k4
(3.83)
Optimalizaˇcn´ı probl´em pak lze ˇreˇsit metodou Lagrangeov´ ych multiplik´ator˚ u, kter´a je pops´ ana v zaˇc´ atku kapitoly. V´ yhodou metod zaloˇzen´ ych na korekˇcn´ı funkci je moˇznost off-line zpracov´an´ı jiˇz ve f´azi parametrizace trajektorie. Pˇri vlastn´ım generov´an´ı pohybu v re´aln´em ˇcase je pak v kaˇzd´em okamˇziku vyhodnocov´ana pouze jedna polynomi´aln´ı funkce oproti sloˇzit´emu vyˇc´ıslov´ an´ı vyˇsˇs´ıch derivac´ı parametrick´e funkce u Taylorova rozvoje, nav´ıc nedoch´ az´ı ke kumulov´ an´ı numerick´ ych chyb. Obr´ azek 3.13 ukazuje pˇr´ıklad pouˇzit´ı korekˇcn´ıch polynom˚ u na dvourozmˇernou trajektorii popsanou NURBS funkc´ı. Vlevo nahoˇre je trajektorie pohybu v rovinˇe, pro kterou je v´ yˇse popsanou metodou navrˇzena polynomi´aln´ı korekˇcn´ı funkce zajiˇst’uj´ıc´ı reparametrizaci kˇrivky s c´ılem omezit fluktuace pˇri generov´an´ı pohybu. Z´ıskan´a funkce
75
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
je sloˇzen´a ze sedmi polynom˚ u sedm´eho ˇr´adu se spoleˇcn´ ymi dvˇema derivacemi v navazovac´ıch bodech a popisuje z´ avislost parametru kˇrivky na poˇzadovan´em posuvu po dan´e trajektorii. Vlevo dole je srovn´ an´ı pr˚ ubˇeh˚ u poˇzadovan´e a skuteˇcn´e rychlosti posuvu pod´el zvolen´e trajektorie bez korekˇcn´ı funkce a s jej´ım pouˇzit´ım. Pr˚ ubˇeh rychlosti je z´ısk´an gener´atorem ˇcasovˇe optim´ aln´ıho pohybu v jedn´e ose s omezen´ım na rychlost, zrychlen´ı a jerk popsan´em v u ´vodu kapitoly. Z obr´azk˚ u je vidˇet, ˇze korekˇcn´ı funkce sn´ıˇzila u ´roveˇ n fluktuac´ı v rychlosti posuvu po kˇrivce z cca 25% na hodnotu 0.3%. Maxim´aln´ı odchylka od poˇzadovan´e rychlosti je pˇribliˇznˇe 3mm/s. Pˇresnost je moˇzn´e d´ale zvyˇsovat pouˇzit´ım sloˇzitˇejˇs´ı korekˇcn´ı funkce s vˇetˇs´ım poˇctem segment˚ u, pˇr´ıpadnˇe doplnˇen´ım gener´atoru trajektorie o iteraˇcn´ı algoritmus, kter´ y d´ale zpˇresˇ nuje odhad spr´avn´e hodnoty parametru kˇrivky v kaˇzd´em okamˇziku vzorkov´an´ı [21]. Korekˇcn´ı polynom v tomto pˇr´ıpadˇe slouˇz´ı pro vypoˇcten´ı poˇc´ ateˇcn´ıho odhadu, kter´ y je d´ale zpˇresˇ nov´an nˇekolika iteracemi pomoc´ı Newtonovo metody. Hladk´ y pr˚ ubˇeh pohybu bez neˇz´ adouc´ıch fluktuac´ı zp˚ usoben´ ych nepˇrirozenou parametrizac´ı trajektorie pˇrin´ aˇs´ı jednoznaˇcnˇe zv´ yˇsen´ı kvality ˇr´ızen´ı, kter´e se projevuje plynulejˇs´ım chodem stroje, niˇzˇs´ı u ´rovn´ı vybuzen´ ych mechanick´ ych vibrac´ı a v pˇr´ıpadˇe obr´abˇec´ıch stroj˚ u vyˇsˇs´ı kvalitou obr´ abˇen´ ych souˇc´ast´ı.
3.3.5
ˇ Casovˇ e optim´ aln´ı pohyb pod´ el zvolen´ e trajektorie
V pˇredchoz´ıch ˇc´ astech pr´ ace byly pops´ any r˚ uzn´e metody popisu trajektori´ı pro v´ıceos´e syst´emy a zp˚ usoby generov´ an´ı sloˇzit´ ych funkc´ı, kter´e nejsou pˇrirozenˇe parametrizov´any d´elkou oblouku a vyˇzaduj´ı dodateˇcn´e algoritmy pro zajiˇstˇen´ı hladk´eho pr˚ ubˇehu pohybu. Pˇrirozen´ ym poˇzadavkem, kter´ y m˚ uˇze vyvstat v pr˚ umyslov´ ych aplikac´ıch, je minimalizace doby pracovn´ıho cyklu stroje, coˇz vede na probl´em generov´an´ı ˇcasovˇe optim´aln´ıho pohybu po zadan´e trajektorii s dan´ ymi konstrukˇcn´ımi omezen´ımi napˇr´ıklad v podobˇe maxim´aln´ıho momentu, rychlosti nebo zrychlen´ı jednotliv´ ych aktu´ator˚ u. Motivac´ı pro takov´ y poˇzadavek m˚ uˇze b´ yt napˇr´ıklad pˇresun materi´alu robotem typu pick and place v co nejkratˇs´ım ˇcase nebo minimalizace doby obr´abˇen´ı u CNC stroje. Probl´em omezen´ı dan´ ych jednotliv´ ymi aktu´atory stroje je nutn´e v praxi ˇreˇsit i v u ´loh´ach, kter´e nevyˇzaduj´ı ˇcasovˇe optim´ aln´ı pohyb. Poˇzadavek na fyzik´aln´ı realizovatelnost pohybu se m˚ uˇze totiˇz kdykoliv dostat do rozporu s aktu´aln´ım vykon´avan´ ym pˇr´ıkazem zadan´ ym obsluhou. Jako pˇr´ıklad uved’me poˇzadavek rovnomˇern´eho pohybu s urˇcenou rychlost´ı posuvu po zvolen´e dr´ aze. Vlivem mˇen´ıc´ı se geometrie sledovan´e trajektorie
76
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
a v z´ avislosti na aktu´ aln´ı kinematick´e konfiguraci stroje pˇri vykon´av´an´ı pohybu m˚ uˇze v urˇcit´em okamˇziku doj´ıt k poruˇsen´ı dan´ ych omezen´ı na pohyb aktu´ator˚ u (pˇrekroˇcen´ı maxim´ aln´ı rychlosti, zrychlen´ı nebo momentu). V tomto okamˇziku nelze d´ale pokraˇcovat v pohybu, protoˇze pro dalˇs´ı sledov´an´ı zadan´e trajektorie by stroj musel vykonat ´ fyzicky nerealizovateln´ y pohyb. Ulohou ˇr´ıdic´ıho syst´emu, konkr´etnˇe interpol´atoru, kter´ y pl´anuje ˇcasov´ y pr˚ ubˇeh pohybu, je oˇsetˇren´ı tˇechto chybov´ ych stav˚ u, tedy doˇcasn´e zpomalen´ı rychlosti posuvu na takovou mez, kter´a umoˇzn´ı fyzicky realizovat zadan´ y pohyb. Z uveden´eho pˇr´ıkladu vypl´ yv´ a, ˇze libovoln´ y pohyb v´ıceos´eho syst´emu po zadan´e trajektorii lze ch´ apat jako probl´em ˇcasovˇe optim´aln´ıho ˇr´ızen´ı nav´ıc s omezen´ımi na pr˚ ubˇeh pohybu v operaˇcn´ıch souˇradnic´ıch. Obecn´ y probl´em ˇcasovˇe optim´aln´ıho ˇr´ızen´ı pro v´ıceos´e syst´emy nen´ı v souˇcasn´e dobˇe kromˇe nejjednoduˇsˇs´ıch pˇr´ıpad˚ u uspokojivˇe vyˇreˇsen. Jeho obt´ıˇznost vypl´ yv´a ze silnˇe neline´ arn´ıch vztah˚ u vystupuj´ıc´ıch jak v kinematick´ ych tak dynamick´ ych modelech. Zat´ımco omezen´ı na pohyb jsou kladena na jednotliv´e pohony, pak v´ ysledkem algoritmu mus´ı b´ yt rychlost posuvu pod´el zvolen´e trajektorie efektoru, z ˇcehoˇz vypl´ yv´a nutnost ˇreˇsit neline´ arn´ı optimalizaˇcn´ı probl´em s neline´arn´ımi omezen´ımi, pro kter´ y vˇetˇsinou neexistuje analytick´e ˇreˇsen´ı. V´ ysledkem jsou nejr˚ uznˇejˇs´ı suboptim´aln´ı metody vyuˇz´ıvaj´ıc´ı r˚ uzn´ ych heuristik a numerick´ ych ˇreˇsen´ı. Pro nasazen´ı pˇri CNC obr´abˇen´ı pˇrevl´adaj´ı v literatuˇre metody vych´azej´ıc´ı z kinematick´eho modelu syst´emu. Veˇsker´a omezen´ı jsou kladena na rychlost a jej´ı derivace v uzlov´em nebo operaˇcn´ım prostoru odkud jsou transformov´ana na limitn´ı hodnoty pr˚ ubˇehu posuvu po zadan´e trajektorii v ˇcase [21, 22, 37]. Tento pˇr´ıstup je do znaˇcn´e m´ıry ovlivnˇen decentralizovanou strukturou ˇr´ıdic´ıho syst´emu, kter´a je pouˇzita ve vˇetˇsinˇe pˇr´ıpad˚ u. Kaˇzd´ y pohon je ˇr´ızen vlastn´ım osov´ ym modulem zajiˇst’uj´ıc´ım sledov´an´ı referenˇcn´ı trajektorie vypoˇcten´e interpol´atorem. Celkov´a dynamika syst´emu a vz´ajemn´e vazby mezi osami nejsou v u ´rovni pl´anov´an´ı trajektorie uvaˇzov´any. Kvalitu sledov´an´ı lze ovlivnit nepˇr´ımo nastaven´ım jednotliv´ ych omezen´ı v gener´atoru pˇr´ıpadnˇe modifikac´ı vrstvy regul´ atoru ve formˇe algoritm˚ u robustn´ıho ˇr´ızen´ı, dopˇredn´e kompenzace tˇren´ı, omezen´ı vibrac´ı apod. Jako z´asadn´ı se uk´azal poˇzadavek generovat trajektorie s omezen´ ym jerkem na jednotliv´ ych aktu´atorech. V oblasti v´ yzkumu robotick´ ych manipul´ator˚ u pˇrevaˇzuje snaha uchopit exaktnˇe probl´em ˇcasovˇe optim´ aln´ıho ˇr´ızen´ı zaveden´ım kompletn´ıho dynamick´eho modelu syst´emu.
77
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
Prvn´ı publikovan´e pr´ ace uk´ azaly, ˇze obecn´ y n-rozmˇern´ y probl´em lze pˇrev´est na jednoduˇsˇs´ı u ´lohu ˇr´ızen´ı syst´emu druh´eho ˇr´adu s neline´arn´ımi omezen´ımi na stav a ˇr´ızen´ı. Pro jednoduˇsˇs´ı variantu u ´lohy, kter´ a zahrnuje omezen´ı na maxim´aln´ı moment jednotliv´ ych aktu´ator˚ u, byly vyvinuty efektivn´ı algoritmy ˇreˇs´ıc´ı pl´anov´an´ı pohybu [23, 24, 25]. V´ ysledn´e trajektorie se vˇsak z´ ahy uk´ azaly jako nevhodn´e pro nasazen´ı v praktick´ ych u ´loh´ach z d˚ uvodu prudk´ ych zmˇen akˇcn´ıch z´asah˚ u jednotliv´ ych aktu´ator˚ u, coˇz vedlo na probl´emy se sledov´ an´ım v regulaˇcn´ı u ´rovni, buzen´ım vibrac´ı, zv´ yˇsen´ ym opotˇreben´ım mechanick´ ych ˇc´ ast´ı stroje apod. Snaha produkovat trajektorie s hladk´ ym pr˚ ubˇehem pohybu vedla k rozˇs´ıˇren´ı u ´lohy o omezen´ı na rychlost zmˇeny momentu (ekvivalent k omezen´emu jerku). Toto rozˇs´ıˇren´ı dramaticky zkomplikovalo cel´ y probl´em, kter´ y vˇetˇsina st´avaj´ıc´ıch metod ˇreˇs´ı numericky [26, 27, 72]. Formulace probl´ emu Pr˚ ubˇeh trajektorie pohybu v souˇradnic´ıch operaˇcn´ıho prostoru je pops´an parametrickou funkc´ı ve tvaru X = Φ (u) ; u ∈ h0, 1i
(3.84)
C´ılem ˇcasovˇe optim´ aln´ıho ˇr´ızen´ı nalezen´ı takov´e funkce popisuj´ıc´ı pr˚ ubˇeh parametru trajektorie v ˇcase u = f (t); f (0) = 0, f (t1 ) = 1
(3.85)
kter´a minimalizuje celkov´ y ˇcas t1 potˇrebn´ y k vykon´an´ı dan´eho pohybu pod´el zadan´e trajektorie pˇri respektov´ an´ı vˇsech kladen´ ych omezen´ı: Z u(t)
t1
dt; C(t) ≤ 0
minJ =
(3.86)
0
kde C(t) je mnoˇzina obecnˇe neline´ arn´ıch omezen´ı, kter´a mohou b´ yt specifikov´ana jak v operaˇcn´ım prostoru tak v souˇradnic´ıch aktu´ator˚ u a vych´azej´ı z fyzik´aln´ıch limit˚ u dan´ ych konstrukc´ı stroje nebo z technologick´ ych d˚ uvod˚ u. Z´akladn´ı podm´ınkou je omezen´ı maxim´aln´ı rychlosti jednotliv´ ych aktu´ator˚ u, kterou lze zapsat |
dθi |=| θ˙i |≤ vimax ; i = 1..n dt
(3.87)
kde n je poˇcet aktu´ ator˚ u stroje a θi pˇredstavuje translaˇcn´ı nebo rotaˇcn´ı rychlost pˇr´ısluˇsn´eho pohonu, kter´ a je limitov´ ana jednak jeho konstrukc´ı, d´ale pˇr´ıpadnˇe pouˇzit´ ymi
78
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
pˇrevodovkami. Dalˇs´ı fyzik´ aln´ı omezen´ı vypl´ yv´a z maxim´aln´ı s´ıly nebo momentu, kterou je aktu´ ator schopen p˚ usobit na pracovn´ı mechanismus stroje | Ti |≤ Timax ; i = 1..n
(3.88)
Pro vyhodnocen´ı vlivu omezen´ı (3.88) na pr˚ ubˇeh pohybu je obvykle nutn´e zn´at dynamick´ y model syst´emu, kter´ y je pro sloˇzit´ y v´ıceos´ y stroj obt´ıˇzn´e z´ıskat kv˚ uli nelinearit´am ˇ v podobˇe tˇren´ı, hystereze a v˚ ule v pˇrevodovk´ach apod. Casto b´ yv´a nahrazeno vhodn´ ym maxim´ aln´ım zrychlen´ım jednotliv´ ych aktu´ator˚ u (kter´e je na maxim´aln´ı moment v´az´ano momentem setrvaˇcnosti z´ atˇeˇze): |
d2 θi |=| θ¨i |≤ amax ; i = 1..n i dt2
(3.89)
Pro hladk´ y pr˚ ubˇeh pohybu je d´ale vhodn´e omezit derivaci zrychlen´ı - jerk, kter´ y reprezentuje frekvenˇcn´ı spektrum akˇcn´ıch z´asah˚ u p˚ usob´ıc´ıch na mechanick´ y syst´em v podobˇe s´ıly nebo momentu. Pˇr´ıliˇs vysok´e hodnoty (pˇr´ıpadnˇe nekoneˇcn´e pro nespojit´ y pr˚ ubˇeh zrychlen´ı) mohou vybudit kmitav´e m´ody zp˚ usoben´e koneˇcnou tuhost´ı konstrukce stroje. |
... d3 θi |=| θi |≤ jimax ; i = 1..n dt3
(3.90)
Dalˇs´ı poˇzadavky mohou b´ yt kladeny na pr˚ ubˇeh pohybu v operaˇcn´ım prostoru. Z technologick´ ych d˚ uvod˚ u m˚ uˇze b´ yt ˇz´adouc´ı omezit rychlost posuvu, zrychlen´ı nebo jerku v teˇcn´em smˇeru pohybu po zadan´e trajektorii (napˇr. velikost ˇrezn´e s´ıly pˇri obr´ abˇen´ı, maxim´ aln´ı vhodn´ a rychlost pro nan´aˇsen´ı laku atd.): ds(t) | = | V (t) |≤ Vmax dt d2 s(t) | | = | A(t) |≤ Amax dt2 d3 s(t) | | = | J(t) |≤ Jmax dt3 |
(3.91) (3.92) (3.93)
Metody zaloˇ zen´ e na kinematick´ em modelu - metoda nejhorˇ s´ıho pˇ r´ıpadu Krit´erium minim´ aln´ıho ˇcasu popsan´e funkcion´alem (3.94) lze pˇreformulovat s pouˇzit´ım vztahu pro polohu na dan´e trajektorii a rychlost´ı posuvu Z minJ = s(t)
0
l
ds(t) dt
1 ds; C(t) ≤ 0 V (t)
79
= V (t) na tvar (3.94)
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
kter´ y ˇr´ık´a, ˇze pˇrejezdu v co nejkratˇs´ım ˇcase lze dos´ahnout maximalizac´ı rychlosti posuvu pod´el dan´e trajektorie. Stejnˇe jako ˇrada dalˇs´ıch metod pro generov´an´ı ˇcasovˇe optim´aln´ıch trajektori´ı pracuje ve dvou kroc´ıch. Nejdˇr´ıve je urˇcen vhodn´ y profil funkce popisuj´ıc´ı pr˚ ubˇeh posuvu, tzv. modulaˇcn´ı strategie, kter´a redukuje dimenzi optimalizaˇcn´ıho probl´emu na koneˇcnou hodnotu. V druh´em kroku jsou optimalizov´any parametry zvolen´eho profilu s c´ılem minimalizovat dobu pohybu pˇri dan´ ych omezen´ıch. Metoda nejhorˇs´ıho pˇr´ıpadu byla vyvinuta pro rychlou a efektivn´ı implementaci algoritm˚ u pro vysokorychlostn´ı obr´ abˇen´ı u CNC stroj˚ u [73] a d´ıky sv´e jednoduchosti a v´ ypoˇcetn´ı nen´ aroˇcnosti je ˇcasto nasazov´ana v praxi. Z´akladn´ım principem je rozdˇelen´ı dan´e trajektorie na d´ılˇc´ı segmenty, pˇriˇcemˇz pro kaˇzdou ˇc´ast jsou vyhodnocena omezen´ı na maxim´aln´ı rychlost posuvu V (t) vypl´ yvaj´ıc´ı ze vˇsech limit˚ u specifikovan´ ych jak v uzlov´em, tak operaˇcn´ım prostoru. Po vyhodnocen´ı vˇsech omezen´ı je vhodnou modulaˇcn´ı strategi´ı vypoˇcten pr˚ ubˇeh posuvu v ˇcase. Princip metody lze snadno demonstrovat na pˇr´ıpadu tˇr´ıos´eho kart´ezsk´eho manipul´atoru. Operaˇcn´ı a uzlov´ y prostor (souˇradn´e syst´emy stroje a aktu´ator˚ u) v tomto pˇr´ıpadˇe spl´ yvaj´ı a nen´ı nutn´e ˇreˇsit vz´ ajemn´e kinematick´e transformace, coˇz celou u ´lohu v´ yraznˇe zjednoduˇsuje. Pohyb aktu´ ator˚ u v jednotliv´ ych os´ach stroje je pro danou napl´anovanou trajektorii pˇr´ımo pops´ an parametrickou funkc´ı
x(u) X(u) = y(u) = Φ(u) z(u)
(3.95)
Pro vyhodnocen´ı vlivu omezen´ı rychlosti, zrychlen´ı a jerku jednotliv´ ych aktu´ator˚ u na rychlost je tˇreba postupnˇe derivovat vztah (3.95) podle ˇcasu: dX ˙ = dΦ ds = Φs V (t) =X dt ds dt 2 d X ¨ = Φs A(t) + Φss V (t)2 =X dt2 d3 X ... = X = Φs J(t) + 3Φss A(t)V (t) + Φsss V (t)3 dt3
(3.96) (3.97) (3.98) (3.99)
kde V (t), A(t), J(t) je rychlost posuvu, zrychlen´ı a jerk v teˇcn´em smˇeru a Φs , Φss , Φsss jsou prvn´ı tˇri geometrick´e derivace dan´e trajektorie, kter´e lze postupn´ ym derivov´an´ım
80
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
sloˇzen´ ych funkc´ı vyj´ adˇrit vztahy Φs = Φss = Φsss = Hodnoty derivac´ı
du ds
dΦ du du ds dΦ d2 u d2 Φ du 2 + ( ) du ds2 du2 ds dΦ d3 u d2 Φ d2 u du d3 Φ du 3 + 3 + ( ) du ds3 du2 ds2 ds du3 ds
(3.100) (3.101) (3.102)
lze pˇri pouˇzit´ı metody korekˇcn´ıho polynomu vyˇc´ıslit pˇr´ımo z hod-
not derivac´ı korekˇcn´ı funkce, ˇc´ımˇz se d´ale zjednoduˇsuje v´ ypoˇcet. Uveden´e vztahy lze vyuˇz´ıt ke stanoven´ı maxim´aln´ı moˇzn´e rychlosti posuvu V (t), pˇri kter´e nedojde k poruˇsen´ı ˇz´adn´ ych omezen´ı na aktu´atory stroje. Z d˚ uvodu rychlosti v´ ypoˇctu jsou tato omezen´ı vyˇsetˇrov´ana pouze bodovˇe na vhodnˇe volen´ ych m´ıstech trajektorie (lze volit adaptivnˇe napˇr´ıklad podle mˇen´ıc´ı se kˇrivosti). Ze vztahu (3.96) lze urˇcit tˇri nerovnosti pro maxim´aln´ı rychlost posuvu dan´e omezen´ım maxim´aln´ı rychlosti aktu´ ator˚ u z nichˇz je vybr´ ana nejmenˇs´ı hodnota, kter´a vyhovuje vˇsem omezen´ım z´aroveˇ n v Vmax ≤ min{
vimax }; i = 1, 2, 3 |Φsi |
(3.103)
kde vimax jsou maxim´ aln´ı rychlosti jednotliv´ ych os Φsi je pˇr´ısluˇsn´ y ˇr´adek vektoru prvn´ı geometrick´e derivace trajektorie. Podobnˇe lze odvodit omezen´ı rychlosti posuvu vlivem maxim´ aln´ıho zrychlen´ı vˇsech aktu´ator˚ u ze vztahu (3.97): s amax − |Φsi |Amax a i Vmax ≤ min{ }; i = 1, 2, 3 |Φssi |
(3.104)
kde za hodnotu teˇcn´eho zrychlen´ı A(t) byla dosazena nejhorˇs´ı moˇzn´ a varianta Amax specifikovan´ a jako omezen´ı v operaˇcn´ım prostoru pˇri n´avrhu stroje (3.92). Pokud je omezen´ı Amax menˇs´ı neˇz maxim´aln´ı hodnoty zrychlen´ı vˇsech os, coˇz je v praxi vˇzdy splnˇeno, je v´ ysledkem v´ yrazu (3.104) vˇzdy kladn´a hodnota pro rychlost posuvu po kˇrivce. Jako posledn´ı zb´ yv´ a vyhodnotit vliv omezen´ı jerku aktu´ator˚ u na rychlost pohybu, za nezn´ am´e veliˇciny A(t) a J(t) jsou opˇet dosazeny nejhorˇs´ı moˇzn´e hodnoty Amax , Jmax :
3 |Φsssi |Vij
j Vmax ≤ min{Vij }; i = 1, 2, 3
(3.105)
+ 3|Φssi |Amax Vij + (|Φsi |Jmax − jimax )
(3.106)
Hodnoty Vij jsou z´ısk´ any ˇreˇsen´ım tˇr´ı kubick´ ych rovnic, kter´e opˇet pro podm´ınku jimax > Jm ax maj´ı alespoˇ n jedno kladn´e ˇreˇsen´ı. V´ ysledn´e omezen´ı na rychlost posuvu je z´ısk´ano kombinac´ı vˇsech tˇr´ı omezen´ı dan´ ych aktu´atory v´ ybˇerem minim´aln´ı hodnoty podle
81
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
58
V(t), [mm/sec]
Chapter 4. Jerk Limited Feedrate Modulation
...
.
9 99 9 F1 F2 F3 F4
9 999 F5 F6F7F8 ...
Prediktivní okénko
t [s]
Hranice segmentů křivky Omezení na rychlost posuvu daná aktuátory Kinematicky kompatibilní profil
9 Ověřená část profilu předaná generátoru
Figure 4-5. Implementation of feed modulation strategy with a look-ahead window. Obr´ azek 3.14: Generov´ an´ı profilu posuvu - prediktivn´ıho ok´enko
kinematically compatible solution may not be achievable by simply modulating the vztahu feed, acceleration, or jerk values. In such circumstances, the real-time commanded v
a
j
≤ min{V , Vmax } (3.107) to max , Vmax interpolator needs to back-trackVmax through the planned feed values and perform adjustment
the earlier NCeblocks, order to yieldpro a kinematically profile for the current Na z´akladˇ omezen´in ı (3.107) je pak kaˇzd´ y segmentcompatible modulov´anafeed rychlost posuvu takov´ ym zp˚ usobem, Hence, aby nedoˇ lo k pˇrekroˇcen´ ı t´etoismeze. Jako vhodnou modulaˇ cn´ı strategii trajectory segment. aslook-ahead buffer implemented for this purpose as shown in lze 4-5. zvolit napˇr´ıklad po ˇc´ astech konstantn´ı profil jerku J(t) podobn´ y ˇcasovˇe optim´aln´ımu Figure ˇr´ızen´ı v jedn´e ose.
When the desired feedrate of a segment provided by the NC programmer or an Uveden´ y algoritmus lze pouˇz´ıt jak pro off-line pl´anov´an´ı trajektorie, kdy je cel´ y po-
optimization routine is incompatible, a bisection method findsvariantˇ a kinematically feasible hyb pˇredpoˇ c´ıt´ an dopˇ redu jeˇstˇe pˇred jeho zapoˇsearch cet´ım, tak v on-line e d´ıky n´ızk´ ym feedv´ efficiently. The feeds the previous nextpoˇ segment are eithers tzv. fixedprediktivn´ or free, ım where ypoˇcetn´ım n´ arok˚ um. Vof tomto pˇr´ıpadˇe jeand pohyb c´ıt´an v pˇ redstihu enkem, e vyhodnocuje profil posuvusegment. a postupnˇ ho ukl´ad´ a do pamˇ eti,feed free ok´ means thatkter´ it is set equal to the feedrychlosti of the current Ineforward planning, the ´ odkud je pˇred´ av´ an modulu gener´ re´aln´ em ˇcase. is Ukolem prediktivof the previous segment is fixed andatoru the trajektorie feed of thev next segment generally free. The n´ıho ok´enka je napl´ anov´ an´ı tzv. kinematicky kompatibiln´ıho profilu, kter´ y zaruˇcuje, ˇze
exception is when the desired feed of the next segment is lower than the test feed f mid , in v kaˇzd´em segmentu trajektorie lze zmˇenit rychlost posuvu pˇri splnˇen´ı vˇsech omezen´ı
which case the feed of the next segment fixed at itsydesired Fk +1na . Similar rules v segmentu n´ asleduj´ ıc´ım a z´ aroveˇ n lzeis napl´ anovan´ pohyb level, realizovat dostupn´ e zb´ yapply vaj´ıc´ı dr´azeplanning do koncov´ bodu (viz k poruˇ sen´ eto podm´by ınky, stroj for backward ineho reverse. The 3.14). rangePokud of thedojde search space isı t´ bounded zero at the bud’to ejede koncov´ y bod nebo mus´ ı b´ yt v posledn´ ım segmentu e pˇrekroˇ eno nˇ ebottom, andpˇrthe desired feedrate Fk at the top. The search algorithmnutnˇ bisects thecfeed search kter´e z urˇcen´ ych omezen´ı na pr˚ ubˇeh pohybu. V tom pˇr´ıpadˇe je nutn´e prov´est zpˇetn´ y
space iteratively, as shown in Figure 4-6, until a feasible solution is found within a specified v´ ypoˇcet od koncov´eho bodu nebo bodu, ve kter´em doˇslo k poruˇsen´ı omezen´ı a opravit
tolerance Ftol . cThe of iterations required to find the newe rychlosti. compatible feed within the dˇr´ıve vypoˇ ten´ ynumber profil posuvu sn´ıˇzen´ım maxim´ aln´ ı dosaˇ ziteln´ Uvedenou lze pouˇ z´ıt iEquation pro obecnou kinematickou strukturu syst´emu. Pro specified tolerancemetodu can be found with (4.24). zn´am´e vztahy mezi uzlov´ ym a operaˇcn´ım prostorem X = G(Θ), Θ = G−1 (X), lze
n = round [log 2 ( Fk / Ftol )] + 1
82
(4.24)
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
u ´lohu pˇreformulovat transformac´ı trajektorie pohybu do souˇradnic aktu´ator˚ u, kde jiˇz lze vyhodnotit vliv jednotliv´ ych omezen´ı na pr˚ ubˇeh profilu posuvu. Vyj´adˇren´ım z´avislosti rychlosti aktu´ ator˚ u na rychlosti posuvu dostaneme vztah −1 ˆ ˙ = dG dΦ V (t) = dΦ V (t) Θ dX ds ds
(3.108)
ˆ = G−1 (Φ) z˚ Ze srovn´ an´ı vztah˚ u (3.108) a (3.96) je vidˇet, ˇze po substituci Φ ust´avaj´ı v platnosti vˇsechny z´ avislosti mezi rychlost´ı posuvu, rychlost´ı aktu´ator˚ u a jejich vyˇsˇs´ıch derivac´ı (3.96-3.98) a stejn´ y je i v´ ypoˇcet omezen´ı popsan´ y nerovnostmi (3.103-3.107). Komplikuje se vˇsak vyhodnocen´ı geometrick´ ych derivac´ı, kter´e mus´ı pro obecn´ y pˇr´ıpad zahrnovat tak´e diferenci´ aln´ı zmˇeny v konfiguraci stroje dan´e jeho kinematickou strukturou: ˆs = Φ ˆ ss = Φ ˆ sss = Φ
dG−1 dΦ dX ds d2 G−1 dΦ 2 dG−1 d2 Φ ( ) + dX2 ds dX ds2 3 −1 d G dΦ 3 d2 G−1 dΦ d2 Φ dG−1 d3 Φ ( ) + 3 + dX3 ds dX2 ds ds2 dX ds3
(3.109) (3.110) (3.111)
Pro v´ ypoˇcet geometrick´ ych derivac´ı je tedy nutn´e v kaˇzd´em bodu poˇc´ıtat hodnotu Jakobi´ anu
dG−1 dX
a jeho dvou derivac´ı, coˇz m˚ uˇze b´ yt pro sloˇzitejˇs´ı kinematick´e struktury
ˇcasovˇe velmi n´ aroˇcn´e, analytick´e ˇreˇsen´ı nav´ıc ˇcasto nen´ı k dispozici a je nutn´e pouˇz´ıt vhodnou numerickou metodu. Dalˇs´ım probl´emem obecn´eho pˇr´ıpadu je, ˇze nerovnosti (3.104) a (3.106) nemus´ı m´ıt pro uvaˇzovan´ y nejhorˇs´ı pˇr´ıpad pˇri dosazen´ı maxim´ aln´ıch hodnot Amax a Jmax kladn´e ˇreˇsen´ı pro maxim´aln´ı hodnotu posuvu. Jin´ ymi slovy pro obecnou kinematiku stroje nelze zaruˇcit, ˇze v libovoln´em m´ıstˇe trajektorie bude existovat kladn´ a rychlost posuvu, pˇri kter´e lze akcelerovat nebo decelerovat s maxim´aln´ımi hodnotami zrychlen´ı a jerku v teˇcn´em smˇeru, aniˇz by bylo poruˇseno nˇekter´e z omezen´ı aktu´ ator˚ u. Lze vˇsak dok´azat, ˇze pˇri volbˇe A(t) = J(t) = 0 existuje v libovoln´em bodu kladn´ a rychlost, kterou se lze pod´el zvolen´e trajektorie pohybovat. Algoritmus pl´ anov´ an´ı lze pak upravit pro oˇsetˇren´ı tˇechto m´ıst, kter´a je potˇreba proj´ıˇzdˇet konstantn´ı rychlost´ı bez zrychlen´ı nebo zpomalen´ı. Zvl´aˇstn´ı vyj´ımku pak pˇredstavuj´ı singul´ arn´ı body stroje, ve kter´ ych geometrick´e derivace nab´ yvaj´ı nekoneˇcn´ ych hodnot vlivem k nekoneˇcnu konverguj´ıc´ım hodnot´am Jakobi´anu a jeho derivac´ı. N´avrh samotn´eho manipul´ atoru stejnˇe jako volba tvaru trajektorie v operaˇcn´ım prostoru tedy mus´ı respektovat moˇznost v´ yskytu tˇechto singularit, kter´ ym je tˇreba se vyhnout.
83
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
V´ yhodou uveden´e metody je mal´ a v´ ypoˇcetn´ı n´aroˇcnost, kter´a ji pˇredurˇcuje pro pouˇzit´ı v on-line gener´ atorech trajektorie jak pro roboty tak obr´abˇec´ı stroje. V´ ypoˇcetn´ı n´aroky ovˇsem stoupaj´ı se sloˇzitˇejˇs´ı kinematikou stroje, coˇz m˚ uˇze komplikovat jej´ı nasazen´ı. Nev´ yhodou m˚ uˇze b´ yt pˇr´ıliˇsn´ a konzervativnost napl´anovan´ ych profil˚ u posuvu pˇri uvaˇzov´an´ı nejhorˇs´ı kombinace nezn´ am´ ych veliˇcin. Existuj´ı r˚ uzn´e heuristick´e strategie, kter´e se snaˇz´ı tuto nev´ yhodu odstranit nahrazen´ım nejhorˇs´ıho pˇr´ıpadu skuteˇcn´ ymi aktu´aln´ımi hodnotami podle zvolen´e modulaˇcn´ı strategie, coˇz umoˇzn ˇuje dos´ahnout kratˇs´ıch ˇcas˚ u pr˚ ujezdu, ovˇsem za cenu delˇs´ı doby v´ ypoˇctu trajektorie [37]. Z pˇr´ısnˇe matematick´eho hlediska nen´ı v´ ysledn´ y profil ˇcasovˇe optim´aln´ı. Omezen´ı dan´a aktu´atory jsou nav´ıc vyhodnocov´ ana bodovˇe, v m´ıstech mezi tˇemito testovac´ımi body pak m˚ uˇze teoreticky pˇri ˇspatnˇe zvolen´em intervalu dˇelen´ı doj´ıt k poruˇsen´ı omezuj´ıc´ıch podm´ınek. Gradientn´ı metody se spline funkcemi Dalˇs´ı skupina metod pouˇz´ıv´ a obecnˇejˇs´ıch pr˚ ubˇeh˚ u profilu posuvu, obl´ıben´e jsou po ´ ˇc´astech polynomi´ aln´ı spline funkce [21, 22, 37]. Uloha je pak pˇrevedena na neline´arn´ı optimalizaˇcn´ı probl´em, kter´ y je ˇreˇsen nˇekterou ze standardn´ıch numerick´ ych metod. Pˇr´ıkladem gradientn´ı metody je postup vyuˇz´ıvaj´ıc´ı kvintick´e spline profily s minim´aln´ım pr˚ ubˇehem jerku [21]. Hledan´ a funkce posuvu po kˇrivce v ˇcase (s = s(t)) mus´ı splˇ novat tyto podm´ınky: • Funkce proch´ az´ı mnoˇzinou koincidenˇcn´ıch bod˚ u dan´ ych dvojicemi {Si , Ti }; i = 1, ..., n, kde Si je d´elka oblouku v jednotliv´ ych segmentech kˇrivky a Ti jsou pˇr´ısluˇsn´e doby pr˚ ujezdu mezi hraniˇcn´ımi body • Funkce s(t) m´ a spojit´e alespoˇ n tˇri derivace • Je minimalizov´ an integr´ al ˇctverce tˇret´ı derivace v pr˚ ubˇehu pohybu: J =
R t ... 2 s 0 [ (t)] dt
Minimalizace jerku v´ ysledn´eho profilu posuvu m´a za c´ıl omezit prudk´e zmˇeny zrychlen´ı a t´ım dos´ ahnout kvalitnˇejˇs´ıho sledov´an´ı napl´anovan´e trajektorie v regulaˇcn´ı vrstvˇe syst´emu. Bylo uk´ az´ ano, ˇze ˇreˇsen´ı popsan´eho u ´lohy vede na kvintickou interpolaˇcn´ı spline funkci, jej´ıˇz parametry lze z´ıskat minimalizac´ı kvadratick´eho krit´eria s vazbov´ ymi podm´ınkami ve tvaru rovnosti, lze tedy pouˇz´ıt v´ yˇse nˇekolikr´at zm´ınˇenou metodu Lagrangeov´ ych multiplik´ ator˚ u.
84
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
ˇ Obr´ azek 3.15: Casovˇ e optim´ aln´ı profil posuvu - kvintick´a spline funkce s minim´aln´ım jerkem [21]
C´ılem synt´ezy ˇcasovˇe optim´aln´ıho profilu je pak nalezen´ı takov´ ych hodnot ˇcas˚ u Ti , kter´e minimalizuj´ı celkovou dobu pohybu pˇri splnˇen´ı vˇsech dynamick´ ych omezen´ı dan´ ych v uzlov´em a operaˇcn´ım prostoru, kter´e lze zapsat ve formˇe nerovnost´ı (viz 3.87-3.93): min J = Ti
n X Ti = TP ; C(t) ≤ 0, 0 ≤ t ≤ TP
(3.112)
i=1
Jedn´ a se o optimalizaˇcn´ı probl´em s line´arn´ım krit´eriem a neline´arn´ımi omezen´ımi (vzhledem k parametr˚ um Ti ), kter´ y lze ˇreˇsit nˇekterou ze zn´am´ ych numerick´ ych metod jako napˇr´ıklad sekvenˇcn´ı kvadratick´e programov´an´ı [74]. Neline´arn´ı optimalizaˇcn´ı probl´em je v tomto pˇr´ıpadˇe pˇreveden na sekvenci subprobl´em˚ u, kdy v kaˇzd´e kroku algoritmu je hled´ ano minimum kvadratick´eho modelu u ´ˇcelov´e funkce pro linearizovan´a omezen´ı. V´ ysledkem tohoto lok´ aln´ıho ˇreˇsen´ı je pak smˇer postupu v prostoru parametr˚ u ke glob´aln´ımu ˇreˇsen´ı p˚ uvodn´ı u ´lohy. V´ yhodou tˇechto metod je pouˇzit´ı obecnˇejˇs´ı polynomi´aln´ı funkce pro popis profilu posuvu oproti standardn´ım modulaˇcn´ım strategi´ım (napˇr. v´ yˇse zm´ınˇen´a po ˇc´astech konstantn´ı funkce jerku), coˇz umoˇzn ˇuje dosaˇzen´ı kratˇs´ıch ˇcas˚ u pr˚ ujezdu. Nev´ yhodou jsou vysok´e v´ ypoˇcetn´ı n´ aroky, kter´e vypl´ yvaj´ı z nutnosti v´ ypoˇctu gradientu u ´ˇcelov´e funkce v˚ uˇci parametr˚ um profilu
∂C(t) ∂T
na vhodnˇe zvolen´e mnoˇzinˇe testovac´ıch bod˚ u pod´el dan´e
85
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
trajektorie. Srovn´ avac´ı studie uv´ ad´ı doby v´ ypoˇctu optim´aln´ıho profilu ˇr´adovˇe v jednotk´ach aˇz des´ıtk´ ach minut na osobn´ım poˇc´ıtaˇci pr˚ umˇern´eho v´ ykonu pro v´ ysledn´ y pohyb trvaj´ıc´ı nˇekolik vteˇrin [21, 37]. Gradientn´ı metody jsou tedy vhodn´e sp´ıˇse pro off-line pl´anov´an´ı trajektori´ı pro CNC obr´ abˇen´ı, kde nen´ı nutn´e mˇenit program za bˇehu stroje. Z matematick´eho hlediska nen´ı zaruˇcena konvergence ke glob´aln´ımu ˇcasovˇe optim´aln´ımu ˇreˇsen´ı. Metody zaloˇ zen´ e na dynamick´ em modelu V´ıceos´ y manipul´ ator s n stupni volnosti lze pˇri zanedb´an´ı pruˇznosti jednotliv´ ych ˇc´ast´ı modelovat jako soustavu tuh´ ych tˇeles. Pouˇzit´ı metody Lagrangeov´ ych rovnic vede na neline´arn´ı soustavu diferenci´ aln´ıch rovnic druh´eho ˇr´adu, kterou lze zapsat v maticov´em tvaru: ¨ +Θ ˙ T C(Θ)Θ ˙ + G(Θ) = T − Fc sgn(Θ) ˙ − Fv Θ ˙ M(Θ)Θ
(3.113)
kde Θn×1 je vektor uzlov´ ych souˇradnic, M(Θ)n×n je pozitivnˇe definitn´ı matice setrvaˇcnosti, kter´ a ud´ av´ a moment setrvaˇcnosti v kaˇzd´em kloubu vzhledem k aktu´aln´ı konfiguraci a vz´ ajemn´ y vliv zrychlen´ı mezi jednotliv´ ymi klouby, C(Θ)n×n×n je tenzor zd´anliv´ ych setrvaˇcn´ ych a Coriolisovo sil, kter´e p˚ usob´ı na kaˇzd´ y kloub vlivem nenulov´ ych rychlost´ı jednotliv´ ych ramen manipul´atoru, G(Θ)n×1 je vektor gravitace popisuj´ıc´ı u ´ˇcinky gravitaˇcn´ı s´ıly a Tn×1 je vektor moment˚ u/sil jednotliv´ ych aktu´ator˚ u, kter´ y leˇz´ı i i v zadan´em intervalu Tmin ≤ Ti ≤ Tmax . Posledn´ı dva ˇcleny na prav´e stranˇe pˇredstavuj´ı
nekonzervativn´ı s´ıly zp˚ usoben´e mechanick´ ym tˇren´ım v jednotliv´ ych kloubech - Fv a Fc jsou diagon´aln´ı matice dimenze n × n a urˇcuj´ı m´ıru visk´ozn´ıho a Coulombovo tˇren´ı. Podobnˇe jako u metody nejhorˇs´ıho pˇr´ıpadu lze pohyb po trajektorii Φ(s) zadan´e v operaˇcn´ım prostoru pˇrev´est do uzlov´ ych souˇradnic pomoc´ı inverzn´ı kinematick´e transformace ˆ Θ = G−1 (Φ(s)) = Φ(s) ˆ ˆ s V (t) ˙ = dΦ s˙ = Φ Θ ds 2ˆ ˆ ¨ = d Φ s˙ 2 + dΦ s¨ Θ 2 ds ds
(3.114) (3.115) (3.116) (3.117)
86
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
Zanedb´ ame-li mechanick´e tˇren´ı v kloubech, lze dosazen´ım tˇechto vztah˚ u do dynamick´eho modelu manipul´ atoru z´ıskat soustavu n diferenci´aln´ıch rovnic druh´eho ˇr´adu [23] ve tvaru: m(s)¨ s + c(s)s˙ 2 + g(s) = T
(3.118)
kde m(s), c(s), g(s) jsou vektory dimenze n × 1 s prvky z´avisl´ ymi na dynamice syst´emu a aktu´ aln´ı konfiguraci stroje vyj´adˇren´e pozic´ı na zadan´e trajektorii: ˆ Φˆs m(s) = M(Φ) T
(3.119)
ˆ Φˆs + M(Φ) ˆ Φˆss c(s) = Φˆs C(Φ)
(3.120)
g(s) = G(Φˆs )
(3.121) (3.122)
Ze vztahu (3.118) jiˇz lze vyhodnotit vliv omezen´ı maxim´aln´ıho momentu jednotliv´ ych aktu´ ator˚ u na pˇr´ıpustn´e hodnoty zrychlen´ı posuvu s¨: Timin ≤ mi (s)¨ s + ci (s)s˙ 2 + gi (s) ≤ Timax
(3.123)
Cel´ y optimalizaˇcn´ı probl´em lze tedy pˇrev´est na u ´lohu ˇr´ızen´ı syst´emu s dvˇema integr´ atory a neline´ arn´ım omezen´ım na stav a ˇr´ızen´ı: s¨ = u(t)
(3.124)
L(s, s) ˙ ≤ u ≤ U (s, s) ˙
(3.125)
kde U, L jsou horn´ı a spodn´ı hranice pˇr´ıpustn´eho intervalu akcelerace po dan´e trajektorii z´ıskan´e pr˚ unikem vˇsech omezen´ı dan´ ych aktu´atory. Jestliˇze je tento interval pr´azdn´ y (L > U ), pak pro aktu´ aln´ı rychlost a pozici neexistuje ˇz´adn´a kombinace moment˚ u/sil v jednotliv´ ych kloubech, kter´ a by zajistila udrˇzen´ı synchronizace s napl´anovanou trajektori´ı. Pro kaˇzd´ y bod trajektorie lze d´ale ze vztahu (3.123) urˇcit maxim´aln´ı rychlost posuvu s, ˙ pˇri kter´em meze pro zrychlen´ı spl´ yvaj´ı v jedinou hodnotu (L = U ). Toto dynamick´e omezen´ı spolu s limity na maxim´aln´ı rychlost jednotliv´ ych aktu´ator˚ u urˇcuj´ı nejvyˇsˇs´ı pˇr´ıpustnou rychlost posuvu. ˇ sen´ım v´ Reˇ yˇse zm´ınˇen´e u ´lohy je ˇr´ızen´ı typu bang-bang, kter´e podle Pontrjaginova principu minima nab´ yv´ a krajn´ıch hodnot pˇr´ıpustn´eho intervalu, pˇr´ıpadnˇe bang-singularbang ve speci´ aln´ıch pˇr´ıpadech tzv. singul´arn´ıch bod˚ u (zde v jin´em kontextu neˇz singu´ l´arn´ı body kinematiky, viz [24]). Ulohou algoritmu pl´anov´an´ı pohybu je pak nalezen´ı
87
defined by L(s, 9) and U(s, B). However, according to Eq. 2.17 the trajectory must lie in the admissible set where L(s, 8 ) 5 U(s, 9) (unshaded area on Fig. 2.1). The boundary of this set is called velocity limit curve and is defined by equation
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
ˇ azek 3.16: Casovˇ e point optim´ aln´ı there pohyb -only trajektorie v rovinˇe s−s, ˙ vlevo jedno pˇrepnut´ı Figure 2.2: Obr´ Obtaining the switching when one. bounds Figure 2.1: Velocity is limit curve the admissible region in the phase plane. Switching bez omezen´ı rychlostipoints posuvu, vpravo s omezen´ ımand a tˇr5'3) emi when pˇrep´ınac´ body [23] occur (points S1, $2, theımi trajectory switches from the acceleration to the
deceleration. urve bounds the subset of the phase plane t o which the trajectory must belong if the lator is t o follow the path without violating actuator constraints. The trajectory is only rep´ınac´ ıch bod˚ u urˇ ıc´According ıch intersect f´ aze akcelerace a decelerace pod´ dan´ e trajektorie. ubˇeh within the admissible t oit.Eq. 2.20 wethe should find ealcan trajectory that liesPr˚ entirely d t o be tangent t opˇ velocity limit curve, itcuj´ cannot Therefore algorithm set, satisfies the tangency constraint at each point and has the property mulated as: pohybu syst´emu (3.125) lze zobrazit ve stavov´e rovinˇe s− s, ˙ coˇz usnadˇ nuje ˇreˇsen´ı, jelikoˇz that the velocity at each point is greater than the velocity on any other admissible trajectory. The switching points obˇeforward okrajov´ e podm´ jsou tˇechto rand adnic´ ıchswitches pevn´ e between (narozd´the ıl odacceleration voln´eho konce Integrate Eq. 2.22 starting atınky the (so,BO) Eq. 2.23 backward willinitial occurvpoint where thesouˇ trajectory and deceleration (points $1, arting a t ( sj, Bj ) t o obtain segments F and B, respectively (Fig. 2.3). If the two intersect S 2 and 5 3 on Fig. 2.1). v pˇr´ıpadˇe ˇcasov´e oblasti). V nejjednoduˇsˇs´ım pˇr´ıpadˇe pohyb obsahuje jednu akceleraˇcn´ı efore intersecting the velocity limit curve there is onlyfirst one the switching pointthere and is weonly found Consider case when one switching point (Fig. 2.2). We know that the ˇ maximum a jednu deceleraˇ cn´ı f´ atrajectory zi (Obr. 3.16 vlevo). Cas pˇrepnut´ıacceleration lze z´ıskat dopˇ a zpˇetnou . Otherwise proceed t o Step 2. starts with the andrednou then switches to maximum deceleration. This suggests that we should integrate the equation ychthe podm´ ınek limit (s0 , s˙curve ˙ ft )(sly a nalezen´ ım pr˚ useˇc´ıku tˇechto dvou tra0 , sf , sa Suppose that theintegrac´ segmentı zFokrajov´ intersects velocity B1). After that is violated so the switching point is somewhere on the interval [so, sl]. oint the Eq. 2.17jektori´ ı ve f´azov´e rovinˇe. Pokud je pˇri integraci v jednom ze smˇer˚ u dosaˇzeno maxim´aln´ı et's pick s E [so,sl] and integrate Eq. 2.23 forward in time starting a t the point on F dan´eforward dynamick´ ymi(Fig. a kinematick´ omezen´ mus´ ı v´ yinitial sledn´aandtrain time starting with theymi initial point ımi, (so, 9,) (the final velocity need not be which correspondsrychlosti t o s. Oneposuvu of the following can happen 2.3): necessarily O), and the equation jektorie obsahovat vˇetˇs´ı poˇ cet curve pˇrepnut´ mezi pˇr´ıpustn´ The trajectory will intersect the velocity limit a t sı = s; (curve 1 onym Fig.ˇr´ızen´ 2.3).ım (Obr. 3.16 vpravo). 5 = L(s,S) (2.23) s was too big, since any admissible acceleration a t s; will force the konstantn´ı nebo obThis means that ´ Uloha se komplikuje pokud omezen´ ı na maxim´ a ln´ ı moment nejsou backward in time starting with the final state (sf,Sf). The switching point is given by the manipulator t o leave the path. intersection of thea two last maxim´aln´ı rychlosti nen´ı souvisl´ (m˚ utrajectories. ˇze nastat pˇri uvaˇzov´an´ı tˇren´ı v kloubech). The trajectory intersects the horizontal line S = B j (curve 2 on Fig. 2.3). This means The algorithm that gives the switching points in the general case also uses the idea of that s was too small: could s forfunkˇ some > 0 so that the trajectory Bylo dok´awe z´ano, ˇze increase pokud jsou cn´ıE z´ aand vislosti kloubov´ ch souˇ radnic a jejich derivac´ ı integrating forward backward in ytime with the maximum acceleration or the maximum starting on F a t s 6 would still not intersect the velocity limit curve and which deceleration. Velocity limit curve determines when to switch. As already said, the velocity na give parametru trajektorie pot oˇc´ aEq. stech analytick´e, pak existuje koneˇcn´ y poˇcet pˇrepnut´ı 2.20. would obviously shorter time according
+
a ispˇrtangent i realizaci pohybu vˇzdy aalespoˇ jeden atorreaches v saturaci [25]. V´ ypoˇcetnˇe The trajectory t o the velocitybude limit curve t somenpoint (s2,aktu´ i z ) and the horizontal line B = if afterwards (curve 3 on Fig. 2.3). Then s is a switching efektivn´ı algoritmus pro z´ısk´ an´ı pˇrep´ınac´ıch ˇcas˚ u je uveden v pr´aci [24]. point and sz will be a new switching point. Nev´ yhodou uveden´eho pˇr´ıstupu je nespojit´ y pr˚ ubˇeh akcelerace a momentu jednotli-
We must therefore find s for which the third case will occur. Then the point of tangency v´ ych u. To pˇrin´ aˇs´ıthe probl´ emy se repeated sledov´an´ napl´ anovan´e trajektorie a buzen´ım s2, B2) can be taken as aktu´ a newator˚ initial point and algorithm aım t Step 1.
mechanick´ ych vibrac´ ı. Ztr´ ac´ı se time t´ım rovnˇ ˇz v´ yhoda ı hladk´ useful simplification that reduces the computation is t o erewrite Eq. pouˇ 2.22zit´ and 2.23 ych trajektori´ı (spline funkce), protoˇze nespojitost vyˇsˇs´ıch derivac´ı je do pohybu zan´aˇsena vrstvou pro ˇr´ı-
88
3.3 Koordinovan´ y pohyb ve v´ıce os´ ach
zen´ı posuvu. Dalˇs´ım probl´emem je, ˇze pˇri ˇcasovˇe optim´aln´ım pohybu je vˇzdy alespoˇ n jeden aktu´ ator v saturaci a p˚ usob´ı maxim´aln´ım momentem. V tomto okamˇziku jiˇz nezb´ yvaj´ı ˇz´ adn´e zdroje pro pˇr´ıpadn´e zpˇetnovazebn´ı kompenzace p˚ usob´ıc´ıch poruch, nelinearit v mechanick´e ˇc´ asti syst´emu a nepˇresnost´ı v dynamick´em modelu pouˇzit´em pro pl´anov´ an´ı trajektorie. V´ yˇse popsan´ y pˇr´ıstup zaloˇzen´ y na dynamick´em modelu lze rozˇs´ıˇrit pro generov´an´ı hladk´ ych trajektori´ı respektuj´ıc´ıch praktick´e poˇzadavky na implementaci v re´aln´ ych zaˇr´ızen´ıch. Ekvivalentem omezen´ı jerku posuvu zav´adˇen´eho u metod pracuj´ıc´ıch s kinematick´ ym modelem je v tomto pˇr´ıpadˇe stanoven´ı limit˚ u pro rychlost zmˇeny momentu i i ˙ ˙ ˙ aktu´ ator˚ u Tmin ≤ Ti ≤ Tmax . Pro vyhodnocen´ı vlivu tˇechto omezen´ı na dynamiku pohybu je potˇreba z´ıskat model tˇret´ıho ˇr´adu derivov´an´ım soustavy (3.126) podle ˇcasu, ˇc´ımˇz pˇri zanedb´ an´ı tˇren´ı dost´ av´ame: ... ˙ ¨ +Θ ¨ T C(Θ)Θ ˙ +Θ ˙ T C(Θ) ˙ ˙ +Θ ˙ T C(Θ)Θ ¨ + G(Θ) ˙ ˙ (3.126) M(Θ)Θ + M(Θ) Θ Θ =T Analogicky k pˇredchoz´ımu pˇr´ıpadu lze transformovat trajektorii pohybu z operaˇcn´ıho do uzlov´eho prostoru, vyj´ adˇrit uzlov´e souˇradnice a jejich derivace a dosazen´ım do dynamick´eho modelu pˇrej´ıt na soustavu n rovnic tˇret´ıho ˇr´adu, ze kter´ ych vypl´ yvaj´ı dynamick´ a omezen´ı na pr˚ ubˇeh posuvu ve formˇe: ˙ imin ≤ ai (s)... ˙ imax s + bi (s)s¨ T ˙ s + ci (s)s˙ 3 + di (s)s˙ ≤ T
(3.127)
kde a(s), b(s), c(s), d(s) jsou vektory n×1 z´avisl´e na dynamice stroje a tvaru trajektorie pohybu. Rozˇs´ıˇren´ı modelu vˇsak dramaticky zvyˇsuje sloˇzitost cel´e u ´lohy a jiˇz omezen´ı (3.127) je nutn´e vyhodnocovat numericky. Probl´emem je rovnˇeˇz nalezen´ı algoritmu analogick´emu k pˇredchoz´ıho pˇr´ıpadu ˇr´ızen´ı v rovinˇe s− s˙ s pˇrep´ın´an´ım hodnot akcelerace posuvu s¨. V u ´loze s omezen´ım na derivaci momentu aktu´ator˚ u je nutn´e pˇrej´ıt do trojrozmˇern´eho f´azov´eho prostoru souˇradnic s − s˙ − s¨, ve kter´em nelze jednoznaˇcnˇe urˇcit spr´avn´ y smˇer postupu mezi okrajov´ ymi body. N´avod ke z´ısk´an´ı ˇreˇsen´ı ned´av´a ani teorie optim´aln´ıho ˇr´ızen´ı, protoˇze pro syst´em s nez´avisl´ ym neline´arn´ım omezen´ım vstupu a stavu nemus´ı t-optim´ aln´ı ˇr´ızen´ı nutnˇe nab´ yvat mezn´ıch pˇr´ıpustn´ ych hodnot. Z literatury jsou zn´amy pouze metody zaloˇzen´e na numerick´e optimalizaci, kter´e hledaj´ı vhodnou kˇrivku v rovinˇe s− s˙ pˇri splnˇen´ı vˇsech dynamick´ ych a kinematick´ ych omezen´ı (viz napˇr. [26, 27, 72] s pouˇzit´ım kubick´ ych nebo kvintick´ ych spline funkc´ı).
89
´ ´ 3. GENEROVAN I TRAJEKTORIE POHYBU
Shrnut´ı C´ılem n´avrhu gener´ atoru trajektorie je z´ısk´an´ı ˇcasov´eho pr˚ ubˇehu poˇzadovan´eho po´ hybu pracovn´ıho mechanismu stroje. Uloha b´ yv´a rozdˇelena do dvou krok˚ u. Prvn´ım je specifikace dan´e trajektorie v prostoru volbou vhodn´e interpolaˇcn´ı funkce. Vedle dˇr´ıve v´ yhradnˇe uˇz´ıvan´ ych line´ arn´ıch a kruhov´ ych interpolac´ı se dnes prosazuj´ı polynomi´aln´ı funkce vyˇsˇs´ıch ˇr´ ad˚ u ve formˇe spline kˇrivek. Zav´adˇen´ı tˇechto hladk´ ych interpolac´ı pˇrin´aˇs´ı zlepˇsen´ı kvality ˇr´ızen´ı pohybu, klade ovˇsem zv´ yˇsen´e n´aroky na n´avrh pokroˇcil´ ych algoritm˚ u gener´atoru. Dalˇs´ı u ´lohou je potom urˇcen´ı ˇcasov´eho pr˚ ubˇehu posuvu po jiˇz zadan´e trajektorii v prostoru. V´ ysledn´ y pohyb mus´ı respektovat vˇsechna fyzik´aln´ı omezen´ı dan´a mechanickou konstrukc´ı stroje a technologick´ ymi limity konkr´etn´ı aplikace. Souˇcasnˇe b´ yv´a minimalizov´ ano vhodn´e krit´erium kvality ˇr´ızen´ı, nejˇcastˇeji ve formˇe hled´an´ı ˇcasovˇe optim´aln´ıho ˇreˇsen´ı. Pro sloˇzitˇejˇs´ı v´ıceos´e syst´emy vede tato u ´loha na neline´arn´ı optimalizaˇcn´ı probl´emy s neline´ arn´ımi omezen´ımi, kter´e jsou natolik komplikovan´e, ˇze nelze ˇ odvodit analytick´e ˇreˇsen´ı. Casto je nutn´e pˇristoupit k pouˇzit´ı nejr˚ uznˇejˇs´ıch heuristik nebo numerick´ ych pˇr´ıstup˚ u.
90
4
´ Uvod do robotiky 4.1
Robotika jako v´ yznamn´ y obor mechatroniky
Ned´ılnou souˇc´ ast´ı mechatroniky je bezpochyby i obor zvan´ y robotika, neboli nauka o robotech. Robotika je dnes velmi rozˇs´ıˇrenou vˇedn´ı discipl´ınou, nebot’ se stoupaj´ıc´ımi moˇznostmi v´ ypoˇcetn´ı techniky a schopnost´ı lid´ı vyr´abˇet velmi pˇresn´e a relativnˇe dokonal´e mechanick´e konstrukce, z´ısk´av´a robotika sv´e neodmysliteln´e m´ısto prakticky ve vˇsech oborech lidsk´e ˇcinnosti. Robotika v sobˇe ukr´ yv´a v podstatˇe cel´ y proces ”zrozen´ı ”robot˚ u od n´ avrhu jejich konstrukce, n´avrhu vhodn´eho algoritmu ˇr´ızen´ı, prov´adˇen´ı simulac´ı, vlastn´ı realizace, aˇz po fin´aln´ı uveden´ı do provozu. Roboty lze ze sv´e podstaty dˇelit na dvˇe z´ akladn´ı skupiny. Prvn´ı z nich jsou tzv. manipul´atory. P˚ uvodn´ım c´ılem konstrukce manipul´ator˚ u bylo zhotovit nejprve mechanick´ a, pozdˇeji elektromechanick´a, zaˇr´ızen´ı, kter´a budou schopn´ a zesilovat ˇci zpˇresˇ novat pr´ aci ˇclovˇeka. Do t´eto kategorie spadaj´ı napˇr´ıklad r˚ uzn´e p´akov´e a kladkov´e mechanismy, bagry, nakladaˇce, atd. Z postupuj´ıc´ım v´ yvojem vˇsak manipul´atory z´ısk´ avaly sv´e m´ısto v cel´e ˇradˇe pr˚ umyslov´ ych aplikac´ı jako programovateln´e, plnˇe automatick´e, pˇresn´e a rychl´e zaˇr´ızen´ı, kter´e ne´ unavnˇe a bezchybnˇe nahrazovaly pr´ aci ˇclovˇeka ve skladech (zakl´ adac´ı, pˇrerovn´avac´ı, tˇr´ıd´ıc´ı manipul´atory), na v´ yrobn´ıch link´ach (manipul´ atory typu pick and place), v pr˚ umyslov´ ych procesech (pˇresn´e svaˇrov´an´ı, brouˇsen´ı, lakov´ an´ı), pˇri pr´ aci v nebezpeˇcn´ ych prostˇred´ıch (mobiln´ı roboty pro pr˚ uzkum prostor s nebezpeˇc´ım v´ ybuchu ˇci otravy, mobiln´ı roboty pro pr˚ uzkum vesm´ırn´ ych tˇeles ˇci roboty vyvinut´e speci´ alnˇe pro arm´adn´ı u ´ˇcely), manipul´atory vyuˇz´ıvan´e v l´ekaˇrstv´ı (pˇresn´e laparoskopick´e chirurgick´e operaˇcn´ı roboty, napˇr. operaˇcn´ı robotick´ y syst´em
91
´ 4. UVOD DO ROBOTIKY
Da Vinci, s jehoˇz pomoc´ı chirurgov´e prov´adˇej´ı dlouhotrvaj´ıc´ı sloˇzit´e z´akroky v tˇele pacienta, pˇriˇcemˇz zat´ıˇzen´ım pacienta, stejnˇe tak jako pˇr´ıpadn´e chyby zp˚ usoben´e u ´navou operat´er˚ u, jsou minimalizov´ any. Operaˇcn´ı syst´em Da Vinci vlastn´ı od roku 2005 i praˇzsk´a nemocnice Na Homolce.) Do druh´e velk´e skupiny patˇr´ı tzv. humanoidn´ı roboty, neboli roboty podobn´e ˇclovˇeku nejen sv´ ym vzhledem, ale i projevem urˇcit´eho stupnˇe inteligence. V´ yvoj takov´ ychto ”inteligentn´ıch”a vysoce autonomn´ıch stroj˚ u je dnes vn´ım´an jako ˇspiˇcka a jeden z hlavn´ıch c´ılu v oblasti robotiky. Nicm´enˇe, i pˇres ˇradu zdaˇril´ ych prototyp˚ u tˇechto robot˚ u (humanoidn´ı robot PETMAN pro testov´an´ı vojensk´ ych protichemick´ ych oblek˚ u, ˇci plnˇe autonomn´ı 4-noh´ y robot BigDog, oba z Bostonsk´e university [75]) je c´ıl zkonstruovat robota s inteligenc´ı a pohybovou dispozic´ı srovnatelnou lidsk´e st´ale jeˇstˇe velmi daleko.
4.2
Robotick´ e manipul´ atory
Vrat’me se vˇsak k prvn´ı kategorii robot˚ u, nebot’ tato pr´ace se zab´ yv´a pr´avˇe z´akladn´ımi probl´emy v oblasti manipul´ ator˚ u. Snaˇz´ı se tak pˇredloˇzit struˇcn´ y a ucelen´ y n´ahled na moˇznosti jejich anal´ yzy a synt´ezy, kter´e tvoˇr´ı neodmysliteln´ y z´aklad pro realizaci vyˇsˇs´ıch forem robot˚ u. Ponechme tak plnˇe autonomn´ı humanoidn´ı roboty stranou zejm´ena proto, ˇze tato ˇc´ast robotiky zasahuje velkou mˇerou pˇredevˇs´ım do oblasti umˇel´e inteligence (algoritmy strojov´eho vn´ım´ an´ı a porozumˇen´ı, autonomn´ı rozhodov´an´ı, atd.), kter´a nen´ı n´apln´ı naˇseho zkoum´ an´ı. Pro usnadnˇen´ı dalˇs´ıho porozumˇen´ı textu, vymezme nejprve nˇekter´e fundament´aln´ı pojmy z oblasti robotiky: • Poˇcet stupˇ n˚ u volnosti (DoF - Degrees of Freedom) Minim´ aln´ı poˇcet parametr˚ u (rotace, translace), kter´ y jednoznaˇcnˇe popisuje polohu bodu nebo tˇelesa v rovinˇe ˇci prostoru. Napˇr.: Bod v rovinˇe m´a 2 DoF, v prostoru 3 DoF. Tuh´e tˇeleso m´ a v rovinˇe 3 DoF, v prostoru 6 DoF. • Obecn´ a poloha tˇelesa v prostoru je urˇcena jeho translac´ı a rotac´ı. Zat´ımco, translace tˇelesa je intuitivnˇe zˇrejm´ a (souˇradnice x,y,z libovoln´eho bodu tˇelesa), nejˇcastˇejˇs´ı reprezentace rotace tˇelesa jsou realizov´any pomoc´ı matice rotace, Eulerovsk´ ych u ´hl˚ u, osy rotace a u ´hlu natoˇcen´ı kolem t´eto osy atd. Podrobnˇeji jsou moˇznosti popisu obecn´e polohy tˇelesa v prostoru diskutov´any v Kapitole 4.5.1
92
4.2 Robotick´ e manipul´ atory
• Z´ akladna manipul´ atoru Pevn´ a (nepohybliv´ a) ˇc´ ast manipul´atoru - na kter´e je definov´an pevn´ y ”svˇetov´ y”souˇradn´ y syst´em. • Koncov´ y efektor manipul´atoru Posledn´ı ˇc´ ast resp. rameno manipul´atoru, ke kter´emu jsou obvykle pˇripevˇ nov´any r˚ uzn´e pracovn´ı n´ astroje a jehoˇz poloha je ˇr´ızena na poˇzadovanou hodnotu (trajektorii). Poloha koncov´eho efektoru bude d´ale reprezentov´ana zat´ım nespecifikovan´ ymi zobecnˇen´ ymi souˇradnicemi X. • Klouby manipul´ atoru Jednotliv´e typy kloub˚ u se od sebe liˇs´ı poˇctem a typem stupˇ n˚ u volnosti, napˇr. P-kloub (Prismatic) - posuvn´ y kloub pˇrid´avaj´ıc´ı jeden translaˇcn´ı stupeˇ n volnosti, R-kloub (Revolute) - rotaˇcn´ı kloub pˇrid´avaj´ıc´ı jeden rotaˇcn´ı stupeˇ n volnosti, Ukloub (Universal) - univerz´aln´ı (Kardan˚ uv) kloub pˇrid´avaj´ıc´ı 2 rotaˇcn´ı stupnˇe volnosti, S-kloub (Spherical) - sf´erick´ y kloub pˇrid´avaj´ıc´ı vˇsechny 3 rotaˇcn´ı stupnˇe volnosti. Nejˇcastˇeji pouˇz´ıvan´e typy kloub˚ u jsou zn´azornˇeny na Obr´azku 4.1
Obr´ azek 4.1: Pouˇz´ıvan´e typy kloub˚ u (zleva P, R, U, S)
• Kinematick´ y ˇretˇezec Skl´ ad´ a se ze soustavy kinematick´ ych dvojic, kter´e jsou definov´any jako spojen´ı dvou pevn´ ych tˇeles (tzv. ramen) danou vazbou. Vazbami rozum´ıme klouby, kter´e omezuj´ı (dle dan´eho poˇctu stupˇ n˚ u volnosti) vz´ajemn´ y pohyb ramen. Kinematick´e ˇretˇezce manipul´ ator˚ u b´ yvaj´ı ˇcasto oznaˇcov´any jako posloupnost kloub˚ u, napˇr.: RRR, RPR atd., kde podtrˇzen´ı oznaˇcuje aktivn´ı kloub (aktu´ator) manipul´atoru. • Aktu´ atory manipul´ atoru Pohony, kter´e zajiˇst’uj´ı pohyb manipul´atoru. Typick´e pohony manipul´atoru pˇredstavuj´ı rotaˇcn´ı pohony (rotaˇcn´ı elektromotory) a line´arn´ı (pˇr´ımoˇcar´e) pohony
93
´ 4. UVOD DO ROBOTIKY
(elektrohydraulick´e v´ alce, line´ arn´ı elektromotory). Klouby, kter´e reprezentuj´ı aktu´atory, naz´ yv´ ame aktivn´ımi klouby a jejich polohu popisuj´ı tzv. vektorem aktivn´ıch kloubov´ ych souˇradnic Θ. V robotice se t´emˇeˇr v´ yhradnˇe jako aktivn´ı klouby vyuˇz´ıvaj´ı 1 DoF klouby typu P a R. • Domovsk´ a poloha manipul´ atoru Poloha koncov´eho efektoru manipul´atoru, pˇri kter´e jsou jeho aktivn´ı kloubov´e souˇradnice Θ nastaveny takov´ ym zp˚ usobem, ˇze koncov´ y efektor manipul´ atoru zauj´ım´a v´ ychoz´ı (domovskou) polohu. • Pracovn´ı prostor manipul´ atoru Mnoˇzina vˇsech pozic koncov´eho efektoru manipul´atoru, kter´e mohou b´ yt dosaˇzeny pro dan´e omezuj´ıc´ı podm´ınky kladen´e na manipul´ator (maxim´aln´ı/minim´aln´ı vysunut´ı/natoˇcen´ı aktu´ ator˚ u, omezen´ı na pohyb pasivn´ıch kloub˚ u, omezen´ı zabran ˇuj´ıc´ı pˇrekˇr´ıˇzen´ı ˇci sr´ aˇzk´ am ramen manipul´atoru, omezen´ı na kvalitu pracovn´ıho prostoru - napˇr. z´ avislosti mezi maxim´aln´ı rychlost´ı a z´atˇeˇz´ı koncov´eho efektoru vzhledem k dan´ ym vlastnostem aktu´ator˚ u, tuhost mechanick´e konstrukce atd.). • Pˇr´ım´a kinematick´ au ´loha Reprezentovan´ a obecnˇe neline´ arn´ı transformac´ı X = G(Θ). Jedn´a se tedy o probl´em nalezen´ı zobecnˇen´ ych souˇradnic X pro dan´e hodnoty souˇradnic kloubov´ ych Θ. Vzhledem k tomu, ˇze v praxi obvykle nen´ı moˇzn´e pˇr´ımo mˇeˇrit polohu koncov´eho efektoru (ˇcasto komplikovan´e ˇci u ´plnˇe nemoˇzn´e um´ıstˇen´ı dostateˇcnˇe pˇresn´ ych senzor˚ u), pˇr´ım´ a kinematick´ au ´loha umoˇzn ˇuje stanovit polohu koncov´eho efektoru prostˇrednictv´ım u ´daj˚ u o poloze jednotliv´ ych aktu´ator˚ u (relativnˇe jednoduch´e mˇeˇren´ı polohy rotaˇcn´ıch ˇci translaˇcn´ıch pohon˚ u). • Zpˇetn´a kinematick´ au ´loha Reprezentovan´ a obecnˇe inverzn´ı neline´arn´ı transformac´ı Θ = G−1 (X). Jedn´a se tedy o probl´em nalezen´ı kloubov´ ych souˇradnic Θ pro dan´e hodnoty souˇradnic zobecnˇen´ ych X. Zpˇetn´ a kinematick´a u ´loha tvoˇr´ı jednu z nejd˚ uleˇzitˇejˇs´ıch transformac´ı v robotice, nebot’ problematika polohov´an´ı koncov´eho efektoru je v naprost´e vˇetˇsinˇe formulov´ ana v prostoru zobecnˇen´ ych souˇradnic, jin´ ymi slovy poˇzadovan´e trajektorie pohybu koncov´eho efektoru mus´ı b´ yt nejprve transformov´ana na poˇzadovanou trajektorii v prostoru kloubov´ ych souˇradnic (polohov´an´ı jednotliv´ ych aktu´ator˚ u).
94
4.2 Robotick´ e manipul´ atory
• Pˇresnost a opakovatelnost Pˇresnost manipul´ atoru je d´ana odchylkou poˇzadovan´e polohy a skuteˇcn´e polohy (z referenˇcn´ıho/kalibraˇcn´ıho mˇeˇridla) koncov´eho efektoru. Opakovatelnost manipul´ atoru pak lze ch´ apat jako maxim´aln´ı rozd´ıl mezi skuteˇcn´ ymi polohami koncov´eho efektoru z´ıskan´ ymi jeho pˇresunem do jedn´e poˇzadovan´e polohy z r˚ uzn´ ych poloh poˇc´ ateˇcn´ıch. • Redundantn´ı manipul´ atory Poˇcet nez´ avisl´ ych aktivn´ıch kloubov´ ych souˇradnic je vˇetˇs´ı neˇz poˇcet DoF koncov´eho efektoru manipul´ atoru. Vyuˇzit´ı vlastnosti kinematick´e redundance je diskutov´ ano v Kapitole 4.9. Robotick´e manipul´ atory je dle kinematick´e konstrukce (uspoˇr´ad´an´ı kinematick´ ych ˇretˇezc˚ u) moˇzn´e dˇelit na dva z´ akladn´ı typy:
S´ eriov´ e manipul´ atory Jejich z´ aklad tvoˇr´ı tzv. otevˇren´ y kinematick´ y ˇretˇezec, kter´ y lze popsat acyklick´ ym grafem, kde uzly grafu reprezentuj´ı ramena manipul´atoru a hrany grafu reprezentuj´ı jednotliv´e klouby. Jin´ ymi slovy, kaˇzd´e rameno manipul´atoru je spojeno klouby pr´avˇe se dvˇema dalˇs´ımi rameny s v´ yjimkou ramen typu z´akladna a koncov´ y efektor. Tyto maj´ı s ostatn´ımi rameny pouze jedinou vazbu. S´eriov´e manipul´atory patˇr´ı dnes k nejrozˇs´ıˇrenˇejˇs´ım mechanism˚ um robotiky, jejichˇz z´aklad byl poloˇzen Georgem Devolem (zamˇestnancem firmy General Motors) v roce 1961, kdy byl pˇredstaven prvn´ı pr˚ umyslov´ y manipul´ ator s n´ azvem Unimate, viz Obr´azek 4.2. Pro sv´e vlastnosti jsou dnes s´eriov´e manipul´ atory vyuˇz´ıv´ any v pr˚ umyslov´ ych aplikac´ıch jako svaˇrov´an´ı, obr´abˇen´ı, lakov´an´ı, skl´ad´ an´ı, paletizace a kompletov´an´ı. Obecnˇe m˚ uˇzeme ˇr´ıci, ˇze pro s´eriov´e manipul´atory lze nal´ezt vˇzdy jednoznaˇcn´e analytick´e ˇreˇsen´ı pˇr´ım´e kinematick´e u ´lohy. Avˇsak vzhledem k faktu, ˇze rovnice X = G(Θ) je obecnˇe sloˇzitou neline´ arn´ı rovnic´ı, jej´ı ˇreˇsen´ı Θ = G−1 (X), a tedy ˇreˇsen´ı zpˇetn´e kinematick´e u ´lohy, nemus´ı b´ yt moˇzn´e nal´ezt analyticky a m˚ uˇze obsahovat celou ˇradu ˇreˇsen´ı. V nˇekter´ ych speci´ aln´ıch pˇr´ıpadech, kter´e budou diskutov´any d´ale v Kapitole 4.6.2 je pˇresto moˇzn´e vˇsechna ˇreˇsen´ı zpˇetn´e kinematick´e u ´lohy analyticky naj´ıt.
95
´ 4. UVOD DO ROBOTIKY
Obr´ azek 4.2: Prvn´ı s´eriov´ y pr˚ umyslov´ y manipul´ator Unimate
Paraleln´ı manipul´ atory Jejich z´aklad tvoˇr´ı tzv. uzavˇren´ y kinematick´ y ˇretˇezec, kter´ y lze popsat cyklick´ ym grafem a vznik´ a uzavˇren´ım otevˇren´eho kinematick´eho ˇretˇezce. Mohli bychom tedy ˇr´ıci, ˇze koncov´ y efektor je spojen se z´ akladnou manipul´atoru dvˇema ˇci v´ıce otevˇren´ ymi kinematick´ ymi ˇretˇezci, v podstatˇe s´eriov´ ymi manipul´atory. Paraleln´ı manipul´atory se dost´avaj´ı do pr˚ umyslov´eho povˇedom´ı aˇz v nˇekolika posledn´ıch letech, nebot’, d´ıky sv´e sloˇzitˇejˇs´ı mechanick´e konstrukci a potenci´alnˇe tak vˇetˇs´ım n´arok˚ um na jejich n´avrh a ˇr´ızen´ı, technickou veˇrejnost sv´ ym zp˚ usobem odrazovaly. To, co vˇsak bylo pro pr˚ umysl ”zbyteˇcnˇe”sloˇzit´e, se brzy stalo v´ yzvou pro akademiky. Pr´avˇe d´ıky nim dnes paraleln´ı manipul´atory smˇele konkuruj´ı sv´ ym s´eriov´ ym protˇejˇsk˚ um a postupnˇe tak smaz´avaj´ı hlubokou propast mezi technologicky-pr˚ umyslov´ ym povˇedom´ım a akademickou sf´erou. Prvotn´ı zm´ınky o paraleln´ıch kinematick´ ych struktur´ach se objevuj´ı mezi technickou veˇrejnost´ı jiˇz v prvn´ı polovinˇe dvac´ at´eho stolet´ı. Pˇresto, ˇze nem˚ uˇzeme s jistotou ˇr´ıci, ˇze se jedn´a o skuteˇcnˇe prvn´ı mechanismus s paraleln´ı architekturou, a dokonce ani zda-li byl skuteˇcnˇe nˇekdy postaven, v roce 1931 si nechal James Gwinnett patentovat mobiln´ı platformu pro z´ abavn´ı pr˚ umysl, viz Obr´azek 4.3.
96
4.2 Robotick´ e manipul´ atory
Obr´ azek 4.3: Pravdˇepodobnˇe prvn´ı paraleln´ı manipul´ator Jamese Gwinnetta
Dnes snad nejzn´ amˇejˇs´ı paraleln´ı kinematickou architekturou je tzv. Stewartova platforma. Jej´ı objev je vˇsak bezpochyby pˇripisov´an Ericu Goughovi, automobilov´emu inˇzen´ yrovi firmy Dunlop Rubber Co., Birmingham, England. Ten v roce 1954 pˇredstavil paraleln´ı platformu pro testov´an´ı pneumatik letadlov´ ych podvozk˚ u pˇri variabiln´ım zat´ıˇzen´ı, viz Obr´ azek 4.4(a). O nˇekolik let pozdˇeji, v roce 1965, D. Stewart na konferenci UK Institution of Mechanical Engineers prezentoval sv˚ uj vyn´alez paraleln´ıho leteck´eho simul´ atoru, viz Obr´ azek 4.4(b). Pˇresto, ˇze Stewart˚ uv vyn´alez nebyl pˇr´ıliˇs podobn´ y tomu Goughovu, je pr´ avˇe Goughova platforma dnes paradoxnˇe naz´ yv´ana platformou Stewartovou, nˇekdy pak jako Gough-Stewartova platforma. V´ıce o historii paraleln´ıch manipul´ ator˚ u lze nal´ezt napˇr. v [76]. Pro sv´e vlastnosti se paraleln´ı manipul´atory pouˇz´ıvaj´ı zejm´ena v aplikac´ıch leteck´ ych simul´ator˚ u, aktivnˇe tlumen´ ych anti-vibraˇcn´ıch ploˇsin, pˇri velmi pˇresn´ ych a rychl´ ych aplikac´ıch typu ”pick and place”, pro testovac´ı standy v automobilov´em pr˚ umyslu atd.
97
´ 4. UVOD DO ROBOTIKY
(a) P˚ uvodn´ı Goughova platforma pro testov´ an´ı pneumatik
(b) P˚ uvodn´ı Stewartova platforma pro leteck´ y simul´ ator
Obr´ azek 4.4: Goughova-Stewartova platforma (1954) a Stewartova platforma (1965)
Pro paraleln´ı manipul´ atory obecnˇe plat´ı, ˇze naopak inverzn´ı kinematick´a u ´loha je vˇetˇsinou ˇreˇsiteln´ a analyticky, nebot’ lze uk´azat, viz Kapitola 4.7, ˇze poloha bodu, kter´ ym je kaˇzd´ y kinematick´ y ˇretˇezec pˇripojen ke koncov´emu efektoru lze jednoznaˇcnˇe analyticky z´ıskat pro kaˇzdou danou polohu X koncov´eho efektoru. Z pˇredchoz´ıho plyne, ˇze kloubov´e souˇradnice Θ kaˇzd´eho kinematick´eho ˇretˇezce mohou b´ yt nalezeny ˇreˇsen´ım zpˇetn´e kinematick´e u ´lohy pro s´eriov´e manipul´atory. Vzhledem k tomu, ˇze jednotliv´e kinematick´e ˇretˇezce paraleln´ıch manipul´ator˚ u b´ yvaj´ı zpravidla velmi jednoduch´e, je u ´loha Θ = G−1 (X) ˇreˇsiteln´ a analyticky. Naopak, kv˚ uli nelinearitˇe a sloˇzitosti funkce Θ = G−1 (X) nen´ı pro paraleln´ı manipul´atory obecnˇe moˇzn´e analyticky ˇreˇsit pˇr´ımou kinematickou u ´lohu X = G(Θ) (s v´ yjimkou velmi jednoduch´ ych mechanick´ ych konstrukc´ı). Tabulka 4.1 shrnuje z´ akladn´ı rozd´ıly mezi s´eriov´ ymi a paraleln´ımi manipul´atory.
98
4.3 C´ıle pr´ ace
Sériové manipulátory + jednoduchá mechanická architektura Zpravidla jednodušší řešení přímé a inverzní kinematiky, přímá kinematika lze vždy řešit analyticky, inverzní kinematika obecně nelze řešit analyticky. - užitné zatížení manipulátoru Všechna ramena manipulátoru jsou zatěžována výhradně na ohyb a každé z ramen musí být dimenzováno tak, aby udrželo celou váhu břemene => manipulátor musí být dostatečně robustní (vyšší hmotnost, horší dynamické vlastnosti, poddajnost manipulátoru) - přesnost a opakovatelnost manipulátoru Nasčítávání chyb vzniklých při polohování jednotlivých ramen nejčastěji v důsledku jejich průhybů, případně chyb snímačů v jednotlivých aktuátorech => ztráta přesnosti a opakovatelnosti. - umístění aktuátorů Aktuátory musí být umístěny v každém kloubu manipulátoru (aktuátory se pohybují společně s manipulátorem) => horší dynamické vlastnosti, větší robustnost manipulátoru, nutnost vézt kabeláž v celé mechanické konstrukci. + pracovní prostor Relativně velký pracovní prostor.
Paralelní manipulátory - složitější mechanická architektura Řešení přímé a inverzní kinematiky může být obtížnější, inverzní kinematika lze většinou řešit analyticky (s výjimkou složitých kinematických architektur), přímá kinematika obecně nelze řešit analyticky. + užitné zatížení manipulátoru Síla potřebná k udržení břemene je rozdělena mezi jednotlivé kinematické řetězce (koncový efektor manipulátoru je podepírán ve více bodech) => manipulátor nemusí být tak robustní (nižší hmotnost, lepší dynamické vlastnosti, vyšší tuhost manipulátoru) + přesnost a opakovatelnost manipulátoru Vzhledem k odlišné mechanické konstrukci jsou chyby vzniklé při polohování jednotlivých ramen průměrovány => dosažení větší přesnosti a opakovatelnosti. + umístění aktuátorů Aktuátory mohou být umístěné na základně manipulátoru (a to v mnoha případech i napevno) => lepší dynamické vlastnosti, lehčí konstrukce manipulátoru, možnost prostorově oddělit aktuátor (v případě že manipulátor musí pracovat v agresivním, vybušném či jinak nestandardním prostředí) - pracovní prostor Pracovní prostor je z důvodu složitější mechanické konstrukce více komplikovaný (neregulární tvar s řadou výdutí a prohlubní).
Tabulka 4.1: Porovn´ an´ı z´ akladn´ıch vlastnost´ı paraleln´ıch a s´eriov´ ych manipul´ator˚ u
4.3
C´ıle pr´ ace
Vzhledem k dneˇsn´ımu ˇsirok´emu uplatnˇen´ı robotick´ ych manipul´ator˚ u v pr˚ umyslov´ ych aplikac´ı, vyvst´ av´ a poˇzadavek na moˇznosti co nejjednoduˇsˇs´ı synt´ezy a anal´ yzy navrˇzen´ ych robotick´ ych architektur. Je pˇrirozenˇe poˇzadov´ano, aby navrˇzen´e manipul´atoru co nejl´epe vykon´ avaly poˇzadovan´e u ´lohy, a to pˇredevˇs´ım pˇresnˇe, rychle a spolehlivˇe. V pr˚ umyslu se setk´ av´ ame s manipul´atory, kter´e se jsou jiˇz postupem ˇcasu odzkouˇseny, jejich chov´ an´ı je dobˇre zn´ amo a mnohdy jsou pro svou ˇcinnost optimalizov´any. Do takov´e
99
´ 4. UVOD DO ROBOTIKY
skupiny patˇr´ı pˇredevˇs´ım cel´ a ˇrada s´eriov´ ych manipul´ator˚ u typu SCARA, antropomorfn´ıho manipul´ atory ˇci jednoduch´e port´ alov´e (pravo´ uhl´e) manipul´atory, viz Obr´azek 4.5, kter´e m˚ uˇzeme nal´ezt v nab´ıdkov´ ych listech pˇredn´ıch svˇetov´ ych v´ yrobc˚ u manipul´ator˚ u jako ABB, Fanuc, Kuka, Motoman, Epson atd.
(a) Scara manipul´ ator (Kuka)
(b) Antropomorfn´ı manipul´ ator (Abb)
(c) Port´ alov´ y (Epson)
manipul´ ator
Obr´ azek 4.5: Nejzn´ amˇejˇs´ı typy pr˚ umyslov´ ych manipul´ator˚ u
Je nutn´e si ovˇsem uvˇedomit, ˇze manipul´atory jako takov´e pˇredstavuj´ı mnohdy sloˇzit´ y mechanick´ y syst´em, tzv. multibody system, jehoˇz synt´eza a anal´ yza vyˇzaduje relativnˇe obs´ahl´e znalosti z oboru mechaniky a matematiky. Odvozen´ı kinematick´ ych a dynamick´ ych z´ avislost´ı a charakteristik manipul´ator˚ u, stejnˇe jako jejich optimalizace za u ´ˇcelem vykon´ av´ an´ı urˇcit´e u ´lohy, tak b´ yv´a ˇcasto velmi zdlouhav´e. C´ılem t´eto pr´ ace je proto upozornit na z´akladn´ı probl´emy pˇri n´avrhu, anal´ yze a ˇr´ızen´ı robotick´ ych manipul´ ator˚ u, kter´e by mˇely st´at z´akladem pro v´ yvoj nov´eho softwarov´eho n´astroje vyv´ıjen´eho na KKY, kter´ y by tyto z´akladn´ı probl´emy robotiky umˇel s´am ˇreˇsit a poskytnout tak sv´emu uˇzivateli relativnˇe jednoduchou cestou simulovat, analyzovat st´ avaj´ıc´ı, ˇci navrhovat a testovat nov´e architektury manipul´ator˚ u. Navrˇzen´ y software by mˇel tak´e v´ yraznˇe urychlit dalˇs´ı moˇzn´ y v´ yvoj pokroˇcil´ ych algoritm˚ u ˇr´ızen´ı manipul´ator˚ u, nebot’ by uˇzivatel mohl vyuˇz´ıt z´ıskan´ ych v´ ysledk˚ u bez nutnosti u ´pln´eho detailn´ıho n´ ahledu do ˇreˇsen´e problematiky a vˇenovat se tak plnˇe dalˇs´ımu zkoum´an´ı. Vyvinut´ y softwarov´ y n´ astroj by byl pochopitelnˇe tak´e cennou pom˚ uckou pro v´ yuku pˇredmˇet˚ u souvisej´ıc´ı s robotikou.
100
4.4 Pouˇ zit´ e architektury manipul´ ator˚ u
4.4
Pouˇ zit´ e architektury manipul´ ator˚ u
N´asleduj´ıc´ı kapitoly zahrnuj´ı nˇekter´e kl´ıˇcov´e u ´lohy v robotice, kter´e budou diskutov´any nejen s ohledem na dostupnou literaturu, ale zejm´ena pak na v´ ysledc´ıch autorova dosavadn´ıho pr˚ uzkumu v t´eto oblasti. Pro lepˇs´ı n´azornost budou v pr˚ ubˇehu textu vyuˇz´ıv´any pˇredevˇs´ım tˇri typy manipul´ ator˚ u na kter´ ych budou vybran´e probl´emy demonstrov´any.
4.4.1
Antropomorfn´ı manipul´ ator se sf´ erick´ ym z´ apˇ est´ım (AM+SZ)
Antropomorfn´ı1 manipul´ ator se sf´erick´ ym z´apˇest´ım patˇr´ı do skupiny s´eriov´ ych neredundantn´ıch manipul´ ator˚ u se ˇsesti stupni volnosti (3 rotaˇcn´ı a 3 translaˇcn´ı) koncov´eho efektoru. Manipul´ ator je proto schopen polohovat sv˚ uj koncov´ y efektor do libovoln´e polohy (pozice a orientace) v prostoru. Jak jiˇz bylo ˇreˇceno, antropomorfn´ı manipul´atory se sf´erick´ ym z´ apˇest´ım dnes patˇr´ı k nejrozˇs´ıˇrenˇejˇs´ım typ˚ um manipul´ator˚ u v pr˚ umyslu a jejich v´ yvojem a v´ yrobou se v r˚ uzn´ ych modifikac´ıch (velikosti, nosnost, pˇresnost, univerzalita atd.) zab´ yvaj´ı vˇsechny velk´e firmy specializovan´e na robotiku. Sch´ematick´e uspoˇr´ ad´ an´ı manipul´atoru je zobrazeno na Obr´azku 4.6 a jedna z jeho moˇzn´ ych praktick´ ych realizac´ı pak na Obr´azku 4.5(b). Pohyb manipul´atoru je zajiˇst’ov´an 6 rotaˇcn´ımi aktu´ atory (motory), kter´e jsou um´ıstˇeny v kloubech typu R jednotliv´ ych ramen.
1
Sv´ ym tvarem napodobuj´ıc´ı ruku ˇclovˇeka.
101
´ 4. UVOD DO ROBOTIKY
Kloub 5
Kloub 4 Kloub 3
Kloub 2
Sférické zápěstí (3 DoF)
Kloub 6
Translační část (3 DoF)
Koncový efektor
Kloub 1
Základna
Obr´ azek 4.6: Antropomorfn´ı manipul´ator se sf´erick´ ym z´apˇest´ım
Kloubov´e souˇradnice: Θ=
θ1 θ2 θ3 θ4 θ 5 θ6
T
(4.1)
Zobecnˇen´e souˇradnice: X=
xe y e z e O e
(4.2)
kde xe , y e , z e jsou jednotkov´e smˇerov´e vektory os souˇradn´eho syst´emu koncov´eho efektoru a O e je jeho polohov´ y vektor (v´ıce v Kapitole 4.5.1) N´avrhov´e parametry: ξ=
4.4.2
l1 l2 l3 l4
T
(4.3)
Paraleln´ı plan´ arn´ı manipul´ ator (PPM)
Paraleln´ı neredundantn´ı plan´ arn´ı manipul´ator, jehoˇz sch´ematick´e uspoˇr´ad´an´ı je zn´azornˇeno na Obr´ azku 4.7(a) je nˇekdy naz´ yv´an jako 2x Scara manipul´ator. Je tvoˇren dvˇema kinematick´ ymi ˇretˇezci s klouby typu R. Rotaˇcn´ı aktu´atory manipul´atoru jsou pevnˇe
102
4.4 Pouˇ zit´ e architektury manipul´ ator˚ u
um´ıstˇeny v jeho z´ akladnˇe. Koncov´ y efektor manipul´atoru vykazuje 2 translaˇcn´ı stupnˇe volnosti. Prototyp manipul´ atoru, viz Obr´azek 4.7(b), je v souˇcasnosti vyv´ıjen ve spolupr´ aci ˇ v Plzni. Vyv´ıjen´ s firmou Atega, s.r.o. [77] na katedˇre kybernetiky, FAV, ZCU y manipul´ ator je nav´ıc osazen jeˇstˇe dvˇema pˇr´ıdavn´ ymi stupni volnosti pro natoˇcen´ı (osa J3) a zdvih (osa z) koncov´eho efektoru, kter´e vˇsak pro anal´ yzu nebudeme d´ale uvaˇzovat.
Motor B
Motor A
(a) Sch´ematick´e uspoˇra ´d´ an´ı
(b) 3D CAD v´ ykres
Obr´ azek 4.7: Paraleln´ı plan´arn´ı manipul´ator
Kloubov´e souˇradnice: Θ=
θA θB
T
(4.4)
Zobecnˇen´e souˇradnice: X=C=
Cx Cy
T
(4.5)
N´ avrhov´e parametry: T T T ξ = kAA1 k kA1 Ck kAk = kBB 1 k kB 1 Ck kBk = l1 l2 l3 (4.6)
4.4.3
Paraleln´ı sf´ erick´ e z´ apˇ est´ı (PSZ)
Sf´erick´e z´ apˇest´ı b´ yv´ a velmi ˇcast´a architektura, kterou m˚ uˇzeme v pr˚ umyslov´ ych aplikac´ıch nal´ezt. Jedn´ a se o mechanismus umoˇzn ˇuj´ıc´ı tˇri rotaˇcn´ı stupnˇe volnosti konco-
103
´ 4. UVOD DO ROBOTIKY
v´eho efektoru a v s´eriov´e podobˇe je pouˇz´ıv´an napˇr´ıklad jako posledn´ı ˇc´ast antropomorfn´ıho manipul´ atoru, viz Kapitola 4.4.1. Zaj´ımavou alternativu k s´eriov´emu sf´erick´emu z´apˇest´ı tvoˇr´ı architektura paraleln´ı, jej´ıˇz sch´ematick´e uspoˇr´ad´an´ı je zn´azornˇeno na Obr´azku 4.8(b) a 3D CAD model na Obr´azku 4.8(a). Manipul´ator je poh´anˇen tˇremi translaˇcn´ı aktu´ atory (line´ arn´ı motory). Koncov´ y efektor je k z´akladnˇe manipul´atoru pˇripevnˇen ˇctyˇrmi nez´ avisl´ ymi kinematick´ ymi ˇretˇezci, z nichˇz prostˇredn´ı tvoˇr´ı tzv. pasivn´ı stabilizaˇcn´ı element, kter´ y omezuje poˇcet stupˇ n˚ u volnosti koncov´eho efektoru (s takov´ ym uspoˇr´ad´ an´ım se u paraleln´ıch manipul´ator˚ u m˚ uˇzeme setkat velmi ˇcasto).
(a) 3D CAD v´ ykres
(b) Sch´ematick´e uspoˇra ´d´ an´ı
Obr´ azek 4.8: Paraleln´ı sf´erick´e z´apˇest´ı
Kloubov´e souˇradnice: Θ=
d1 d2 d3
T
(4.7)
kde di = kBi Ci k je okamˇzit´e vysunut´ı line´arn´ıch aktu´ator˚ u a l0 je vysunut´ı aktu´ator˚ u v domovsk´e poloze manipul´ atoru (di = l0 ). Zobecnˇen´e souˇradnice: X=
α β γ
104
T
(4.8)
4.4 Pouˇ zit´ e architektury manipul´ ator˚ u
kde α, β, γ Eulerovy u ´hly koncov´eho efektoru podle sch´ematu XYZ. N´ avrhov´e parametry: ξ=
a1 a2 l v ψ
T
(4.9)
kde a1 , a2 jsou strany rovnostrann´ ych troj´ uheln´ık˚ u v z´akladnˇe a koncov´em efektoru manipul´ atoru a ψ je jejich vz´ ajemn´e torzn´ı natoˇcen´ı (ve smˇeru osy z0 ) pro manipul´ator v domovsk´e poloze. Prototyp manipul´ atoru, kter´ y vyuˇz´ıv´a jako svou posledn´ı ˇc´ast paraleln´ı sf´erick´e z´ aˇ v Plzni, tentokr´ate ve spolupr´ pˇest´ı je vyv´ıjen opˇet na katedˇre kybernetiky, FAV, ZCU aci s firmou Eurotec JKR s.r.o. [78]. Cel´e zaˇr´ızen´ı bude slouˇzit jako univerz´aln´ı manipul´ ator pro odlakovac´ı, odmaˇst’ovac´ı a konzervaˇcn´ı linky, viz Obr´azek 4.9. Paraleln´ı sf´erick´e z´apˇest´ı (PM) je zde vyuˇzito jako posledn´ı souˇc´ast s´eriov´eho manipul´atoru se ˇctyˇrmi stupni volnosti (SM) zejm´ena kv˚ uli moˇznosti oddˇelit elektronickou ˇc´ast z´apˇest´ı (motory, ˇcidla) od agresivn´ıho prostˇred´ı uvnitˇr oplachovac´ı komory, nebot’ pˇr´ımoˇcaˇre se pohybuj´ıc´ı v´ ysuvy line´ arn´ıch aktu´ator˚ u lze relativnˇe snadno utˇesnit.
Obr´ azek 4.9: Vyuˇzit´ı paraleln´ıho sf´erick´eho z´apˇest´ı (PM) v pr˚ umyslov´e aplikaci.
105
´ 4. UVOD DO ROBOTIKY
4.5
Kinematika manipul´ ator˚ u: Modelov´ an´ı, polohov´ e z´ avislosti
Pod pojmem kinematika manipul´ ator˚ u rozum´ıme z´avislosti mezi obecn´ ymi polohami, rychlostmi, zrychlen´ımi ˇci vyˇsˇs´ımi derivacemi polohy jednotliv´ ych ˇc´ast´ı manipul´atoru bez ohledu na s´ıly a silov´e momenty tyto z´avislosti ovlivˇ nuj´ıc´ı. Vyˇsetˇrov´an´ı kinematick´ ych z´avislost´ı proto ˇrad´ıme mezi nejv´ıce podstatn´e oblasti pˇri anal´ yze a synt´eze manipul´ator˚ u. Lze obecnˇe ˇr´ıci, ˇze kinematika v podstatˇe kaˇzd´eho manipul´atoru je zaloˇzena na z´akladn´ı myˇslence vyuˇz´ıt jednoduch´eho pohybu line´arn´ıch ˇci rotaˇcn´ıch aktu´ator˚ u a prostˇrednictv´ım vhodn´eho mechanismu jej pˇrev´ezt na komplexn´ı rovinn´ y ˇci prostorov´ y pohyb. Takov´ y mechanismus si je pak, spoleˇcnˇe s aktu´atory, moˇzn´e pˇredstavit jako mechatronick´ y poˇc´ıtaˇc, kde jeho program je ukryt v jeho mechanick´e konstrukci. V posledn´ıch letech se zaˇc´ın´ a ve svˇetˇe objevovat n´azor, ˇze kinematika manipul´ator˚ u je jiˇz postupnˇe upadaj´ıc´ım oborem robotiky, vˇetˇsina d˚ uleˇzit´ ych probl´em˚ u je jiˇz d´avno vyˇreˇsena a d´ıky st´ ale se zdokonaluj´ım a zlevˇ nuj´ıc´ım komponent´am (v´ ykonn´e poˇc´ıtaˇce, inteligentn´ı ˇcidla a pohony), kter´e mohou snadno kompenzovat nedokonalosti pˇri jej´ım n´avrhu, nehraje zvl´ aˇst’ v´ yznamnou roli. Jean Paul Merlet, jeden z pˇredn´ıch odborn´ık˚ u, zejm´ena pak v oblasti paraleln´ı robotiky, pˇredstavil v roce 2000 na konferenci IEEE-International Conference on Robotics zaj´ımav´ y pˇr´ıspˇevek s pˇr´ıznaˇcn´ ym n´azvem ”Kinematics’ not dead!”, viz [79], ve kter´em argumentuje pr´avˇe proti zm´ınˇen´emu n´azoru n´asleduj´ıc´ımi poznatky: • n´aklady na mechanickou ˇc´ ast manipul´atoru nepˇresahuj´ı zpravidla 20-30 % cel´eho syst´emu • mˇeˇren´ı chyb v mechanick´e konstrukci je sice moˇzn´e, nicm´enˇe v´ yvoj ˇr´ıd´ıc´ıho syst´emu pro jejich n´ aslednou kompenzaci je velmi obt´ıˇzn´e a zdlouhav´e • v´ ypoˇcetn´ı v´ ykon by mˇel b´ yt vyuˇzit pˇredevˇs´ım na v´ yvoj ”inteligence”manipul´atoru bez nutnosti jej vyuˇz´ıvat pro z´ asah do chov´an´ı manipul´atoru dan´e jeho architekturou D´ale zmiˇ nuje nˇekter´e doposud otevˇren´e probl´emy v robotice, kter´e dodnes nejsou uspokojivˇe ˇreˇseny, zejm´ena pak probl´em synt´ezy a optimalizace manipul´ator˚ u.
106
4.5 Kinematika manipul´ ator˚ u: Modelov´ an´ı, polohov´ e z´ avislosti
4.5.1
Reprezentace obecn´ eho pohybu v robotice
Pro popis pohybu manipul´ ator˚ u, jako syst´emu sloˇzen´eho s pevn´ ych hmotn´ ych ramen a nehmotn´ ych kloub˚ u, byla zavedena cel´a ˇrada metodik (jin´ ymi slovy u ´mluv), z nichˇz dvˇe nejpouˇz´ıvanˇejˇs´ı budou detailnˇe zm´ınˇeny v Kapitole 4.5.2. Vˇsechny u ´mluvy vˇsak vych´ az´ı z myˇslenky vz´ ajemn´e transformace polohy souˇradn´ ych syst´em˚ u, um´ıstˇen´ ych v jednotliv´ ych ramenech manipul´atoru. Zab´ yvejme se proto nejprve reprezentac´ı polohy, rychlosti a zrychlen´ı obecn´eho tˇelesa pˇredstavuj´ıc´ı jedno rameno manipul´atoru. 4.5.1.1
Reprezentace polohy
Pˇredpokl´ adejme dvojici souˇradn´ ych syst´em˚ u (d´ale jen s.s.), z nichˇz prvn´ı s.s. F1 = {O 1 − x1 y 1 z 1 } je pevnˇe spojen s ramenem Link 1 a druh´ y s.s F2 = {O 2 − x2 y 2 z 2 } s ramenem Link 2, viz Obr´ azek 4.10.
Obr´ azek 4.10: Transformace souˇradn´ ych syst´em˚ u
Vz´ ajemn´e translace s.s. F1 a F2 je zˇrejmˇe d´ana vektorem translace r 11,2 = O 12 −O 11 = ajemn´ a rotace m˚ uˇze b´ yt vyj´adˇrena matic´ı rotace R12 , pro kterou plat´ı O 12 . Jejich vz´ n´asleduj´ıc´ı1 : aln´ a matice rozmˇeru [3x3], jej´ıˇz sloupce reprezentuj´ı souˇradnice jednot• R12 je re´ kov´ ych smˇerov´ ych vektor˚ u s.s. F2 v s.s. (ˇci vzhledem k s.s.) F1 . R12 = x12 y 12 z 12 1
Doln´ı index oznaˇcuje konkr´etn´ı vektor (bod), zat´ımco horn´ı index vyjadˇruje, v souˇradnic´ıch kter´eho souˇradn´eho syst´emu je tento vektor (bod) vyj´ adˇren. Napˇr.: r 11,2 a r 21,2 oznaˇcuje ten sam´ y vektor, ale jeho souˇradnice jsou jednou uvaˇzov´ any vzhledem k s.s. F1 a podruh´e k s.s. F2 .
107
´ 4. UVOD DO ROBOTIKY
aln´ı matic´ı (jej´ı sloupce jsou vz´ajemnˇe ortogon´aln´ı-kolm´e). Tedy • R12 je ortogon´ nutnˇe plat´ı: (R12 )T · R12 = I
⇒
(R12 )−1 = (R12 )T = R21
(4.10)
kde R21 je matice rotace, jej´ıˇz sloupce reprezentuj´ı souˇradnice jednotkov´ ych smˇerov´ ych vektor˚ u s.s. F1 v s.s. F2 . R21 = x21 y 21 z 21 Poznamenejme, ˇze prvky matice rotace jsou vz´ajemnˇe z´avisl´e. Lze uk´azat, ˇze minim´ aln´ı poˇcet parametr˚ u pro popis obecn´e rotace je tˇri (viz napˇr. Eulerovy u ´hly). Necht’ P 1 oznaˇcuje souˇradnice bodu P v s.s. F1 a P 2 souˇradnice stejn´eho bodu v s.s. F2 , pak: P 1 = r 11,2 + R12 · P 2
a z´ aroveˇ n, viz (4.10) P 2 = −R21 · r 11,2 + R21 · P 1 (4.11)
• Rozliˇsujeme tˇri z´ akladn´ı element´ arn´ı rotace dvou s.s. – Rotace kolem osy x1 s.s. F1 o u ´hel α Tedy s.s. F2 vznikne natoˇcen´ım souˇradn´eho syst´emu F1 kolem osy x1 o u ´hel α.
1 0 0 Rx (α) = 0 cos(α) − sin(α) 0 sin(α) cos(α)
(4.12)
– Rotace kolem osy y 1 s.s. F1 o u ´hel β Tedy s.s. F2 vznikne natoˇcen´ım souˇradn´eho syst´emu F1 kolem osy y 1 o u ´hel β.
cos(β) 0 sin(β) 0 1 0 Ry (β) = − sin(β) 0 cos(β)
(4.13)
– Rotace kolem osy z 1 s.s. F1 o u ´hel γ Tedy s.s. F2 vznikne natoˇcen´ım souˇradn´eho syst´emu F1 kolem osy z 1 o u ´hel γ. cos(γ) − sin(γ) 0 Rz (γ) = sin(γ) cos(γ) 0 0 0 1
108
(4.14)
4.5 Kinematika manipul´ ator˚ u: Modelov´ an´ı, polohov´ e z´ avislosti
´ • Uhly α, β, γ se naz´ yvaj´ı Eulerovsk´e u ´hly a tvoˇr´ı jeden ze zn´am´ ych moˇznost´ı popis˚ u rotace tˇeles v prostoru. • Obecnou rotaci m˚ uˇzeme vyj´adˇrit skl´ad´an´ım rotac´ı element´arn´ıch, a to dvˇema z´ akladn´ımi postupy:
1. Postupn´ a rotace kolem os souˇ radn´ ych syst´ em˚ u Napˇr´ıklad podle sch´ematu XYZ1 : – Odrotuj s.s. F1 okolo osy x1 o u ´hel α ⇒ vznik´a nov´ y souˇradn´ y syst´em F10 (matice rotace Rx (α)). – Odrotuj s.s. F10 okolo osy y 01 o u ´hel β ⇒ vznik´a nov´ y souˇradn´ y syst´em F100 (matice rotace Ry (β)). – Odrotuj s.s. F100 okolo osy z 001 o u ´hel γ ⇒ vznik´a v´ ysledn´ y souˇradn´ y syst´em F2 (matice rotace Rz (γ)). V´ yslednou matici rotace m˚ uˇzeme tak ps´at jako: R12 = Rx (α) · Ry (β) · Rz (γ)
(4.15)
2. Rotace kolem os souˇ radn´ eho syst´ emu F1 (fixovan´ e osy rotace) Opˇet podle sch´ematu XYZ: – Odrotuj s.s. F1 okolo osy x1 o u ´hel α ⇒ vznik´a nov´ y souˇradn´ y syst´em F10 (matice rotace Rx (α)). ´hel β ⇒ vznik´a nov´ y souˇradn´ y syst´em – Odrotuj s.s. F10 okolo osy y 1 o u F100 (matice rotace Ry (β)). – Odrotuj s.s. F100 okolo osy z 1 o u ´hel γ ⇒ vznik´a v´ ysledn´ y souˇradn´ y syst´em F2 (matice rotace Rz (γ)). Lze uk´ azat, ˇze v´ yslednou matici rotace m˚ uˇzeme tak ps´at jako: R12 = Rz (γ) · Ry (β) · Rx (α) 1
(4.16)
Sch´ema XYZ oznaˇcuje posloupnost rotac´ı kolem jednotliv´ ych os, ˇcasto pouˇz´ıvan´e sch´ema je napˇr. ZYZ.
109
´ 4. UVOD DO ROBOTIKY
Pozn´ amka 4.1 (Singularity v transformaci: matice rotace - Eulerovy u ´ hly) Je zˇrejm´e, ˇze ze zn´ am´ ych Eulerov´ ych u ´hl˚ u α, β, γ lze bez jak´ ychkoliv pot´ıˇz´ı vˇzdy 1 vypoˇc´ıtat matici rotace R2 (α, β, γ), viz vztah (4.15,4.16), mluv´ıme o tzv. dopˇredn´e transformaci. Zpˇetnou transformac´ı pak rozum´ıme stanoven´ı Eulerovsk´ ych u ´hl˚ u α, β, γ ze zn´am´e matice rotace r11 r12 r13 R12 (α, β, γ) = r21 r22 r23 (4.17) r31 r32 r33 Budeme-li uvaˇzovat sch´ema XYZ s postupnou rotac´ı kolem os souˇradn´ ych syst´em˚ u, matice rotace je d´ ana podle (4.15): R12 (α, β, γ) =
− cos (β) sin (γ)
cos (β) cos (γ)
sin (β)
sin (α) sin (β) cos (γ) + cos (α) sin (γ) − sin (α) sin (β) sin (γ) + cos (α) cos (γ) − sin (α) cos (β) − cos (α) sin (β) cos (γ) + sin (α) sin (γ) cos (α) sin (β) sin (γ) + sin (α) cos (γ) cos (α) cos (β)
(4.18) Porovn´an´ım rovnice (4.18) a (4.17) lze urˇcit Eulerovy u ´hly n´asledovnˇe1 : Pro β ∈ (− π2 , π2 ), cos β ≥ 0:
α = atan2(−r23 , r33 ) q 2 + r 2 )) β = atan2(r13 , (r23 33 γ = atan2(−r12 , r11 ) Pro β ∈ ( π2 ,
3 2 π),
cos β < 0:
α = atan2(r23 , −r33 ) q 2 + r 2 )) β = atan2(r13 , − (r23 33 γ = atan2(r12 , −r11 ) 1
Funkce φ = atan2(x, y) zohledˇ nuje znam´enka argument˚ u x a y a vrac´ı korektn´ı ˇreˇsen´ı ve vˇsech kvadrantech φ ∈< 0, 2ß >
110
4.5 Kinematika manipul´ ator˚ u: Modelov´ an´ı, polohov´ e z´ avislosti
Pro β = − π2 matice rotace degeneruje na matici:
0 0 −1 R12 (α, β, γ) = − sin (α − γ) cos (α − γ) 0 cos (α − γ) sin (α − γ) 0 Lze urˇcit jen u ´hel φ = α − γ. Pro β =
π 2
matice rotace degeneruje na matici:
0 0 1 R12 (α, β, γ) = sin (α + γ) cos (α + γ) 0 − cos (α + γ) sin (α + γ) 0 Lze urˇcit jen u ´hel φ = α + γ. Jin´ ymi slovy, u ´hel β = ± π2 pˇredstavuje singularitu v reprezentaci rotace pomoc´ı Eulerov´ ych u ´hl˚ u, nebot’ osa x1 a z 001 jsou rovnobˇeˇzn´e (opaˇcn´e ˇci shodn´e) a nelze tak jednoznaˇcnˇe urˇcit u ´hly α, γ. Jednoznaˇcnˇe lze urˇcit jen jejich rozd´ıl, ˇci souˇcet φ. Pro popis rotace dvou souˇradn´ ych syst´em˚ u se jeˇstˇe vyuˇz´ıvaj´ı krom matice rotace dalˇs´ı zp˚ usoby reprezentace. Mezi nejzn´amˇejˇs´ı patˇr´ı zejm´ena: 1. Reprezentace pomoc´ı obecn´ e osy rotace Kaˇzdou obecnou rotaci lze pˇrev´ezt na rotaci okolo obecn´e osy r o u ´hel θ. Zat´ımco pˇr´ım´ a transformace {r, θ} 7→ R12 (r, θ) lze opˇet vypoˇc´ıtat vˇzdy, pro inverzn´ı transformaci R12 (r, θ) 7→ {r, θ} doch´az´ı k singul´arn´ımu pˇr´ıpadu pro θ = 0 a θ = π, kdy nelze jednoznaˇcnˇe urˇcit smˇer vektoru r. 2. Reprezentace pomoc´ı jednotkov´ eho kvaternionu Jednotkov´ y quaternion Q lze stanovit z u ´hlu θ a osy rotace r jako: θ θ , sin · r} Q = {η, } = {cos 2 2 V´ yhodou jednotkov´eho kvaternionu je, ˇze umoˇzn ˇuje popsat rotaci okolo obecn´e osy r pro u ´hel θ ∈< − π2 ,
π 2
> pˇriˇcemˇz nevykazuje ˇz´adn´e singularity.
Detailn´ı popis pˇr´ım´e a inverzn´ı transformace pro v´ yˇse uveden´e reprezentace rotace lze nal´ezt napˇr. v [80], [81].
111
´ 4. UVOD DO ROBOTIKY
Homogenn´ı transformaˇ cn´ı matice
K celkov´emu popisu polohy (rotace a translace)
s.s. lze s v´ yhodou vyuˇz´ıt tzv. homogenn´ıch souˇradnic. Zaveden´ı homogenn´ıch souˇradnic u ´zce souvis´ı s problematikou geometrick´e projekce (jedn´a se v podstatˇe o projektivn´ı transformaci) a podrobn´e vysvˇetlen´ı vˇcetnˇe ˇrady n´azorn´ ych animac´ı lze nal´ezt napˇr. v [82]. Polohu s.s. F2 vzhledem k s.s. F1 lze pomoc´ı homogenn´ı transformaˇcn´ı matice T 12 ps´at jako, viz Obr. 4.10:
R12
T 12 = 0
0
r 11,2 0
(4.19)
1
Je zˇrejm´e, ˇze pomoc´ı homogenn´ı transformaˇcn´ı matice T 12 lze homogenn´ı souˇradnice bodu P v s.s. F1 vyj´ adˇrit pomoc´ı jeho homogenn´ıch souˇradnic v s.s. F2 jako, viz (4.11):
P1 1
=
T 12
·
P2 1
=
r 11,2 + R12 · P 2 1
(4.20)
Homogenn´ı transformaˇcn´ı matice n´ am tak zahrnuje informaci o obecn´e poloze tˇelesa v prostoru, a to jeho rotaci T 12 [1 : 3, 1 : 3] = R12 a translaci T 12 [1 : 3, 4] = r 11,2 najednou1 . Vyj´adˇr´ıme-li nyn´ı z rovnice (4.20) souˇradnice P 2 v z´avislosti na souˇradnic´ıch P 1 , s ohledem na platnost (4.10), dost´ av´ ame inverzn´ı transformaˇcn´ı vztah:
P2 1
=
(R12 )T · P 1 − (R12 )T · r 11,2 1
= (T 12 )−1 · | {z }
P1 1
(4.21)
T 21
kde
(R12 )T
(T 12 )−1 = T 21 = 0
0
−(R12 )T · r 11,2 0
(4.22)
1
je inverze homogenn´ı transformaˇcn´ı matice T 12 . Poznamenejme, ˇze pro homogenn´ı transformaˇcn´ı matici jiˇz neplat´ı, ˇze jej´ı inverze lze nahradit jej´ı transpozic´ı (nesplˇ nuje podm´ınky ortogonality!) (T 12 )−1 6= (T 12 )T . 1
Z´ apis X[a : b, c : d] oznaˇcuje submatici (subvektor), kter´ y se skl´ ad´ a z a. aˇz b. ˇra ´dku a c. aˇz d. sloupce matice X.
112
4.5 Kinematika manipul´ ator˚ u: Modelov´ an´ı, polohov´ e z´ avislosti
Skl´ ad´ an´ı transformac´ı s.s. Uvaˇzujme trojici s.s. F1 , F2 , F3 a odpov´ıdaj´ıc´ı homogenn´ı transformaˇcn´ı matice mezi s.s. F1 , F2 : T 12 a F2 , F3 : T 23 . Potom v´ ysledn´a homogenn´ı transformaˇcn´ı matice mezi s.s. F1 , F3 lze ps´at: R13
T 13 = 0
r 12,3 = T 12 · T 23 =
0
0
1
R12
= 0
0
R23
r 11,2 · 0
1
0
R12 R23
r 22,3 =
0
0
1
0
R12 r 22,3 + r 11,2
0
0
1 (4.23)
Tedy pro skl´ ad´ an´ı rotac´ı s.s. plat´ı: R13 = R12 R23 A pro skl´ ad´ an´ı translac´ı s.s.: r 11,3 = r 11,2 + R12 r 22,3 ˇ Casto je velmi v´ yhodn´e
Transformace souˇ radnice bod versus vektor mezi s.s.
transformovat souˇradnice bodu ˇci vektoru z jednoho s.s. do jin´eho. Pˇresto, ˇze po form´aln´ı str´ ance je bod a vektor velmi podobn´ y, v pˇr´ıpadˇe jejich transformac´ı mezi s.s. se tato dvojice geometrick´ ych objekt˚ u chov´a diametr´alnˇe odliˇsnˇe. Pˇredpokl´adejme opˇet dva s.s. F1 a F2 se vz´ ajemnou homogenn´ı transformaˇcn´ı matic´ı T 12 a dva libovoln´e body P a Q jednoznaˇcnˇe definuj´ıc´ı vektor v = Q − P . • Transformace souˇ radnic bodu P ze s.s. F2 do s.s. F1
P1 1
=
T 12
·
P2 1
=
r 11,2 + R12 · P 2 1
viz (4.20)
Tedy: P 1 = r 11,2 + R12 · P 2 • Transformace souˇ radnic vektoru v ze s.s. F2 do s.s. F1 v 1 = Q1 − P 1 = r 11,2 + R12 · Q2 − r 11,2 − R12 · P 2 = R12 · (Q2 − P 2 ) Tedy: v 1 = R12 · v 2
113
´ 4. UVOD DO ROBOTIKY
4.5.1.2
Reprezentace rychlosti a zrychlen´ı
Rychlost respektive zrychlen´ı bodu P v s.s. F1 lze, v z´avislosti na jeho rychlosti respektive zrychlen´ı v s.s. F2 , snadno z´ıskat ˇcasovou derivac´ı vztahu (4.20): " " # # 2 1 2 ˙ ˙ P 1 P P + T 12 · = T˙ 2 · 1 0 0 # # " # " " 2 ¨2 ¨1 ˙2 P 1 1 P P P 1 = T¨ 2 · + T2 · + 2T˙ 2 · 1 0 0 0 kde
˙1 R 2
1 T˙ 2 = 0
0
r˙ 11,2 0 0
1 T¨ 2 =
(4.24) (4.25)
¨1 R 2 0
0
r¨ 11,2 0 0
ˇ Casov´ a derivace translaˇcn´ıho vektoru r 11,2 je zˇrejm´a a vyjadˇruje translaˇcn´ı rychlost respektive zrychlen´ı s.s. F2 v s.s. F1 . Je intuitivnˇe jasn´e, ˇze ˇcasov´a derivace matice rotace R12 bezpochyby souvis´ı s vektorem u ´hlov´e rychlosti ω21 . Z ortogonality matice rotace (4.10) plyne R12 · (R12 )T = I
˙ 1 · (R1 )T + R1 · (R ˙ 1 )T = 0 R 2 2 2 2
→ derivac´ı podle ˇcasu →
˙ 1 · (R1 )T dost´av´ame S + S T = 0 (tedy S je antisymetrick´a Zaveden´ım matice S = R 2 2 ˇ matice). Casovou derivaci matice rotace R12 lze tedy pomoc´ı zat´ım nespecifikovan´e antisymetrick´e matice S vyj´ adˇrit jako: ˙ 1 = S · R1 R 2 2
(4.26)
Uvaˇzujme nyn´ı konstantn´ı vektor P 2 (bod P je pevnˇe spojen se s.s. F2 ) a pˇredpokl´adejme, ˇze se s.s. F2 vzhledem k s.s. F1 pouze ot´aˇc´ı u ´hlovou rychlost´ı ω 12 = T ωx ωy ωz (neposouv´ a se r 11,2 = konst.) a jejich okamˇzitou orientaci popisuje matice rotace R12 . Tedy ˇcasovou derivaci vektoru P 1 v s.s. F1 lze ps´at jako, viz (4.24): 1 ˙ 1 · P2 P˙ = R 2
(4.27)
Ze z´akonitost´ı kinematiky plyne, ˇze rychlost konstantn´ıho vektoru P 2 lze v s.s. F1 ps´at tak´e jako: 1 P˙ = ω 12 × R12 · P 2
⇒
114
1 P˙ = ω 12 × P 1
(4.28)
4.5 Kinematika manipul´ ator˚ u: Modelov´ an´ı, polohov´ e z´ avislosti
Dosad´ıme-li vztah (4.26) pro ˇcasovou derivaci matice rotace do rovnice (4.27) a n´aslednˇe porovn´ ame s rovnic´ı (4.28) 1 P˙ = S · R12 · P 2 = S · P 1
←→
P˙ 1 = ω 12 × P 1
(4.29)
je zˇrejm´e, ˇze n´ asoben´ı vektoru P 1 matic´ı S je ekvivalentn´ı vektorov´emu souˇcinu vektoru u ´hlov´e rychlosti ω 12 a vektoru P 1 . Z definice vektorov´eho n´asoben´ı tedy plyne tvar matice S:
0 −ωz ωy 0 −ωx S = S(ω 12 ) = ωz −ωy ωx 0
(4.30)
Z´ avislosti mezi polohou, rychlost´ı a zrychlen´ım s.s. F2 vzhledem k s.s. F1 lze pak zn´azornit pomoc´ı n´ asleduj´ıc´ıch simulaˇcn´ıch sch´emat:
´ (b) Uhlov´ e zrychlen´ı, u ´hlov´ a rychlost, matice rotace, viz rovnice (4.26), (4.30)
(a) Translaˇcn´ı poloha, rychlost a zrychlen´ı
Obr´ azek 4.11: Simulaˇcn´ı sch´emata generov´an´ı polohy s.s. F2 vzhledem k s.s. F1 (parametr (0) oznaˇcuje poˇc´ ateˇcn´ı podm´ınky integr´ator˚ u)
Nˇekdy b´ yv´ a vhodn´e u ´hlovou rychlost s.s. F2 v s.s. F1 vyj´adˇrit m´ısto vektoru u ´hlov´e T 1 ˙ = α˙ β˙ γ˙ rychlosti ω ˇcasovou derivac´ı Eulerov´ ych u ´hl˚ uX (dle dan´eho sch´ematu 2
rotace, napˇr. XYZ). Tuto transformaci ˇreˇs´ı tzv. Eulerovy kinematick´e rovnice. Pˇr´ımou ˙ 1 · (R1 )T lze uk´azat, ˇcasovou derivac´ı matice R1 , viz (4.18), z definice matice S(ω) = R 2
2
2
ˇze pro sch´ema rotace XYZ plat´ı: ˙ ω 12 = H (X) · X kde 1 0 sin β H (X) = 0 cos α − sin α cos β 0 sin α cos α cos β
115
(4.31)
´ 4. UVOD DO ROBOTIKY
4.5.2
´ Umluvy pro popis kinematiky manipul´ ator˚ u
Pro popis geometrick´eho uspoˇr´ ad´ an´ı ramen a kloub˚ u manipul´ator˚ u bylo zavedeno mnoho metod. Ty se pokouˇs´ı jednoduchou a systematickou cestou rekurzivnˇe definovat souˇradn´e syst´emy reprezentuj´ıc´ı jednotliv´ a ramena manipul´atoru a jejich vz´ajemnou polohovou transformaci. Polohov´ a transformace dvou po sobˇe jdouc´ıch s.s. z´avis´ı na dan´ ych konstantn´ıch geometrick´ ych parametrech (zahrnuj´ı geometrick´ y tvar ramen, kloub˚ u a jej´ıch vz´ ajemnou konfiguraci) a kloubov´ ych souˇradnic´ıch Θ (zahrnuj´ı aktu´aln´ı polohu kloub˚ u manipul´ atoru). Mezi nejzn´amˇejˇs´ı takov´e u ´mluvy patˇr´ı tzv. DenavitHartenbergova u ´mluva [83] a Khalil-Kleinfingerova u ´mluva [84]. Obˇe u ´mluvy budou demonstrov´any na AM+SZ pˇredstaven´eho v kapitole 4.4.1 (n = 6), viz [85]. 4.5.2.1
Denavit-Hartenbergova u ´ mluva (D-H)
Dnes snad nejzn´ amˇejˇs´ı u ´mluva pro elegantn´ı popis geometrie s´eriov´ ych manipul´ator˚ u. Pˇredpokl´adejme dvˇe ramena manipul´ atoru Link i − 1 a Link i, kter´a jsou spojena kloubem Joint i s jedn´ım stupnˇem volnosti, viz Obr. 4.12.
Obr´ azek 4.12: D-H u ´mluva
Definice s.s. Fi = {O i − xi y i z i } za pˇredpokladu znalosti s.s. Fi−1 = {O i−1 − xi−1 y i−1 z i−1 } dle D-H u ´mluvy je vyj´ adˇrena n´asledovnˇe:
116
4.5 Kinematika manipul´ ator˚ u: Modelov´ an´ı, polohov´ e z´ avislosti
• Zvol osu z i pod´el osy rotace, resp. translace kloubu Joint i + 1 a osu z 0i pod´el osy rotace, resp. translace kloubu Joint i • Um´ısti poˇc´ atek O i s.s. Fi do pr˚ useˇc´ıku osy z i a norm´aly1 os z i−1 a z i . Um´ısti poˇc´ atek O 0i s.s. Fi0 = {O 0i − x0i y 0i z 0i } do pr˚ useˇc´ıku osy z i−1 a t´eˇze norm´aly. • Zvol osu xi a x0i pod´el norm´aly ve smˇeru od kloubu Joint i do kloubu Joint i + 1. • Zvol osu y i a y 0i tak, aby v´ ysledn´e s.s. byly pravotoˇciv´e. Lze snadno uk´ azat, ˇze D-H u ´mluva nedefinuje jednoznaˇcnˇe um´ıstˇen´ı s.s. v n´asleduj´ıc´ıch pˇr´ıpadech. • Pro s.s. F0 = {O 0 − x0 y 0 z 0 } je urˇcena jednoznaˇcnˇe pouze osa z 0 (podle osy rotace, resp. translace prvn´ıho kloubu manipul´atoru Joint 1). Osu x0 a poˇc´atek O 0 lze proto volit libovolnˇe. Osa y 0 je pak urˇcena tak, aby v´ ysledn´ y syst´em byl opˇet pravotoˇciv´ ym. • Pro s.s. Fn = {O n − xn y n z n }, kde n je poˇcet kloub˚ u s jedn´ım stupnˇem volnosti uvaˇzovan´eho manipul´atoru nen´ı jednoznaˇcnˇe urˇcena osa z n , nebot’ kloub Joint n + 1 jiˇz neexistuje. Osa xn vˇsak mus´ı z˚ ustat kolm´a k ose z n−1 . • Pokud jsou dvˇe po sobˇe jdouc´ı osy kloub˚ u (z i−1 a z i ) paraleln´ı, jejich norm´ala nen´ı jednoznaˇcnˇe definov´ana (m˚ uˇze b´ yt libovolnˇe posunuta ve smˇeru os kloub˚ u). • Pokud se dvˇe po sobˇe jdouc´ı osy kloub˚ u (z i−1 a z i ) prot´ınaj´ı (norm´ala je nulov´e d´elky), osa xi bude volena tak, aby byla kolm´a k rovinˇe definovan´e osami z i−1 a z i . Jej´ı kladn´y smˇer vˇsak m˚ uˇze b´ yt volen libovolnˇe. Nyn´ı m˚ uˇze b´ yt vz´ ajemn´ a poloha s.s. Fi−1 a Fi pops´ana pouze pomoc´ı ˇctyˇr D-H parametr˚ u: ai . . . vzd´ alenost mezi poˇc´ atky O i a O 0i di . . . vzd´ alenost mezi poˇc´ atky O i−1 a O 0i αi . . . u ´hel mezi osami z i−1 a z i dan´ y pootoˇcen´ım s.s. Fi0 pod´el osy x0i θi . . . u ´hel mezi osami xi−1 a xi dan´ y pootoˇcen´ım s.s. Fi pod´el osy z i−1 1
norm´ ala os x a y je spojnice tˇechto os s minim´ aln´ı vzd´ alenost´ı sv´ıraj´ıc´ı s osami prav´ yu ´hel
117
´ 4. UVOD DO ROBOTIKY
Je zˇrejm´e, ˇze pro z´ akladn´ı typy kloub˚ u s jedn´ım stupnˇem volnosti plat´ı: kloub Joint i je typu P promˇenn´ a definuj´ıc´ı pohyb kloubu je di , promˇenn´e ai , αi , θi jsou konstanty definuj´ıc´ı geometrick´e uspoˇr´ad´an´ı ramene Link i kloub Joint i je typu R promˇenn´ a definuj´ıc´ı pohyb kloubu je θi , promˇenn´e ai , di , αi jsou konstanty definuj´ıc´ı geometrick´e uspoˇr´ad´an´ı ramene Link i Transformaˇcn´ı vztah, v naˇsem pˇr´ıpadˇe homogenn´ı transformaˇcn´ı matice v analogick´em tvaru k (4.19), mezi s.s. Fi−1 a Fi je d´ an n´asleduj´ıc´ım zp˚ usobem. • Vyber s.s. Fi−1 • Posuˇ n tento syst´em pod´el osy z i−1 o vzd´alenost di a otoˇc jej okolo osy z i−1 o u ´hel θi ⇒ dost´ av´ ame s.s. Fi0 . Matice pˇrechodu1 : 1 0 0 0 1 0 T i−1 = Trans(z, di )·Rot(z, θi ) = i0 0 0 1 0 0 0
0 cθi 0 sθi · di 0 1 0
−sθi cθi 0 0
0 0 1 0
0 cθi 0 sθi = 0 0 0 1
−sθi cθi 0 0 (4.32)
0 0 0 0 1 di 0 1
• Posuˇ n s.s. Fi0 pod´el osy x0i o vzd´alenost ai a otoˇc jej okolo osy x0i o u ´hel αi ⇒ dost´av´ ame s.s. Fi . Matice pˇrechodu: 1 0 0 0 1 T ii = Trans(x, ai )·Rot(x, αi ) = 0 0 0 0
0 ai 1 0 0 0 0 cαi · 1 0 0 sαi 0 1 0 0
0 −sαi cαi 0
0 1 0 0 0 0 cαi −sαi = 0 0 sαi cαi 1 0 0 0 (4.33)
• V´ ysledn´ a matice pˇrechodu ze s.s. Fi−1 do s.s. Fi je d´ana: 0
i T ii−1 = T i−1 i0 · T i = Trans(z, di ) · Rot(z, θi ) · Trans(x, ai ) · Rot(x, αi ) = cθi −sθi cαi sθi sαi ai cθi sθ cθi cαi −cθi sαi ai sθi i (4.34) = 0 sαi cαi di 0 0 0 1
Pˇripomeˇ nme, ˇze matice pˇrechodu matice (4.34) je funkc´ı pouze kloubov´ ych souˇradnic θi (pro rotaˇcn´ı klouby R) a di (pro translaˇcn´ı klouby P). 1
zkratka cθi , resp. sθi oznaˇcuje cos θi , resp. sin θi . Podobnˇe s(θ1 +θ2 ) oznaˇcuje sin(θ1 + θ2 )
118
ai 0 0 1
4.5 Kinematika manipul´ ator˚ u: Modelov´ an´ı, polohov´ e z´ avislosti
F Pˇ r´ıklad 4.1 (D-H u ´ mluva pro AM+SZ) Obr´ azek 4.13 zn´ azorˇ nuje zaveden´ı souˇradn´ ych syst´em˚ u pro jednotliv´a ramena AM+SZ.
Joint 5 Joint 4
Joint 3
Joint 2
Joint 6
Joint 1
Obr´ azek 4.13: Zaveden´ı souˇradn´ ych syst´em˚ u pro AM+SZ dle D-H u ´mluvy
Geometrick´e parametry manipul´atoru (tzv. D-H parametry), jsou pak d´any n´asleduj´ıc´ı tabulkou: Joint i
di
ai
αi
1
l1
0
π 2
2
0
l2
0
3
0
0
4
l3
0
5
0
0
π 2 − π2 π 2
6
l4
0
0
Tabulka 4.2: D-H parametry AM+SZ
F
119
´ 4. UVOD DO ROBOTIKY
4.5.2.2
Khalil-Kleinfingerova u ´ mluva (K-K)
Khalil-Kleinfingerova u ´mluva, viz [86], pˇredstavuje modifikaci D-H u ´mluvy pro popis
souˇradn´ ych syst´em˚ u manipul´ atoru. Pˇredpokl´adejme opˇet dvˇe ramena manipul´atoru
Link i − 1 a Link i, kter´ a jsou spojena kloubem Joint i s jedn´ım stupnˇem volnosti, viz
Obr. 4.15. Tentokr´ ate jsou vˇsak souˇradn´e syst´emy v jednotliv´ ych kloubech definov´any
jin´ ym zp˚ usobem. Zat´ımco v D-H u ´mluvˇe byl s.s. Fi , kter´ y je pevnˇe sv´az´an s rame-
nem Link i, um´ıstˇen na konci tohoto ramene, tedy s osou zi shodnou s osou rotace
kloubu Joint i + 1, u K-K u ´mluvy je tento souˇradn´ y syst´em um´ıstˇen pˇr´ımo na rotaˇcn´ı
ose kloubu Joint i. Opodstatnˇen´ı takov´eto zmˇeny nen´ı pouze ve zv´ yˇsen´ı pˇrehlednosti
popisu souˇradn´ ych syst´em˚ u, ale pˇredevˇs´ım v moˇznosti popisu komplexn´ıch architektur
manipul´ator˚ u, jako jsou rozvˇetven´e (tree chains) a uzavˇren´e (closed chains) kinematick´e
ˇretˇezce, pro kter´e popis prostˇrednictv´ım D-H u ´mluvy pˇrin´aˇs´ı komplikace a nejasnosti.
120
4.5 Kinematika manipul´ ator˚ u: Modelov´ an´ı, polohov´ e z´ avislosti
Obr´ azek 4.14: D-H u ´mluva vedouc´ı na nejednoznaˇcnost v definici souˇradn´ ych syst´em˚ u pˇri pouˇzit´ı na rozvˇetven´e kinematick´e ˇretˇezce
Pro ilustraci pˇredpokl´ adejme, ˇze bychom chtˇeli popsat pomoc´ı D-H u ´mluvy rozvˇetven´ y kinematick´ y ˇretˇezec, viz Obr. 4.14. Je zˇrejm´e, ˇze transformace polohy z kloubu Joint i do kloubu Joint i + 1 vede na definici s.s. Fi , kter´ y je pevnˇe spˇraˇzen s ramenem Link i, a jeho poloha je dan´ a rotac´ı θi respektive translac´ı di (spoleˇcnˇe s parametry urˇcuj´ıc´ı geometrii ramene ai , αi ) vyvolanou kloubem Joint i. Jin´ ymi slovy s.s. Fi jednoznaˇcnˇe urˇcuje vliv kloubu Joint i na rameno Link i. Toto zd´anlivˇe jednoznaˇcn´e urˇcen´ı vˇsak selh´ av´ a v okamˇziku, kdy je tˇreba definovat s.s. v kloubu Joint i + 2 v rozvˇetven´e ˇc´ asti mechanismu. Vyuˇzijeme-li znovu stejn´eho postupu, mus´ıme definovat s.s. ˜i − x ˜ iy ˜ iz ˜ i }, kter´ F˜i = {O y je opˇet pevnˇe spˇraˇzen s ramenem Link 1 a jeho poloha je dan´ a rotac´ı (θ˜i ) respektive translac´ı (d˜i ) (spoleˇcnˇe s parametry urˇcuj´ıc´ı geometrii
121
´ 4. UVOD DO ROBOTIKY
ramene a ˜i , α ˜ i ) vyvolanou kloubem Joint i. Zde se vˇsak dost´ av´ ame do sporu, nebot’ poloha ramene Link i vyvolan´a jedin´ ym kloubem Joint i je pops´ ana dvˇema r˚ uzn´ ymi s.s. Fi a F˜i . Nab´ız´ı se tak ot´azka, jak´e jsou vlastnˇe kloubov´e souˇradnice kloubu Joint i ? (θi respektive di nebo θ˜i respektive d˜i )
Obr´ azek 4.15: K-K u ´mluva
Zavedeme-li nyn´ı souˇradn´e syst´emy kloub˚ u dle K-K u ´mluvy, viz Obr. 4.15 , definice s.s. Fi za pˇredpokladu znalosti s.s. Fi−1 je vyj´adˇrena n´asledovnˇe: • Zvol osu z i a z 0i pod´el osy rotace, resp. translace kloubu Joint i. (Pro i = 1 nen´ı s.s. Fi0 definov´ an.) • Um´ısti poˇc´ atek O i s.s. Fi do pr˚ useˇc´ıku osy z i a norm´aly os z i a z i+1 . Um´ısti poˇc´atek O 0i s.s. Fi0 do pr˚ useˇc´ıku osy z i a norm´aly os z i−1 a z i . • Zvol osu xi pod´el norm´ aly ve smˇeru od kloubu Joint i do kloubu Joint i + 1. • Zvol osu y i tak, aby v´ ysledn´e s.s. byly pravotoˇciv´e. Lze snadno uk´ azat, ˇze K-K u ´mluva nedefinuje jednoznaˇcnˇe um´ıstˇen´ı s.s. v n´asleduj´ıc´ıch pˇr´ıpadech. • Pro s.s. Fn , kde n je poˇcet kloub˚ u s jedn´ım stupnˇem volnosti uvaˇzovan´eho manipul´atoru je jednoznaˇcnˇe urˇcena pouze osa z n , nebot’ kloub Joint n + 1 jiˇz neexistuje. Osa xn m˚ uˇze b´ yt volena totoˇzn´a s osou xn−1 .
122
4.5 Kinematika manipul´ ator˚ u: Modelov´ an´ı, polohov´ e z´ avislosti
• Pokud jsou dvˇe po sobˇe jdouc´ı osy kloub˚ u (z i a z i+1 ) paraleln´ı, jejich norm´ala nen´ı jednoznaˇcnˇe definov´ana (m˚ uˇze b´ yt libovolnˇe posunuta ve smˇeru os kloub˚ u). M˚ uˇze b´ yt volena tak, ˇze parametr di = 0 nebo di+1 = 0. • Pokud se dvˇe po sobˇe jdouc´ı osy kloub˚ u (z i a z i+1 ) prot´ınaj´ı (norm´ala je nulov´e d´elky), osa xi bude volena tak, aby byla kolm´a k rovinˇe definovan´e osami z i a z i+1 . Jej´ı kladn´y smˇer vˇsak m˚ uˇze b´ yt volen libovolnˇe. • S.s. F0 m˚ uˇze b´ yt volen libovolnˇe, zpravidla jako totoˇzn´ y se s.s. F1 pro θ1 = 0, respektive d1 = 0 (nulov´a poloha kloubu Joint 1). Nyn´ı m˚ uˇze b´ yt vz´ ajemn´ a poloha s.s. Fi−1 a Fi pops´ana pouze pomoc´ı ˇctyˇr K-K parametr˚ u: αi . . . u ´hel mezi osami z i−1 a z i kolem norm´aly xi−1 ai . . . vzd´ alenost mezi poˇc´ atkem O i−1 a osou z i (kolm´a vzd´alenost mezi osami) di . . . vzd´ alenost mezi poˇc´ atky O 0i a O i θi . . . u ´hel mezi osami xi−1 a xi dan´ y kolem osy z i Je zˇrejm´e, ˇze pro z´ akladn´ı typy kloub˚ u s jedn´ım stupnˇem volnosti opˇet plat´ı: kloub Joint i je typu P promˇenn´a definuj´ıc´ı pohyb kloubu je di , promˇenn´e ai , αi , θi jsou konstanty definuj´ıc´ı geometrick´e uspoˇr´ad´an´ı ramene Link i kloub Joint i je typu R promˇenn´a definuj´ıc´ı pohyb kloubu je θi , promˇenn´e ai , di , αi jsou konstanty definuj´ıc´ı geometrick´e uspoˇr´ad´an´ı ramene Link i Transformaˇcn´ı vztah (homogenn´ı transformaˇcn´ı matice) mezi s.s. Fi−1 a Fi je d´ an n´asleduj´ıc´ım zp˚ usobem. • Vyber s.s. Fi−1 • Posuˇ n tento syst´em pod´el osy xi−1 o vzd´alenost ai a otoˇc jej okolo osy xi−1 o u ´hel αi ⇒ dost´ av´ ame s.s. Fi0 . Matice pˇrechodu: 1 0 0 ai 1 0 0 1 0 0 0 cαi T i−1 = Trans(x, ai )·Rot(x, αi ) = i0 0 0 1 0 ·0 sαi 0 0 0 1 0 0
123
0 −sαi cαi 0
0 1 0 0 0 0 cαi −sαi = 0 0 sαi cαi 1 0 0 0 (4.35)
ai 0 0 1
´ 4. UVOD DO ROBOTIKY
• Posuˇ n s.s. Fi0 pod´el osy z 0i o vzd´alenost di a otoˇc jej okolo osy z 0i o u ´hel θi ⇒ dost´av´ ame s.s. Fi . Matice pˇrechodu:
1 0 0 T ii = Trans(z, di )·Rot(z, θi ) = 0 0
0 1 0 0
cθi 0 0 sθ 0 0 · i 1 di 0 0 1 0
−sθi cθi 0 0
0 0 1 0
cθi 0 sθ 0 = i 0 0 1 0
−sθi 0 0 cθi 0 0 0 1 di 0 0 1 (4.36)
• V´ ysledn´ a matice pˇrechodu ze s.s. Fi−1 do s.s. Fi je d´ana:
0
i T i−1 = T i−1 i i0 · T i = Trans(x, ai ) · Rot(x, αi ) · Trans(z, di ) · Rot(z, θi ) = cθi −sθi 0 ai cα sθ cα cθ −sα −sα di i i i i i i = (4.37) sα sθ sα cθ c c d α α i i i i i i i
0
0
0
1
Porovn´an´ım parametr˚ u D-H u ´mluvy, viz Obr. 4.12 a K-K u ´mluvy, viz Obr. 4.15 m˚ uˇzeme vypozorovat fakt, ˇze D-H parametry [θi , di , αi , ai ] odpov´ıdaj´ı K-K parametr˚ um [θi , di , αi+1 , ai+1 ] (vyjma pˇr´ıpadu pro prvn´ı a posledn´ı ˇctveˇrici parametr˚ u, nebot’ s.s. F0 m˚ uˇze b´ yt volen libovolnˇe a s.s. Fn m´a pevnˇe urˇcen´ y jen smˇer osy z n ).
124
4.5 Kinematika manipul´ ator˚ u: Modelov´ an´ı, polohov´ e z´ avislosti
Obr´ azek 4.16: Khalil-Kleinfingerova u ´mluva pro rozvˇetven´e kinematick´e ˇretˇezce
Ukaˇzme nyn´ı v´ yhodu K-K u ´mluvy pro definov´an´ı rozvˇetven´ ych architektur. Pro ilustraci pˇredpokl´ adejme stejn´ y pˇr´ıpad jako na Obr. 4.14, vˇsak se zaveden´ım souˇradn´ ych syst´em˚ u jako na Obr. 4.16. Transformaci s.s. Fi−1 kloubu Link i − 1 do s.s. Fi kloubu Link i jiˇz zn´ ame a je d´ ana pouze 4 parametry ai , αi , di , γi , viz rovnice (4.37). Poznamenejme, ˇze zm´ınˇen´e s.s. jsou k sobˇe navz´ajem ve specifick´e poloze, uˇcen´e norm´alou os z i−1 a z i , a tedy postaˇcuj´ı pouze 4 DoF k jejich vz´ajemn´emu polohov´an´ı. Definujme d´ ale norm´ alu os z i−1 a z i+1 a s n´ı souvisej´ıc´ı pomocn´ y souˇradn´ y syst´em 1F i−1
=1 O i−1 −1xi−1 1y i−1 1z i−1 , kter´ y m´a osu 1z i−1 totoˇznou s osou z i−1 a osu 1xi−1
ve smˇeru definovan´e norm´ aly. Poloha s.s. Fi−1 a 1Fi−1 bude pops´ana pouze dvojic´ı parametr˚ u: γi+1 . . . u ´hel mezi osami xi−1 a 1xi−1 kolem osy z i−1 bi+1 . . . vzd´ alenost mezi poˇca´tkem O i−1 a poˇc´atkem 1O i−1
125
´ 4. UVOD DO ROBOTIKY
Transformaˇcn´ı vztah tˇechto s.s. je d´ an pak n´asleduj´ıc´ım zp˚ usobem: • Posuˇ n s.s. Fi−1 pod´el osy z i−1 o hodnotu bi+1 a otoˇc jej kolem osy z i−1 o u ´hel γi+1 ⇒ dost´ av´ ame s.s. 1Fi−1 Matice pˇrechodu:
T i−1 1i−1
1 0 = Trans(z, bi+1 )·Rot(z, γi+1 ) = 0 0
0 1 0 0
cγi+1 0 0 sγi+1 0 0 · 1 bi+1 0 0 0 1
cγi+1 sγ i+1 = 0 0
−sγi+1 cγi+1 0 0
−sγi+1 cγi+1 0 0 0 0 0 0 1 bi+1
0
0 0 1 0
0 0 = 0 1
(4.38)
1
Nyn´ı je zˇrejm´e, ˇze transformace mezi s.s. 1Fi−1 a Fi+1 jiˇz bude d´ana zn´amou transfor1
i−1 maˇcn´ı matic´ı T i+1 analogickou jako v rovnici (4.37) s parametry [θi+1 , di+1 , αi+1 , ai+1 ]: 1
i−1 T i+1 = Trans(x, ai+1 ) · Rot(x, αi+1 ) · Trans(z, di+1 ) · Rot(z, θi+1 ) = cθi+1 −sθi+1 0 ai+1 cα sθ i+1 i+1 cαi+1 cθi+1 −sαi+1 −sαi+1 di+1 = sα sθ cαi+1 di+1 i+1 i+1 sαi+1 cθi+1 cαi+1
0
0
0
(4.39)
1
V´ ysledn´a matice pˇrechodu ze s.s Fi−1 do s.s. Fi+1 je d´ana: 1
i−1 i−1 T i−1 i+1 = T 1i−1 · T i+1 =
= Trans(z, bi+1 )·Rot(z, γi+1 )·Trans(x, ai+1 )·Rot(x, αi+1 )·Trans(z, di+1 )·Rot(z, θi+1 ) = cγi+1 cθi+1 − sγi+1 cαi+1 sθi+1 −cγi+1 sθi+1 − sγi+1 cαi+1 cθi+1 sγi+1 sαi+1 sγi+1 sαi+1 di+1 + cγi+1 ai+1 sγ cθ + cγ cα sθ −sγi+1 sθi+1 + cγi+1 cαi+1 cθi+1 −cγi+1 sαi+1 −cγi+1 sαi+1 di+1 + sγi+1 ai+1 i+1 i+1 i+1 i+1 i+1 sαi+1 sθi+1 sαi+1 cθi+1 cαi+1 cαi+1 di+1 + bi+1 0
0
0
1 (4.40)
Poznamenejme, ˇze stejn´ ym zp˚ usobem lze odvodit transformaˇcn´ı vztahy pro libovoln´ y poˇcet kloub˚ u pˇripojen´ ych na rameno Link i−1. Je patrn´e, ˇze K-K u ´mluvu jednoznaˇcnˇe definuje, narozd´ıl od D-H u ´mluvy, kloubov´e souˇradnice kaˇzd´eho kloubu.
126
4.5 Kinematika manipul´ ator˚ u: Modelov´ an´ı, polohov´ e z´ avislosti
F Pˇ r´ıklad 4.2 (K-K u ´ mluva pro SM+SZ) Obr´ azek 4.17 zn´ azorˇ nuje zaveden´ı souˇradn´ ych syst´em˚ u pro jednotliv´a ramena AM+SZ.
Joint 5 Joint 4
Joint 3
Joint 2
Joint 6
Joint 1
Obr´ azek 4.17: Zaveden´ı souˇradn´ ych syst´em˚ u pro AM+SZ dle K-K u ´mluvy
Geometrick´e parametry manipul´atoru (tzv. K-K parametry), jsou pak d´any n´asleduj´ıc´ı tabulkou: Joint i
di
ai
αi
1
0
0
0
2
0
0
π 2
3
0
l2
0
4
l3
0
5
0
0
6
0
0
π 2 − π2 π 2
Tabulka 4.3: K-K parametry AM+SZ. Poznamenejme, ˇze se nyn´ı v popisu manipul´atoru nevyskytuj´ı parametry l1 respektive l4 . Bez u ´jmy na obecnosti mohou b´ yt tyto parametry b n souˇc´ ast´ı konstantn´ıch transformaˇcn´ıch matic T 0 respektive T e , viz Pozn´amka 4.2.
F
127
´ 4. UVOD DO ROBOTIKY
4.6
Polohov´ e z´ avislosti manipul´ ator˚ u
Polohov´ ymi z´ avislostmi rozum´ıme vztah mezi kloubov´ ymi Θ a zobecnˇen´ ymi X souˇradnicemi manipul´ atoru a dˇel´ıme je na dva z´akladn´ı probl´emy: ´ • pˇ r´ım´ y kinematick´ y probl´ em/´ uloha (PKU) Tedy nalezen´ı z´ avislosti zobecnˇen´ ych souˇradnic X na kloubov´ ych souˇradnic´ıch Θ. V cizojazyˇcn´e literatuˇre ˇcasto naz´ yvan´ y jako direct/forward kinematics problem [80], direct geometric model [81]. ´ • inverzn´ı kinematick´ y probl´ em/´ uloha (IKU) Tedy nalezen´ı z´ avislosti kloubov´ ych souˇradnic Θ na zobecnˇen´ ych souˇradnic´ıch X. V cizojazyˇcn´e literatuˇre ˇcasto naz´ yvan´ y jako inverse kinematic problem [80], inverse geometric problem [81]. Pro lepˇs´ı orientaci v textu zaved’me n´asleduj´ıc´ı znaˇcen´ı pro kloubov´e souˇradnice manipul´atoru: Θ=
ϑ1 . . . ϑn
T
(4.41)
kde ϑi = θi (Joint i je typu R) a
4.6.1
ϑi = di (Joint i je typu P)
Pˇ r´ım´ y kinematick´ y probl´ em pro s´ eriov´ e manipul´ atory
Pˇr´ım´ y kinematick´ y probl´em pro s´eriov´e manipul´atory nepˇredstavuje v´aˇzn´e komplikace a pro jeho ˇreˇsen´ı lze s v´ yhodou vyuˇz´ıt u ´mluvy pro popis manipul´ator˚ u z Kapitoly 4.5.2, nebot’ kaˇzd´a transformaˇcn´ı matice T i−1 z´avis´ı pˇr´ımo na aktivn´ı kloubov´e souˇradnici i ϑi . Pˇr´ım´a kinematickou u ´loha pro n aktivn´ıch kloub˚ u s´eriov´eho manipul´atoru lze tak formulovat ve tvaru: T 0n (Θ) =
n Y
T i−1 i (ϑi )
(4.42)
i=1
kde transformaˇcn´ı matice T i−1 any dle pouˇzit´e u ´mluvy pˇr´ımo rovnicemi (4.34) i (ϑi ) jsou d´ nebo (4.37). Je tedy zˇrejm´e, ˇze pˇr´ım´ a kinematick´a u ´loha pro s´eriov´e manipul´atory m´a vˇ zdy analytick´ eˇ reˇ sen´ı.
128
4.6 Polohov´ e z´ avislosti manipul´ ator˚ u
Pozn´ amka 4.2 (Kompenzace polohy z´ akladny a koncov´ eho efektoru) Z praktick´eho hlediska je v´ yhodn´e definovat jeˇstˇe dva dalˇs´ı s.s., a to s.s. z´akladny (r´amu) manipul´ atoru Fb = {O b − xb y b z b } a s.s. koncov´eho efektoru Fe = {O e − xe y e z e }. Je zˇrejm´e, ˇze s.s. jsou nez´ avisl´e na poloze kloub˚ u manipul´atoru, a lze je tedy vyj´adˇrit vzhledem k poloze s.s. prvn´ıho F0 a posledn´ıho Fn kloubu konstantn´ımi maticemi pˇrechodu T b0 a T ne , viz Obr. 4.18. V technick´e praxi tyto matice pˇredstavuj´ı vˇetˇsinou kompenzaci um´ıstˇen´ı konkr´etn´ıho manipul´atoru na v´ yrobn´ı lince (T b0 ) ˇci kompenzaci polohy pracovn´ıho n´ astroje na koncov´em efektoru manipul´atoru (T ne ). V´ ysledn´ a matice pˇrechodu T be (Θ) z´avisej´ıc´ı na poloze kloubov´ ych souˇradnic Θ respektuj´ıc´ı i v´ yˇse uveden´e kompenzace polohy bude m´ıt tak n´asleduj´ıc´ı tvar: T be (Θ)
=
T b0
·
T 0n
·
T ne
Rbe
= 0
0
r bb,e 0 1
(4.43)
kde Rbe =
h
xbe y be z be
i
(4.44)
je matice rotace a r bb,e = O be − O bb = O be je translaˇcn´ı vektor s.s. Fe vzhledem k s.s. Fb .
Obr´ azek 4.18: Princip kompenzac´ı manipul´atoru
Poznamenejme, ˇze vektor zobecnˇen´ ych souˇradnic manipul´atoru X lze z´ıskat z transformaˇcn´ı matice T be napˇr, jako: X = Rbe O be matice rotace + translaˇcn´ı vektor (4.45) T Eulerovy u ´hly (z Rbe ) + translaˇcn´ı vektor X = α β γ (O be )T . . . atd.
129
´ 4. UVOD DO ROBOTIKY
Pˇr´ım´ y kinematick´ y probl´em lze tak s vyuˇzit´ım rovnice (4.42) interpretovat jako neline´arn´ı vektorovou transformaˇcn´ı funkci parametrizovanou n´avrhov´ ymi parametry manipul´atoru ξ (v literatuˇre ˇcasto oznaˇcov´ ana jako geometrick´ a omezen´ı manipul´ atoru): X = G(Θ), kde G = G(ξ)
(4.46)
F Pˇ r´ıklad 4.3 (Pˇ r´ım´ a kinematick´ au ´ loha pro SM+SZ) Pˇr´ım´a kinematick´ au ´loha SM+SZ pro domovskou (Θ = 0) a obecnou polohu koncov´eho efektoru manipul´ atoru (bez kompenzace T b0 = T 6e = I): 1 0 0 1 h iT Θ= 0 0 0 0 0 0 ⇒ X = 0 −1 0 0 0 0 −1 −0.3 0.51 0.33 0.79 1.28 iT h ⇒ X = −0.20 0.94 −0.27 0.96 Θ = π4 π3 π4 π3 π3 π2 −0.84 −0.02 0.55 2.29
Joint 3 & Joint 4
Joint 2
Joint 5 & Joint 6
Joint 3 & Joint 4
Joint 2
Joint 5 & Joint 6
Joint 1
Joint 1
(a) Domovsk´ a poloha
(b) Obecn´ a poloha
Obr´ azek 4.19: Pˇr´ım´ a kinematick´ au ´loha pro SM+SZ (model v SimMechanicsu)
130
4.6 Polohov´ e z´ avislosti manipul´ ator˚ u
F
4.6.2
Inverzn´ı kinematick´ y probl´ em pro s´ eriov´ e manipul´ atory
Formulace inverzn´ıho kinematick´eho probl´emu plyne pˇr´ımo z rovnice (4.46). Polohu kloubov´ ych souˇradnic Θ lze pro danou polohu koncov´eho efektoru X stanovit jako: Θ = G−1 (X)
(4.47)
Nalezen´ı inverze neline´ arn´ı vektorov´e transformaˇcn´ı funkce G−1 je v obecn´em pˇr´ıpadˇe velmi sloˇzit´e, nebot’ ve funkci G se, d´ıky transformaˇcn´ı matici T be , vyskytuj´ı souˇcty n´asobk˚ u a mocnin ˇclen˚ u sin ϑi , cos ϑi . Uvaˇzujme obecn´ y neredundantn´ı s´eriov´ y prostorov´ y manipul´ator se vˇsemi 6 stupni T volnosti koncov´eho efektoru, a tedy pr´avˇe 6 kloubov´ ymi souˇradnicemi Θ = ϑ1 . . . ϑ6 . Jelikoˇz zobecnˇen´ y vektor souˇradnic X m´a v prostoru maxim´alnˇe 6 nez´avisl´ ych promˇenn´ ych (napˇr. 3 Eulerovy u ´hly a vektor translace koncov´eho efektoru) je zˇrejm´e, ˇze ˇreˇsen´ı inverzn´ı kinematick´e u ´lohy pro obecn´y neredundantn´ı manipul´ ator vede na soustavu 6 neline´ arn´ıch rovnic pro 6 nezn´ am´ych. Metody pro nalezen´ı ˇreˇsen´ı inverzn´ı kinematick´e u ´lohy pro s´eriov´e manipul´atory lze rozdˇelit v podstatˇe do n´ asleduj´ıc´ıch skupin, kter´e budou struˇcnˇe zm´ınˇeny v n´asleduj´ıc´ıch kapitol´ ach: • Pˇr´ım´e analytick´e ˇreˇsen´ı jednoduch´ ych architektur manipul´ator˚ u • Specializovan´e metody pro ˇreˇsen´ı konkr´etn´ıch variant architektur manipul´ator˚ u (omezen´e uspoˇr´ ad´ an´ı kloub˚ u dan´ ych typ˚ u) • Metody pro ˇreˇsen´ı obecn´ ych architektur manipul´ator˚ u • Numerick´e gradientn´ı metody, viz Kapitola 4.9 4.6.2.1
Pˇ r´ım´ e analytick´ eˇ reˇ sen´ı jednoduch´ ych architektur manipul´ ator˚ u
Vyuˇz´ıv´ a se zejm´ena pro jednoduch´e konstrukce manipul´ator˚ u, kde je moˇzn´e s urˇcitou d´avkou zkuˇsenost´ı a matematick´e intuice relativnˇe snadno nal´ezt inverzn´ı kinematickou transformaci G−1 (X). Metody pˇr´ım´eho analytick´eho ˇreˇsen´ı b´ yvaj´ı aplikov´any na jednoduch´e plan´ arn´ı ˇci prostorov´e manipul´atory, viz Pˇr´ıklad 4.4, 4.5. Velmi ˇcasto se m˚ uˇzeme s tˇemito metodami setkat pˇri ˇreˇsen´ı inverzn´ı kinematick´e u ´lohy pro paraleln´ı
131
´ 4. UVOD DO ROBOTIKY
manipul´atory s jednoduch´ ymi kinematick´ ymi ˇretˇezci, kde kaˇzd´ y kinematick´ y ˇretˇezec je v podstatˇe samostatn´ y s´eriov´ y manipul´ator, viz Pˇr´ıklad 4.6. F Pˇ r´ıklad 4.4 (Translaˇ cn´ı ˇ c´ ast AM+SZ z Obr´ azku 4.6) Zobecnˇen´e souˇradnice translaˇcn´ı ˇc´ asti AM+SZ definujme jako (pˇredpokl´adejme T b0 = I ⇒ F0 = Fb , ˇz´ adn´ a kompenzace polohy z´akladny manipul´atoru): X tran = r 00,E = O 0E =
h
0 OE x
0 OE y
0 OE z
iT
Obr´ azek 4.20: Translaˇcn´ı ˇc´ ast AM+SZ (zaveden´ı s.s. dle D-H u ´mluvy, viz Pˇr´ıklad 4.1)
´ pro translaˇcn´ı ˇc´ast AM+SZ, lze rozdˇelit na dvˇe f´aze. Vˇsimnˇeme si, ˇze ˇreˇsen´ı IKU Pro kloubovou souˇradnici θ1 budou d´ıky um´ıstˇen´ı kloub˚ u zˇrejmˇe existovat dvˇe ˇreˇsen´ı ve tvaru: 0 0 θ1 = atan2 (OE , OE ) x y
(4.48)
0 0 θ1 = atan2 (OE , OE )+π x y
(4.49)
´ plan´arn´ıho s´eriov´eho manipul´atoru typu T´ım jsme probl´em pˇrevedli na ˇreˇsen´ı IKU RR (PSM-RR) se dvˇema DoF, jehoˇz sch´ematick´e uspoˇr´ad´an´ı je zn´azornˇeno na Ob-
132
4.6 Polohov´ e z´ avislosti manipul´ ator˚ u
r´azku 4.21(a). Zobecnˇen´e souˇradnice PSM-RR definujme jako: X psm = O 1E =
h
1 OE x
1 OE y
0
iT
kde X psm lze d´ıky znalosti kloubov´e souˇradnice θ1 z´ıskat z X tran jako O 1E = (T 01 (θ1 ))−1 · O 0E kde matice pˇrechodu T 01 (θ1 ) je d´ana rovnic´ı (4.34) a hodnotami z Tabulky ??.
2.5
2
1.5
y
1
0.5
Joint 2
0
Joint 1
−0.5
−1 −1
−0.5
0
0.5
1
1.5
2
2.5
x
´ pro r˚ (b) Dvojice moˇzn´ ych ˇreˇsen´ı IKU uzn´e polohy Θ
(a) Sch´ematick´e uspoˇra ´d´ an´ı PSM-RR
´ pro plan´arn´ı s´eriov´ Obr´ azek 4.21: IKU y manipul´ator typu RR
ˇ sen´ı PKU ´ pro PSM-RR lze ps´at: Reˇ
T 1E (Θ) =
3 Y
3 T i−1 i (θi ) · T E
i=2
kde
? ? = ? 0
? ? ? 0
T 3E
=
? l3 s(θ2 +θ3 ) + l2 cθ2 ? −l3 c(θ2 +θ3 ) + l2 sθ2 ? 0 0 1
I
0 0 l3
0 0 0
1
(4.50)
je konstantn´ı kompenzaˇcn´ı matice polohy koncov´eho efektoru a matice pˇrechodu T i−1 i (θi ) jsou opˇet urˇceny rovnic´ı (4.34) a hodnotami z Tabulky ??.
133
´ 4. UVOD DO ROBOTIKY
Poloha koncov´eho efektoru PSM-RR je tedy d´ana jako:
X psm
1 O Ex l3 s(θ2 +θ3 ) + l2 cθ2 = T 1E [1 : 3, 3] = O 1Ey = −l3 c(θ2 +θ3 ) + l2 sθ2 0 0
´ pro PSM-RR lze potom ˇreˇsit n´asledovnˇe: IKU 1 a O 1 dost´ Umocnˇen´ım a seˇcten´ım prvk˚ u OE av´ame, s vyuˇzit´ım souˇctov´ ych goniEy x ometrick´ ych vzorc˚ u, hodnotu kloubov´e souˇradnice θ2 : 1 2 1 2 (OE ) + (OE ) = l32 + l22 + 2l2 l3 sθ3 x y
sθ3 =
s2θ3 + c2θ3
1 )2 + (O 1 )2 − l2 − l2 (OE 2 3 Ey x
2l2 l3 q 2 = 1 ⇒ cθ3 = ± 1 − sθ3 s θ3 θ3 = atan2 cθ3
(4.51) (4.52) (4.53)
ˇ sen´ım soustavy rovnic, opˇet s vyuˇzit´ım souˇctov´ Reˇ ych goniometrick´ ych vzorc˚ u, 1 OE = l3 c(θ2 +θ3 ) + l2 cθ2 x 1 OE = −l3 s(θ2 +θ3 ) + l2 sθ2 y
pro nezn´am´e sθ2 , cθ2 , dost´ av´ ame hodnotu kloubov´e souˇradnice θ2 : s θ2 = cθ2 =
1 + (l + l s )O 1 l3 cθ3 OE 2 3 θ3 Ey x 1 )2 + (O 1 )2 (OE Ey x 1 − l c O1 (l2 + l3 sθ3 )OE 3 θ3 Ey x
1 )2 + (O 1 )2 (OE Ey x sθ2 θ2 = atan2 cθ2
(4.54)
´ pro PSM-RR m´a dvˇe ˇreˇsen´ı, viz Obr´azek 4.21(b). Ze vztahu (4.52) je zˇrejm´e, ˇze IKU ´ existuje pouze za pˇredpokladu −1 ≤ sθ ≤ 1, viz Poznamenejme d´ ale, ˇze ˇreˇsen´ı IKU 3 rovnice (4.52). Pro moˇzn´e um´ıstˇen´ı koncov´eho efektoru tak plat´ı (po jednoduch´ ych u ´prav´ach) nerovnost, l2 − l3 ≤ kXpsm k ≤ l2 + l3 kter´a definuje pracovn´ı prostor manipul´atoru. Lze uk´azat, ˇze se manipul´ator na hranic´ıch pracovn´ıho prostoru nal´ez´ a v tzv. singul´arn´ıch poloh´ach, v´ıce v Kapitole 4.8.4.
134
4.6 Polohov´ e z´ avislosti manipul´ ator˚ u
´ pro translaˇcn´ı ˇc´ast AM+SZ m´a celkem 4 r˚ Je tedy zˇrejm´e, ˇze IKU uzn´a ˇreˇsen´ı (dvojice ˇreˇsen´ı pro kloubovou souˇradnici θ1 a dvojice ˇreˇsen´ı pro PSM-RR), viz Obr´azek 4.22.
´ pro translaˇcn´ı ˇc´ast AM+SZ Obr´ azek 4.22: Moˇzn´a ˇreˇsen´ı IKU
F
F Pˇ r´ıklad 4.5 (Sf´ erick´ e z´ apˇ est´ı AM+SZ z Obr´ azku 4.6) Zobecnˇen´e souˇradnice sf´erick´eho z´apˇest´ı AM+SZ definujme jako (pˇredpokl´adejme T 6e = I ⇒ F6 = Fe , ˇz´ adn´ a kompenzace polohy koncov´eho efektoru manipul´atoru):
h i X sz = R36 = x36 y 36 z 36
135
´ 4. UVOD DO ROBOTIKY
(a) Sch´ematick´e sf´erick´eho z´ apˇest´ı (zaveden´ı s.s. dle (b) Dvˇe moˇzn´ a ˇreˇsen´ı IKU sf´erick´eho z´ aD-H u ´mluvy, viz Pˇr´ıklad 4.1 pˇest´ı
´ sf´erick´e z´apˇest´ı Obr´ azek 4.23: IKU
ˇ sen´ı PKU ´ pro sf´erick´e z´ Reˇ apˇest´ı lze ps´at: T 36
R36
= 0
0
6 Y O 36 = T ii−1 (θi ) i=4 0 1
(4.55)
kde cθ4 cθ5 cθ6 − sθ4 sθ6 3 R6 = sθ4 cθ5 + cθ4 sθ6 −sθ5 cθ6
−cθ4 cθ5 cθ6 − sθ4 cθ6 −sθ4 cθ5 sθ6 + cθ4 cθ6 sθ5 sθ6
cθ4 sθ5 n3x s3x a3x sθ4 sθ5 , n3y s3y a3y cθ5 n3z s3z a3z
(4.56)
a matice pˇrechodu T i−1 et urˇceny rovnic´ı (4.34) a hodnotami z Tabulky ??. i (θi ) jsou opˇ ´ pro sf´erick´e z´apˇest´ı rozpad´a na dvˇe Ze soustavy rovnic (4.56) lze odvodit, ˇze se IKU ˇreˇsen´ı, viz Obr´ azek 4.23(b). Umocnˇen´ım a seˇcten´ım prvk˚ u a3x a a3y dost´av´ame: (a3x )2 + (a3y )2 = s2θ5
⇒
q sθ5 = ± (a3x )2 + (a3y )2
q 1. Pro sθ5 = + (a3x )2 + (a3y )2 ≥ 0 ⇒ θ5 ∈< 0, π >
136
4.6 Polohov´ e z´ avislosti manipul´ ator˚ u
θ4 = atan2(sθ4 sθ5 , cθ4 sθ5 ) = atan2(sθ4 , cθ4 ) = atan2(a3y , a3x ) q θ5 = atan2(sθ5 , cθ5 ) = atan2( (a3x )2 + (a3y )2 , a3z )
(4.57)
θ6 = atan2(sθ5 sθ6 , −(−sθ5 cθ6 )) = atan2(sθ6 , cθ6 ) = atan2(s3z , −n3z ) q 2. Pro sθ5 = − (a3x )2 + (a3y )2 ≤ 0 ⇒ θ5 ∈< −π, 0 > θ4 = atan2(sθ4 (−sθ5 ), cθ4 (−sθ5 )) = atan2(sθ4 , cθ4 ) = atan2(−a3y , −a3x ) q θ5 = atan2(sθ5 , cθ5 ) = atan2(− (a3x )2 + (a3y )2 , a3z )
(4.58)
θ6 = atan2(−sθ5 sθ6 , −(sθ5 cθ6 )) = atan2(sθ6 , cθ6 ) = atan2(−s3z , n3z ) F F Pˇ r´ıklad 4.6 (Paraleln´ı sf´ erick´ e z´ apˇ est´ı z Obr´ azku 4.8) Jak jiˇz bylo zm´ınˇeno v kapitole 4.4.3, paraleln´ı sf´erick´e z´apˇest´ı je vybaveno tˇremi ak´ lze tivn´ımi kinematick´ ymi ˇretˇezci (v podstatˇe s´eriov´ ymi manipul´atory), jejichˇz IKU snadno ˇreˇsit n´ asleduj´ıc´ım postupem.
´ pro kinematick´ Obr´ azek 4.24: Dvˇe moˇzn´a ˇreˇsen´ı IKU y ˇretˇezec PSZ
Pˇredpokl´ adejme, ˇze pro jeden kinematick´ y ˇretˇezec, viz Obr´azek 4.24 zn´ame polohu jeho koncov´eho efektoru: h iT −−−→ (4.59) X i = B i D i b = bdix bdiy bdiz Jedinou hledanou kloubovou souˇradnic´ı je pak pouze d´elka vysunut´ı line´arn´ıho aktu´ atoru di , nebot’ zb´ yvaj´ıc´ı 2 kloubov´e souˇradnice (´ uhly natoˇcen´ı pro kloub typu U) jsou
137
´ 4. UVOD DO ROBOTIKY −−−→ pasivn´ı a nelze je tud´ıˇz jakkoliv ˇr´ıdit. Vektor C i D i konstantn´ı d´elky l lze pak vyj´adˇrit jako: iT −−−→b h C i D i = bdix bdiy bdiz − di (4.60) A tedy: −−−→ kC i D i b k2 = bd2ix + bd2iy + (bdiz − di )2 = l2 (bdiz − di )2 = l2 − bd2ix − bd2iy q bdiz − di = ± l2 − bd2ix − bd2iy q di = bdiz ∓ l2 − bd2ix − bd2iy
(4.61)
´ pro kaˇzd´ Z rovnice (4.61) je zˇrejm´e, ˇze IKU y kinematick´ y ˇretˇezec PSZ m´a dvˇe ˇreˇsen´ı 2 2 2 za podm´ınky l > bdix + bdiy . F ´ pro jednoduch´e manipul´atory, pro kter´e plat´ı, ˇze vˇetˇsina D-H Systematick´e ˇreˇsen´ı IKU respektive K-K parametr˚ u ai , di jsou nulov´e a αi , θi jsou nulov´e ˇci rovn´e ± π2 , popsal ´ demonstrovan´e Richard P. Paul, viz [87]. Jeho metoda v podstatˇe zobecˇ nuje ˇreˇsen´ı IKU v Pˇr´ıkladech 4.4 a 4.5 a v literatuˇre je ˇcasto naz´ yvan´a jako Paulova metoda (Paul method). Metoda je zaloˇzena na myˇslence postupn´eho vyjadˇrov´an´ı kloubov´ ych souˇradnic ´ ϑi z celkov´eho kinematick´eho popisu manipul´atoru dan´eho rovnic´ı PKU, n´asleduj´ıc´ım postupem: • V´ ypoˇcet matice pˇrechodu T 0n ze zobecnˇen´ ych souˇradnic X, viz Kapitola 4.6.1. Z´ısk´av´ ame tak soustavu 12 neline´arn´ıch rovnic pro n nezn´am´ ych (4.42): T 0n = T 01 (ϑ1 ) · T 12 (ϑ2 ) · · · · · T n−1 n
(4.62)
• Pˇren´asoben´ım rovnice (4.62) zleva matic´ı pˇrechodu (T 01 (ϑ1 ))−1 dost´av´ame: T 10 (ϑ1 ) · T 0n = T 12 (ϑ2 ) · T 23 (ϑ3 ) · · · · · T nn−1 (ϑn )
(4.63)
Lev´a strana rovnice (4.63) je pak z´avisl´a pouze na kloubov´e souˇradnici ϑ1 , kterou se pokus´ıme vypoˇc´ıtat. • Pˇren´asoben´ım rovnice (4.63) zleva matic´ı pˇrechodu (T 12 (ϑ2 ))−1 dost´av´ame: T 21 (ϑ2 ) · T 10 (ϑ1 ) · T 0n = T 23 (ϑ3 ) · T 34 (ϑ3 ) · · · · · T n−1 n (ϑn )
(4.64)
Lev´a strana rovnice (4.64) je pak z´avisl´a pouze na kloubov´e souˇradnici ϑ2 (ϑ1 jiˇz zn´ame), kterou se pokus´ıme vypoˇc´ıtat.
138
4.6 Polohov´ e z´ avislosti manipul´ ator˚ u
• Analogick´ ym zp˚ usobem se pokus´ıme postupnˇe dopoˇc´ıtat vˇsechny zb´ yvaj´ıc´ı kloubov´e souˇradnice ϑi , i = 3 . . . n Bylo experiment´ alnˇe uk´ az´ ano, ˇze pro celou ˇradu pr˚ umyslovˇe pouˇz´ıvan´ ych manipul´ator˚ u ´ pomoc´ı Paulovo metody na ˇreˇsen´ı 8 z´akladn´ıch typ˚ degeneruje IKU u rovnic uveden´ ych v Tabulce 4.4, jejichˇz analytick´e ˇreˇsen´ı je zn´am´e (zde jej vˇsak d´ale neuv´ad´ıme a lze ´ pro AM+SZ) napˇr. v [81]). nal´ezt spoleˇcnˇe s konkr´etn´ım pˇr´ıkladem (ˇreˇsen´ı IKU Typ 1
X · di = Y
Typ 2
X · sθi + Y · cθi = Z
Typ 3
X1 · sθi + Y1 · cθi = Z1 X2 · sθi + Y2 · cθi = Z2
Typ 4
X1 · dj · sθi = Y1 X2 · dj · cθi = Y2
Typ 5
X1 · sθi = Y1 + Z1 · dj X2 · cθi = Y2 + Z2 · dj
Typ 6
W · sθj = X · cθi + Y · sθi + Z1 W · cθj = X · cθi − Y · sθi + Z2
Typ 7
W1 · cθj + W2 · sθj = X · cθi + Y · sθi + Z1 W1 · sθj − W2 · cθj = X · sθi − Y · cθi + Z2
Typ 8
X · cθi + Y · c(θi +θj ) = Z1 X · sθi + Y · s(θi +θj ) = Z2
Tabulka 4.4: Typick´e rovnice ˇreˇsen´e pˇri pouˇzit´ı Paulovy metody, kde θi resp. di jsou kloubov´e souˇradnice kloubu typu R resp. P a Xi , Yi , Zi , Wi jsou re´aln´e koeficienty z´avisl´e na D-H ˇci K-K geometrick´ ych parametrech manipul´atoru
4.6.2.2
Specializovan´ e metody pro ˇ reˇ sen´ı konkr´ etn´ıch variant architektur manipul´ ator˚ u
V pr˚ umyslov´ ych aplikac´ıch se velmi ˇcasto vyskytuj´ı s´eriov´e manipul´atory, jejichˇz ar´ co moˇzn´a nejv´ıce chitektura je navrˇzena takov´ ym zp˚ usobem, aby byl v´ ypoˇcet IKU usnadnˇen. Jedn´ a se zejm´ena o pˇr´ıpady, kdy je moˇzn´e dan´ y manipul´ator vhodnˇe de´ ˇreˇsit pro kaˇzd´ komponovat na v´ıce funkˇcn´ıch celk˚ u a IKU y celek samostatnˇe (napˇr. prostˇrednictv´ım pˇr´ım´ ych analytick´ ych metod, viz kapitola 4.6.2.1). Vypust´ıme-li jed´ trivi´aln´ı, drtivou vˇetˇsinu noduch´e pravo´ uhl´e (port´ alov´e) manipul´atory, pro kter´e je IKU manipul´ ator˚ u v pr˚ umyslu tvoˇr´ı pr´avˇe antropomorfn´ı manipul´ator se sf´erick´ ym z´apˇest´ım, viz kapitola 4.4.1.
139
´ 4. UVOD DO ROBOTIKY
´ AM+SZ z Obr´ F Pˇ r´ıklad 4.7 (IKU azku 4.6) Necht’ i h X = xbe y be z be O be
(4.65)
jsou zobecnˇen´e souˇradnice manipul´ atoru se zaveden´ ymi s.s. dle D-H u ´mluvy, viz Pˇr´ıklad 4.1. Je zˇrejm´e, ˇze smˇerov´e vektory os a poˇc´atek s.s. F6 vzhledem k s.s. F0 lze z´ıskat z X pomoc´ı kompenzaˇcn´ıch matic T b0 a T 6e jako:
0 b b b b x6 y 06 z 06 O 06 = (T b0 )−1 · xe y e z e O e · (T 6e )−1 0 0 0 1 0 0 0 1
(4.66)
Zobecnˇen´e souˇradnice translaˇcn´ı ˇc´ asti manipul´atoru z Pˇr´ıkladu 4.4 lze potom vypoˇc´ıtat jako: X tran = O 0E = O 04 = O 05 = O 06 − l4 · z 06
(4.67)
Z´ısk´av´ame tak ˇreˇsen´ı pro kloubov´e souˇradnice θ1 , θ2 a θ3 . Zobecnˇen´e souˇradnice sf´erick´eho z´ apˇest´ı z Pˇr´ıkladu 4.5 nyn´ı lze spoˇc´ıtat jako: h i X sz = R36 = (T 03 (θ1 , θ2 , θ3 )[1 : 3, 1 : 3])T · x06 y 06 z 06 {z } | | {z } 3 R0
(4.68)
R06
kde T 03 (θ1 , θ2 , θ3 ) =
3 Y
T i−1 i (θi )
i=1
a matice pˇrechodu T i−1 ceny rovnic´ı (4.34) a hodnotami z Tabulky ??. i (θi ) jsou urˇ ´ pro AM+SZ Z´ısk´av´ame tak ˇreˇsen´ı pro zb´ yvaj´ıc´ı kloubov´e souˇradnice θ4 , θ5 a θ6 . IKU m´a tedy celkem 8 r˚ uzn´ ych ˇreˇsen´ı (4 pro translaˇcn´ı ˇc´ast kr´at 2 pro sf´erick´e z´apˇest´ı). F ´ AM+SZ pomoc´ı vhodn´e dekompozice na translaˇcn´ı ˇc´ast (obecnˇe Metodu ˇreˇsen´ı IKU ˇreˇsen´ı polohov´e rovnice) a sf´erick´e z´ apˇest´ı (obecnˇe ˇreˇsen´e rotaˇcn´ı rovnice) uk´azanou v Pˇr´ıkladu 4.7 lze zobecnit pro dva z´ akladn´ı typy architektury manipul´ator˚ u se 6 DoF, viz [88]. Uved’me d´ ale jen n´ astin ˇreˇsen´ı, podrobnˇejˇs´ı popis lze nal´ezt napˇr´ıklad v [81].
140
4.6 Polohov´ e z´ avislosti manipul´ ator˚ u
Manipul´ ator obsahuj´ıc´ı sf´ erick´ e z´ apˇ est´ı v libovoln´ eˇ c´ asti kinematick´ eho ˇ retˇ ezce Moˇ zn´ e varianty kinematick´ eho ˇ retˇ ezce: XXX(RRR), X(RRR)XX, XX(RRR)X, XXX(RRR), kde (RRR) oznaˇcuje sf´erick´e z´ apˇest´ı a X oznaˇcuje kloub typu P nebo R.
Joint m Joint m-1
Joint m+1
Obr´ azek 4.25: Sf´erick´e z´apˇest´ı (obecn´ y pˇr´ıpad)
N´ astin metody: Pˇredpokl´ ad´ ame-li popis s.s. manipul´atoru pomoc´ı K-K u ´mluvy, viz Pˇr´ıklad 4.2, trojice po sobˇe jdouc´ıch kloub˚ u Joint m − 1, Joint m a Joint m + 1, kde 2 ≤ m ≤ 5, tvoˇr´ı sf´erick´e z´ apˇest´ı, pokud pro K-K parametry plat´ı n´asleduj´ıc´ı, viz Obr´azek 4.25:
am = dm = am+1 = 0 sαm 6= 0
(4.69)
sαm+1 6= 0 Je zˇrejm´e, ˇze poˇc´ atky s.s. Fm−1 a Fm jsou shodn´e a nez´avisl´e na kloubov´ ych souˇradnic´ıch sf´erick´eho z´ apˇest´ı θm−1 , θm , θm+1 . Polohu tˇechto poˇc´atk˚ u m˚ uˇzeme tedy vyj´adˇrit
141
´ 4. UVOD DO ROBOTIKY
vzhledem k s.s. Fm−2 jako: am−1 0 −d 0 m−1 sαm−1 = T m−2 m+1 · Trans(z, −dm+1 ) · 0 = d m−1 cαm−1 1 1
O m−2 m−1 1
(4.70)
Rovnice (4.42) lze tedy ps´ at s vyuˇzit´ım rovnice (4.70) jako (matice pˇrechodu T 06 lze opˇet vypoˇc´ıtat ze zobecnˇen´ ych souˇradnic manipul´atoru): m+1 T 0m−2 · T m−2 = T 06 m+1 · T 6
(4.71)
T 0m−2 ·
O m−2 m−1
1
0 0 = T 06 · T 6m+1 · Trans(z, −dm+1 ) · 0 1
(4.72)
Rovnice (4.72) se naz´ yv´ a polohovou rovnic´ı, nebot’ z´avis´ı pouze na kloubov´ ych souˇradnic´ıch, kter´e nepˇr´ısluˇs´ı sf´erick´emu z´ apˇest´ı ϑ1 , . . . , ϑm−2 a ϑm+1 , . . . , ϑ6 . Zˇrejmˇe tedy plat´ı: T 0m−2 = T 0m−2 (ϑ1 , . . . , ϑm−2 )
a T 6m+1 = T 6m+1 (ϑm+1 , . . . , ϑ6 )
kde ϑi je kloubov´ a souˇradnice kloubu typu P respektive R, viz (4.41). Lze uk´azat, ˇze rovnice (4.72) vede na ˇreˇsen´ı 6 typ˚ u rovnic, jejichˇz analytick´e ˇreˇsen´ı je opˇet zn´am´e. Prvn´ı tˇri rovnice jsou shodn´e s rovnicemi typu 1, 2 a 3 z Tabulky 4.4 a zb´ yvaj´ıc´ı tˇri typy rovnic jsou uvedeny v Tabulce 4.5. Poˇcet moˇzn´ ych ˇreˇsen´ı je roven ˇctyˇrem. Typ 9
a2 d2i + a1 di + a0 = 0
Typ 10
a4 d4i + a3 d3i + a2 d2i + a1 di + a0 = 0
Typ 11
a4 s2θi + a3 cθi sθi + a2 cθi + a1 sθi + a0 = 0
´ manipul´atoru se sf´erick´ Tabulka 4.5: Dalˇs´ı typick´e rovnice pouˇzit´e pˇri ˇreˇsen´ı IKU ym z´ apˇest´ım, kde θi resp. di jsou kloubov´e souˇradnice kloubu typu R resp. P a ai jsou re´aln´e koeficienty z´ avisl´e na K-K geometrick´ ych parametrech manipul´atoru
Z rovnice (4.71) je moˇzn´e vzhledem k (4.19) odvodit z´avislosti pro rotaˇcn´ı matice jednotliv´ ych souˇradn´ ych syst´em˚ u: m+1 R0m−2 · Rm−2 = R06 m+1 · R6
142
(4.73)
4.6 Polohov´ e z´ avislosti manipul´ ator˚ u
Rovnice (4.73) se naz´ yv´ a rotaˇcn´ı rovnic´ı, nebot’, pro nyn´ı jiˇz zn´am´e hodnoty kloubov´ ych souˇradnic nepˇr´ısluˇsej´ıc´ım sf´erick´emu z´apˇest´ı (matice R0m−2 a Rm+1 jsou konstantn´ı 6 matice), z´ avis´ı pouze na kloubov´ ych souˇradnic´ıch sf´erick´eho z´apˇest´ı: m−2 Rm−2 m+1 = Rm+1 (θm−1 , θm , θm+1 )
Lze uk´ azat, ˇze rovnice (4.73) vede na ˇreˇsen´ı rovnic typu 2 a 3 z Tabulky 4.4. Poˇcet moˇzn´ ych ˇreˇsen´ı je roven dvˇema. Manipul´ ator obsahuj´ıc´ı 3 klouby typu P a 3 klouby typu R v libovoln´ em uspoˇ r´ ad´ an´ı Moˇ zn´ e varianty kinematick´ eho ˇ retˇ ezce: PPPRRR, PPRPRR, ... + dalˇs´ıch 18 kombinac´ı. N´ astin metody: Oznaˇcme klouby typu R jako Joint i, Joint j a Joint k a klouby typu P jako Joint i0 , Joint j 0 a Joint k 0 . Je zˇrejm´e, ˇze klouby typu P nemohou jakkoliv ovlivnit orientaci koncov´eho efektoru manipul´ atoru. Rotaˇcn´ı rovnici m˚ uˇzeme tedy odvodit z rovnice (4.42) opˇet s vyuˇzit´ım (4.19) jako: R0i (θi ) · Rij (θj ) · Rj6 (θk ) = R06
(4.74)
Lze uk´ azat, ˇze rovnice (4.74) vede na podobn´e ˇreˇsen´ı jako rotaˇcn´ı rovnice (4.73). Stejnˇe tak i poˇcet ˇreˇsen´ı je roven dvˇema. Polohovou rovnici lze ps´ at jako: T 0i (di0 ) · T ij (dj 0 ) · T k6 = T 06
(4.75)
Vzhledem k tomu, ˇze matice pˇrechodu v rovnici (4.75) jsou z´avisl´e pouze na kloubov´ ych souˇradnic´ıch kloub˚ u typu P (kloubov´e souˇradnice θi , θj , θk kloub˚ u typu R zn´ame z ˇreˇsen´ı rovnice (4.74)), nevyskytuj´ı se v t´eto polohov´e rovnici ˇz´adn´e ˇcleny typu sin, cos a rovnice je tak soustavou line´arn´ıch rovnic v nezn´am´ ych di0 , dj 0 , dk0 . 4.6.2.3
Metody pro ˇ reˇ sen´ı obecn´ ych architektur manipul´ ator˚ u
Pod obecnou architekturou manipul´atoru se 6 Dof rozum´ıme manipul´ator se 6 klouby typu P respektive R s libovolnˇe orientovan´ ymi osami translace respektive rotace. D-H
143
´ 4. UVOD DO ROBOTIKY
ˇci K-K geometrick´e parametry mohou tedy nab´ yvat libovoln´ ych re´aln´ ych hodnot. Dnes ´ takov´ nejzn´amˇejˇs´ı obecnou metodou pro ˇreˇsen´ı IKU ych manipul´ator˚ u je RaghavanRothova metoda (Raghavan-Roth method), viz [89]. N´ astin metody (pro s´ eriov´ y manipul´ ator se 6 klouby typu R): Rovnice (4.42) lze pˇrepsat n´ asledovnˇe: T 01 (θ1 ) · T 12 (θ2 ) · T 23 (θ3 ) · T 34 (θ4 ) = T 06 · T 65 (θ6 ) · T 54 (θ5 )
(4.76)
Prvky matice na prav´e stranˇe rovnice (4.76) jsou funkcemi pouze kloubov´ ych souˇradnic θ5 , θ6 a prvky na lev´e stranˇe kloubov´ ych souˇradnic θ1 , θ2 , θ3 , θ4 , coˇz v´ yraznˇe usnadˇ nuje dalˇs´ı symbolick´e v´ ypoˇcty. Lze uk´ azat, ˇze porovn´an´ım vhodn´ ych pˇr´ısluˇsej´ıc´ıch si prvk˚ u matic na lev´e a prav´e stranˇe, rozˇs´ıˇren´ım soustavy rovnic o pˇr´ıdavn´e rovnice a n´asledn´em zjednoduˇsen´ı, dost´ av´ ame soustavu rovnic, jiˇz nez´avislou na θ4 (form´alnˇe shodnou s line´arn´ım soustavou rovnic): A · X1 = B · Y
(4.77)
kde T X 1 = sθ2 sθ3 sθ2 cθ3 cθ2 sθ3 cθ2 cθ3 sθ2 cθ2 sθ3 cθ3 1 T Y = sθ5 sθ6 sθ5 cθ6 cθ5 sθ6 cθ5 cθ6 sθ5 cθ5 sθ6 cθ6 A je matice [14 × 9] s prvky dan´ ymi line´arn´ı kombinac´ı funkc´ı sθ1 , cθ1 B je konstantn´ı matice [14 × 8] Za u ´ˇcelem eliminace θ5 a θ6 lze rovnice (4.77) rozdˇelit n´asledovnˇe: A1 B1 A1 · X 1 = B 1 · Y · X1 = ·Y ⇒ A2 B2 A2 · X 1 = B 2 · Y
(4.78)
kde A1 je matice [6 × 9], A2 je matice [8 × 9], B 1 je matice [6 × 8] a B 2 je matice [8 × 8]. Eliminac´ı Y dost´ av´ ame rovnici: D · X 1 = 06×1
(4.79)
kde D = [A1 − B1 · B −1 avisl´a pouze na line´arn´ı kombinaci sθ1 2 · A2 ] je matice [6 × 9] z´ a cθ1 . Dosazen´ım zn´ ame substituce, viz [90] θi xi = tan 2
⇒
sθi = cθi =
2xi 1+x2i 1−x2i 1+x2i
144
,
pro i = 1, 2, 3
(4.80)
4.6 Polohov´ e z´ avislosti manipul´ ator˚ u
do rovnice (4.79) dost´ av´ ame rovnici: E · X 2 = 06×1 kde X 2 =
x22 x23 x22 x3 x22 x2 x23 x2 x3 x2 x23 x3 1
(4.81) T
a E je matice [6×9] jej´ıˇz
prvky jsou kvadratick´ ymi funkcemi v promˇenn´e x1 . Z rovnice (4.81) lze eliminovat x2 a x3 tzv. dyalitickou eliminac´ı, kter´a je zaloˇzena na n´ asleduj´ıc´ım postupu. Vyn´asoben´ım rovnice (4.81) promˇenou x2 dost´av´ame rovnici: E · X 3 = 06×1 kde X 3 =
(4.82)
x32 x23 x32 x3 x32 x22 x23 x22 x3 x22 x2 x23 x2 x3 x2 .
Kombinac´ı rovnic (4.81) a (4.82) dost´av´ame soustavu rovnic: S · X = 012×1 kde X =
x32 x23 x32 x3 x32 x22 x23 x22 x3 x22 x2 x23 x2 x3 x2 x23 x3 1
(4.83) T
a S je
matice [12 × 12] jej´ıˇz prvky jsou opˇet kvadratick´ ymi funkcemi v promˇenn´e x1 a plat´ı: E 06×3 S= 06×3 E Vzhledem k tomu, ˇze X 6= 012×1 soustava rovnic (4.83) m´a netrivi´aln´ı ˇreˇsen´ı, a to pouze za pˇredpokladu: det(S) = 0
(4.84)
Raghavan uk´ azal, ˇze det(S) = 0 je polynomem stupnˇe 24 v promˇenn´e x1 a m´a jedin´ y spoleˇcn´ y faktor (x21 + 1)4 vedouc´ı na komplexn´ı koˇreny. Rovnice 4.84 lze tedy ps´at: det(S) = f (x1 )(x21 + 1)4 = 0
(4.85)
kde f (x1 ) je polynom stupnˇe 16 s re´aln´ ymi koˇreny (nˇekdy v literatuˇre naz´ yv´an jako charakteristick´y polynom manipul´ atoru). ´ Je tedy zˇrejm´e ˇze obecn´ y 6R manipul´ator m´a maxim´alnˇe 16 r˚ uzn´ ych ˇreˇsen´ı IKU. Kloubov´e souˇradnice jsou postupnˇe vypoˇc´ıt´any pro vˇsechny koˇreny x1i , i = 1, 2, . . . polynomu f (x1 ) n´ asledovnˇe: • x2i a x3i lze z´ıskat ˇreˇsen´ım soustavy line´arn´ıch rovnic (4.83), nebot’ S(x1i ) je numerick´ a matice a obecnˇe plat´ı rank(S(x1i )) = 11 (ˇreˇs´ıme tak soustavu 11 rovnic pro 11 nezn´ am´ ych). Zpˇetnou substituc´ı (4.80) z´ısk´av´ame kloubov´e souˇradnice θ1i , θ2i a θ3i .
145
´ 4. UVOD DO ROBOTIKY
• Substituc´ı θ1i , θ2i , θ3i do rovnice B 2 · Y = A2 · X 1 z (4.78) dost´av´ame soustavu 8 line´arn´ıch rovnic pro 8 nezn´ am´ ych v Y . Kloubov´e souˇradnice θ5i a θ6i lze pak vypoˇc´ıtat prostˇrednictv´ım funkce atan2(?). • Pˇreorganizov´ an´ım rovnice (4.76) a substituc´ı θ1i , θ2i , θ3i , θ5i , θ6i dost´av´ame T 34 (θ4 ) = T 32 (θ3 ) · T 21 (θ2 ) · T 10 (θ1 ) · T 06 · T 65 (θ6 ) · T 54 (θ5 ) a vzhledem k (4.37), pro popis s.s. podle K-K u ´mluvy, lze posledn´ı kloubovou souˇradnici vypoˇc´ıtat z prvk˚ u T 34 (θ4 )[1, 1] a T 34 (θ4 )[1, 2] opˇet pomoc´ı funkce atan2(?). Metodu je moˇzn´e v m´ırnˇe pozmˇenˇen´e podobˇe aplikovat i na manipul´atory s klouby typu P. Poznamenejme, ˇze pro manipul´ator nach´azej´ıc´ı se v singul´arn´ı poloze, viz Kapitola 4.8.4 se v matici S objevuj´ı line´ arnˇe z´avisl´e ˇr´adky, u ´loha potom nem´a ˇreˇsen´ı. Naopak line´arnˇe z´ avisl´e sloupce se mohou v matici S vyskytovat pro specifickou kombinaci K-K ˇci D-H geometrick´ ych parametr˚ u manipul´atoru. V takov´em pˇr´ıpadˇe je tˇreba zmˇenit prvky matice, kter´e se porovn´ avaj´ı v rovnici (4.76), pˇr´ıpadnˇe tuto ”startovac´ı”rovnici zvolit jin´ ym zp˚ usobem, existuje celkem 6 moˇzn´ ych variant: T 01 · T 12 · T 23 · T 34 = T 06 · T 65 · T 54 T 12 · T 23 · T 34 · T 45 = T 10 · T 06 · T 65 T 23 · T 34 · T 45 · T 56 = T 21 · T 10 · T 06 T 34 · T 45 · T 56 · T 60 = T 32 · T 21 · T 10 T 45 · T 56 · T 60 · T 01 = T 43 · T 32 · T 21 T 56 · T 60 · T 01 · T 12 = T 54 · T 43 · T 32
ˇ sen´ı IKU ´ pro specifick´e hodnoty geometrick´ Reˇ ych parametr˚ u (konkr´etn´ı kombinace kloub˚ u) zaloˇzen´e na modifikace Raghavanovy metody je podrobnˇe diskutov´ano napˇr. v [91]. Efektivn´ı implementaci obecn´e Raghavanovy metody s ohledem na v´ ypoˇcetn´ı n´aroˇcnost, stabilitu a potlaˇcen´ı numerick´ ych chyb lze nal´ezt v [92]. ´ pro s´eriov´e manipul´atory Poznamenejme, ˇze z obecn´eho pohledu je tedy ˇreˇsen´ı IKU pˇrevoditeln´e na probl´em ˇreˇsen´ı soustavy polynomi´aln´ıch rovnic. Raghavanova metoda tento probl´em d´ ale pˇrev´ ad´ı na hled´ an´ı koˇrenu polynomu n-t´eho stupnˇe. Alternativn´ım pˇr´ıstupem se dnes st´ av´ a pomalu, ale jistˇe se rozˇsiˇruj´ıc´ı metoda Gr¨ obnerov´ych b´ az´ı,
146
4.7 Paraleln´ı manipul´ atory
zaloˇzen´ a na principu pˇrevodu soustavy polynomi´aln´ıch rovnic na ekvivalentn´ı soustavu, jej´ıˇz ˇreˇsen´ı lze vypoˇc´ıtat postupn´ ym dosazov´an´ım d´ılˇc´ıch v´ ysledk˚ u (napˇr. jedna rovnice soustavy je polynomem v jedn´e promˇenn´e, atd.). N´azorn´ y pˇr´ıklad s vyuˇzit´ım pr´avˇe ˇ sen´ı IKU ´ pro obecn´ Gr¨ obnerov´ ych b´ az´ı pro PSM-RR z Pˇr´ıkladu 4.4 lze nal´ezt v [93]. Reˇ y s´eriov´ y manipul´ ator z Kapitoly 4.6.2.3 pak napˇr. v [94].
4.7
Paraleln´ı manipul´ atory
Jak jiˇz bylo zm´ınˇeno v Kapitole 4.2, paraleln´ı manipul´atory jsou tvoˇreny uzavˇren´ ymi kinematick´ ymi ˇretˇezci, neboli z´akladna manipul´atoru je s koncov´ ym efektorem spojena alespoˇ n dvˇema nez´ avisl´ ymi otevˇren´ ymi kinematick´ ymi ˇretˇezci. Existuj´ı v podstatˇe dva z´akladn´ı systematick´e pohledy na ˇreˇsen´ı kinematiky takov´ ych manipul´ator˚ u: • Dekompozice paraleln´ıho mechanismu na nez´avisl´e uzavˇren´e kinematick´e ˇretˇezce • Dekompozice paraleln´ıho mechanismu na s´eriov´e manipul´atory Poznamenejme, ˇze pro nˇekter´e (zejm´ena jednoduch´e plan´arn´ı) paraleln´ı manipul´atory ´ a IKU ´ s pomoc´ı geometrie, jako napˇr. kinematick´a anal´ je moˇzn´e nal´ezt ˇreˇsen´ı PKU yza PPM v [95]. Nicm´enˇe takov´ y postup je obt´ıˇznˇe algoritmizovateln´ y pro odliˇsn´e architektury manipul´ ator˚ u. Zab´ yvejme se proto nad´ale uveden´ ymi systematick´ ymi pˇr´ıstupy.
4.7.1
Dekompozice paraleln´ıho mechanismu na nez´ avisl´ e uzavˇ ren´ e kinematick´ eˇ retˇ ezce
Pˇredpokl´ adejme manipul´ ator s L klouby a n pohybliv´ ymi rameny (bez zapoˇcten´ı pevn´e z´akladny). Vzhledem k moˇznosti popsat takov´ y manipul´ator cyklick´ ym plan´arn´ım grafem, kde ramena jsou reprezentov´any jeho vrcholy a klouby jeho hranami je moˇzn´e z Eulerova vzorce 1 pro plan´ arn´ı graf z´ıskat vztah: (n + 1) − |{z} L + | {z }
vrcholy grafu
hrany
(B + 1) | {z }
=2
⇒
B =L−n
(4.86)
uzavˇren´ e oblasti
kde n + 1 oznaˇcuje poˇcet ramen manipul´atoru vˇcetnˇe pevn´e z´akladny a B + 1 poˇcet oblast´ı ohraniˇcen´ ymi hranami grafu vˇcetnˇe vnˇejˇs´ı nekoneˇcn´e oblasti, kde B v naˇsem pˇr´ıpadˇe oznaˇcuje poˇcet nez´ avisl´ ych uzavˇren´ ych kinematick´ ych ˇretˇezc˚ u (smyˇcek). 1
Pro plan´ arn´ı graf s V vrcholy, E hranami, kter´e se vz´ ajemnˇe nekˇr´ıˇz´ı a F oblastmi ohraniˇcen´ ymi hranami (vˇcetnˇe vnˇejˇs´ı oblasti s nekoneˇcn´ ym obsahem) plat´ı vztah: V − E + F = 2.
147
´ 4. UVOD DO ROBOTIKY
Obr´azek 4.26 ukazuje odpov´ıdaj´ıc´ı plan´arn´ı grafy PPM z Kapitoly 4.4.2 a pr˚ umyslov´eho s´erioparaleln´ıho manipul´ ator s oznaˇcen´ım AKR-3000.
(a) PPM, B = 5 − 4 = 1
(b) AKR-300, B = 8−6 = 2 (Link 1 je z´ akladnou pro paraleln´ı ˇca ´st manipul´ atoru)
Obr´ azek 4.26: Paraleln´ı manipul´ atory a jejich reprezentace pomoc´ı plan´arn´ıho grafu s urˇcen´ım poˇctu nez´ avisl´ ych uzavˇren´ ych kinematick´ ych ˇretˇezc˚ u (podtrˇzen´ı oznaˇcuje aktivn´ı klouby manipul´ atoru)
Pro transformaci mezi jednotliv´ ymi s.s. um´ıstˇen´ ych v kloubech paraleln´ıho manipul´atoru lze s v´ yhodou vyuˇz´ıt K-K u ´mluva, viz Kapitola 4.5.2.2 (D-H u ´mluva v tomto pˇr´ıpadˇe pouˇz´ıt nelze). Vytvoˇrme nyn´ı ekvivalentn´ı stromovou strukturu z p˚ uvodn´ıho paraleln´ıho manipul´atoru rozpojen´ım vˇsech uzavˇren´ ych kinematick´ ych ˇretˇezc˚ u v jednom z jejich pasivn´ıch kloub˚ u n´asleduj´ıc´ım zp˚ usobem, viz Obr´ azek 4.27. Pˇredpokl´adejme pouze klouby s 1 DoF (typ P a R): • Definujme funkci i = a(j), kter´ a vrac´ı ˇc´ıslo s.s. Fi bezprostˇrednˇe pˇredch´azej´ıc´ıho s.s. Fj . • Vyberme pasivn´ı klouby, ve kter´ ych provedeme rozpojen´ı a oznaˇcme je Joint k,
148
4.7 Paraleln´ı manipul´ atory
kde k = n + 1 . . . L. V kaˇzd´em rozpojen´em kloubu definujme s.s. Fk , kter´ y je pevnˇ e spojen s jedn´ım ramenem k tomuto kloubu pˇripojen´emu, napˇr. Link j. Osa z k je totoˇzn´ a s osou rozpojen´eho kloubu a osa xk je um´ıstˇena ve smˇeru norm´ aly os z j a z k .
Ostatn´ı klouby oznaˇcme jako Joint 1 . . . Joint n a um´ıstˇeme je dle K-K u ´mluvy.
Je zˇrejm´e, ˇze transformaˇcn´ı matice T jk mezi pevnˇe sv´azan´ ymi s.s. Fj a Fk je konstantn´ı matice, popisuj´ıc´ı pouze geometrii ramene Link j.
• Transformaˇcn´ı matice T ik , kde i = a(k) oznaˇcuje ˇc´ıslo druh´eho ramene Link i pˇripojen´eho ke kloubu Joint k, je definov´ana obecnˇe 6 parametry K-K u ´mluvy: αk , ak , dk , θk , γk , bk (kloubov´a souˇradnice Joint k je opˇet ϑk = θk pro kloub typu R a ϑk = dk pro kloub typu P, ostatn´ı parametry definuj´ı pouze geometrii ramene Link i).
• Jelikoˇz geometrick´e parametry konstantn´ı transformaˇcn´ı matice T jk a transformaˇcn´ı matice T ik rozpojen´eho kloubu Joint k by mˇely stejn´ y doln´ı index k, zavedeme jeˇstˇe pro kaˇzd´ y kloub Joint k pˇr´ıdavn´ y s.s. Fk+B , kter´ y je totoˇzn´ y se s.s. Fk (Fk+B ≡ Fk ). Konstantn´ı transformaˇcn´ı matici T jk pak tedy d´ale nahrad´ıme konstantn´ı transformaˇcn´ı matic´ı T jk+B s geometrick´ ymi parametry αk+B , ak+B , dk+B = 0, θk+B = 0, γk+B , bk+B .
149
´ 4. UVOD DO ROBOTIKY
Obr´ azek 4.27: Princip rozpojen´ı uzavˇren´eho kinematick´eho ˇretˇezce
Je zˇrejm´e, ˇze v pˇr´ıpadˇe paraleln´ıch manipul´ator˚ u m˚ uˇzeme tedy vektor kloubov´ ych souˇradnic Θ d´ale dˇelit na subvektor N nez´ avisl´ ych aktivn´ıch kloubov´ ych souˇradnic (aktu´ator˚ u) Θa a L − N pasivn´ıch kloubov´ ych souˇradnic (nepoh´anˇen´e klouby) Θp : Θa Θ= Θp
(4.87)
Vzhledem k faktu, ˇze v geometrick´em popisu manipul´atoru se vyskytuje pouze N nez´avisl´ ych promˇenn´ ych Θa , mus´ı zde pro staticky urˇcen´ y manipul´ator1 existovat pr´avˇe 1
Manipul´ ator, jehoˇz koncov´ y efektor nevykazuje ˇza ´dn´ y n´ ych”aktu´ atorech (aktivn´ı kloubov´e souˇradnice jsou konstantn´ı).
150
stupeˇ n
volnosti
pˇri
”uzamˇce-
4.7 Paraleln´ı manipul´ atory
L − N nez´ avisl´ ych rovnic definuj´ıc´ıch vztah mezi aktivn´ımi Θa a pasivn´ımi Θp kloubov´ ymi souˇradnicemi manipul´atoru. Tyto omezuj´ıc´ı rovnice lze z´ıskat z podm´ınky na uzavˇren´ı kinematick´e ˇretˇezce v rozpojen´em kloubu Joint k, nebot’ nutnˇe plat´ı: a(i)
T ca(a(...a(i))) · · · · · T i
a(j)
· T ik = T ca(a(...a(j))) · · · · · T j
· T jk+B
(4.88)
kde Fc souˇradn´ y syst´em ramene Link c, kter´e je prvn´ım spoleˇcn´ ym ramenem na cestˇe smˇerem od rozpojen´eho kloubu Joint k v obou vˇetv´ıch kinematick´eho ˇretˇezce. ˇ sen´ı rovnice (4.88) vede tedy na stanoven´ı hodnoty pasivn´ıch kloubov´ Reˇ ych souˇradnic Θp v z´ avislosti na hodnot´ ach souˇradnic aktivn´ıch Θp definovan´e vztahem: Θp = g(Θa )
(4.89)
Je zˇrejm´e, ˇze rovnice (4.88) soustavou 12 neline´arn´ıch rovnic pro maxim´alnˇe 6 nez´avisl´ ych promˇenn´ ych (obecn´ y pohyb v prostoru je pops´an celkem 6 nez´avisl´ ymi promˇenn´ ymi 3 pro translaci + 3 pro rotaci, uvedenou soustava tak lze redukovat na nejv´ yˇse 6 nez´ avisl´ ych rovnic). Z uveden´eho vypl´ yv´a, ˇze pro existenci koneˇcn´eho poˇctu izolovan´ ych ˇreˇsen´ı rovnice (4.88) mus´ı plan´arn´ı uzavˇren´ y kinematick´ y ˇretˇezec obsahovat maxim´alnˇe 3 a prostorov´ y kinematick´ y ˇretˇezec maxim´alnˇe 6 pasivn´ıch kloub˚ u. Poznamenejme, ˇze rovnici (4.88) lze pˇrepsat do tvaru: a(a(...a(j)))
a(i)
T k+B · T ja(j) · · · · · T a(a(...a(i))) · · · · · T i j
· T ik = I
(4.90)
kde nezn´ am´ ymi jsou kloubov´e souˇradnice pasivn´ıch kloub˚ u Θp . ´ z Kapitoly 4.6.2. Z tohoto d˚ Nalezen´ı funkce g je tedy podobn´e ˇreˇsen´ı IKU uvodu je ˇreˇsen´ı rovnice (4.88) obecnˇe komplikovan´e. Speci´aln´ı pˇr´ıpad tvoˇr´ı paraleln´ı manipul´atory, kde kaˇzd´ y uzavˇren´ y kinematick´ y ˇretˇezec obsahuje pr´avˇe 3 pasivn´ı klouby, nebo lze na tuto situaci postupnˇe pˇrev´ezt. Takov´ y pˇr´ıpad nast´av´a pro oba typy uvaˇzovan´ ych manipul´ ator˚ u (v technick´e praxi tento pˇr´ıpad b´ yv´a velmi ˇcast´ y): • PPM obsahuje jeden uzavˇren´ y kinematick´ y ˇretˇezec pr´avˇe se 3 pasivn´ı klouby Joint 2, Joint 4, Joint 5 • AKR-3000 obsahuje dva uzavˇren´e kinematick´e ˇretˇezce, kde vˇsak prvn´ı z nich obsahuje pouze 3 pasivn´ı klouby Joint 2, Joint 4, Joint 12. Jejich polohy mohou b´ yt tedy stanoveny v z´ avislosti na poloze aktivn´ıho kloubu Joint 6. Druh´ y uzavˇren´ y kinematick´ y ˇretˇezec pak obsahuje opˇet pouze 3 pasivn´ı klouby Joint 3, Joint 7 Joint 11, nebot’ poloha kloubu Joint 2 je jiˇz zn´am´a.
151
´ 4. UVOD DO ROBOTIKY
N´ astin metody (3 pasivn´ı klouby): Metodu lze nal´ezt v [96], [97] a pˇr´ıklad jej´ıho pouˇzit´ı pro manipul´ator AKR-3000 pak v [81]. Oznaˇcme 3 pasivn´ı klouby v uzavˇren´em kinematick´em ˇretˇezci jako Joint i, Joint j, Joint k a pˇredpokl´ adejme, ˇze zn´ ame hodnoty vˇsech aktivn´ıch kloubov´ ych souˇradnic Θa . Rovnici (4.90) lze pro obecn´ y pˇr´ıpad ps´at jako: T ik+B (ϑi ) · T ij (ϑj ) · T jk (ϑk ) = I
(4.91)
kde matice pˇrechodu jsou z´ avisl´e na hledan´ ych hodnot´ach pasivn´ıch kloubov´ ych sou T ˇradnic Θp = ϑi ϑj ϑk . Lze uk´azat, ˇze rovnice (4.91) vede na podobn´e rovnice, jako je polohov´a a rotaˇcn´ı rovnice v Kapitole 4.6.2.2 s t´ım rozd´ılem, ˇze tyto rovnice jsou funkcemi stejn´ ych promˇenn´ ych (ϑi , ϑj , ϑk ) a mus´ı b´ yt ˇreˇseny souˇcasnˇe. 4.7.1.1
Pˇ r´ım´ y kinematick´ y probl´ em
Vybereme nejkratˇs´ı cestu (vzhledem k poˇctu kloub˚ u) od z´akladny manipul´atoru do jeho koncov´eho efektoru, kterou lze jednoznaˇcnˇe popsat posloupnost´ı transformaˇcn´ıch matic, analogicky k (4.42): a(a(n))
T 0n (Θ) = T 0i · . . . T a(n)
· T na(n)
(4.92)
Pokud nejkratˇs´ı cesta obsahuje nˇekterou z pasivn´ıch kloubov´ ych souˇradnic Θp , mus´ı b´ yt tyto souˇradnice vyj´ adˇreny jako funkce aktivn´ıch kloubov´ ych souˇradnic, ˇreˇsen´ım rovnice (4.91). 4.7.1.2
Inverzn´ı kinematick´ y probl´ em
Vybereme opˇet nejkratˇs´ı cestu od z´ akladny manipul´atoru do jeho koncov´eho efek´ pro s´eriov´e manipul´atory z Kapitoru. T´ım p´ adem jsme u ´lohu pˇrevedli na ˇreˇsen´ı IKU toly 4.6.2. Metody z uveden´e kapitoly vyuˇz´ıvaj´ıc´ı popis s.s. pomoc´ı K-K metody pˇredpokl´adaj´ı, ˇze geometrick´e parametry bj a γj jsou nulov´e, viz Obr. 4.16. Pokud toto nelze zaˇr´ıdit jiˇz vhodnou volbou s.s., je moˇzn´e geometrick´e parametry bj respektive γj slouˇcit s kloubov´ ymi souˇradnicemi di respektive θi , nebot’ pro dvˇe po sobˇe bezprostˇrednˇe
152
4.7 Paraleln´ı manipul´ atory
n´asleduj´ıc´ı ramena Link i a Link j plat´ı, viz (4.40): a(i)
Ti
· T ij = Trans(z, bi ) · Rot(z, γi ) · Trans(x, ai ) · Rot(x, αi ) · Trans(z, di ) · Rot(z, θi ) · | {z } a(i)
Ti
· Trans(z, bj ) · Rot(z, γj ) · Trans(x, aj ) · Rot(x, αj ) · Trans(z, dj ) · Rot(z, θj ) | {z } T ij
Tedy zˇrejmˇe plat´ı: a(i)
Ti
· T ij = Trans(z, bi ) · Rot(z, γi ) · Trans(x, ai ) · Rot(x, αi ) · Trans(z, di + bj ) · Rot(z, θi + γj ) · {z } | a(i)
Ti
· Trans(x, aj ) · Rot(x, αj ) · Trans(z, dj ) · Rot(z, θj ) | {z } T ij
4.7.2
Dekompozice paraleln´ıho mechanismu na s´ eriov´ e manipul´ atory
´ a PKU ´ uveden´ Jak jiˇz bylo ˇreˇceno, postup v´ ypoˇctu IKU y v Kapitole 4.7.1 je moˇzn´ y pˇr´ımo aplikovat pouze na typy paraleln´ıch manipul´ator˚ u, kter´e obsahuj´ı alespoˇ n jeden uzavˇren´ y kinematick´ y ˇretˇezec s nejv´ yˇse 3 pasivn´ımi klouby (plan´arn´ı manipul´atory) nebo 6 pasivn´ımi klouby (prostorov´e manipul´atory) a vˇsechny zb´ yvaj´ıc´ı uzavˇren´e kinematick´e ˇretˇezce je moˇzn´e postupn´ ym v´ ypoˇctem kloubov´ ych souˇradnic do tohoto stavu pˇrev´ezt. V takov´e situaci m˚ uˇze b´ yt kaˇzd´ y uzavˇren´ y kinematick´ y ˇretˇezec ch´ap´an jako staticky urˇcen´ y a nalezen´ı z´ avislosti mezi aktivn´ımi Θa a pasivn´ımi Θp souˇradnicemi manipul´ atoru je v obecn´em pˇr´ıpadˇe d´ano ˇreˇsen´ım rovnice (4.90), coˇz je analogick´ y pro´ pro s´eriov´e manipul´atory (koneˇcn´ bl´em k probl´emu stanoven´ı IKU y poˇcet izolovan´ ych ˇreˇsen´ı). Z definice pro obecn´ y paraleln´ı manipul´ator vˇsak plat´ı, ˇze koncov´ y efektor je pˇripojen k z´ akladnˇe manipul´ atoru alespoˇ n dvˇema nez´avisl´ ymi otevˇren´ ymi kinematick´ ymi ˇretˇezci. Obecnˇe nemus´ı b´ yt splnˇena podm´ınka, ˇze uzavˇren´e kinematick´e ˇretˇezce jsou staticky urˇcen´e (tedy obsahuj´ı nejv´ yˇse 3 respektive 6 pasivn´ıch kloub˚ u). Pˇr´ıklady takov´ ych manipul´ ator˚ u, vˇcetnˇe jejich grafick´e reprezentace zn´azorˇ nuje Obr´azek 4.28.
153
´ 4. UVOD DO ROBOTIKY
(a) Plan´ arn´ı manipul´ ator, B = 9 − 7 = 2
(b) PSZ, B = 10 − 7 = 3
Obr´ azek 4.28: Paraleln´ı manipul´ atory a jejich reprezentace pomoc´ı plan´arn´ıho grafu (staticky neurˇcen´e uzavˇren´e kinematick´e ˇretˇezce)
Vˇsimnˇeme si, ˇze v pˇr´ıpadˇe plan´ arn´ıho manipul´atoru je koncov´ y efektor pˇripojen k z´akladnˇe 3 identick´ ymi nez´ avisl´ ymi otevˇren´ ymi kinematick´ ymi ˇretˇezci typu RRR a kaˇzd´ y ze dvou nez´ avisl´ ych uzavˇren´ ych kinematick´ y ˇretˇezc˚ u tedy obsahuje celkem 4 pasivn´ı klouby typu R (4 pasivn´ı kloubov´e souˇradnice). V pˇr´ıpadˇe PSZ je koncov´ y efektor pˇripojen k z´ akladnˇe 3 identick´ ymi kinematick´ ymi ˇretˇezci typu PUS a jedn´ım pasivn´ım kinematick´ ym ˇretˇezcem tvoˇren´ ym samostatn´ ym kloubem S. Dva nez´avisl´e uzavˇren´e kinematick´e ˇretˇezce tak obsahuj´ı celkem 10 pasivn´ıch kloub˚ u s 1 DoF (S=(RRR), U=(RR)) a zb´ yvaj´ıc´ı celkem 9 pasivn´ıch kloub˚ u s 1 DoF. V obou pˇr´ıpadech nem˚ u-
154
4.7 Paraleln´ı manipul´ atory
ˇzeme nal´ezt ani jeden staticky urˇcen´ y uzavˇren´ y kinematick´ y ˇretˇezec. 4.7.2.1
Inverzn´ı kinematick´ y probl´ em
´ pro obecn´ IKU y paraleln´ı manipul´ator lze obecnˇe ˇreˇsit n´asleduj´ıc´ım zp˚ usobem, viz [98]. Pˇredpokl´ adejme paraleln´ı manipul´ator s n nez´avisl´ ymi kinematick´ ymi ˇretˇezci, viz Obr´azek 4.29. Poloha z´ akladny manipul´atoru je jednoznaˇcnˇe urˇcena s.s. F0 a poloha koncov´eho efektoru pak s.s. Fe . Poloha i-t´eho kinematick´eho ˇretˇezce (v podstatˇe s´eriov´eho manipul´ atoru) je d´ ana rovnic´ı:
ˇ sen´ı IKU ´ obecn´eho paraleln´ıho manipul´atoru Obr´ azek 4.29: Reˇ
−−−→b −−−→b −−−→b −−−→b Ai B i = Ai O b + O b O e + O e B i
(4.93)
at Je zˇrejm´e, ˇze rovnici (4.93) lze se znalost´ı zobecnˇen´ ych souˇradnic X = Rbe O be ps´ jako: −−−→ −−−→ −−−→ −−−→ (i) H 1 (X) = Ai B i b = Ai O b b + O b O e b +Rbe · O e B i e | {z }
(4.94)
O be
−−−→ −−−→ kde Ai O b b respektive O e B i e jsou konstantn´ı vektory dan´e n´avrhov´ ymi parametry ξ manipul´ atoru urˇcuj´ıc´ı m´ısto pˇripojen´ı uzavˇren´ ych kinematick´ ych ˇretˇezc˚ u k z´akladnˇe respektive koncov´emu efektoru manipul´atoru.
155
´ 4. UVOD DO ROBOTIKY −−−→ Pˇr´ım´ y kinematick´ y probl´em, tedy opˇet poloha vektoru Ai B i , i-t´eho kinematick´eho ˇretˇezce m˚ uˇze b´ yt pro jeho kloubov´e souˇradnice Θi (pasivn´ı i aktivn´ı) vyj´adˇren rovnic´ı: −−−→ (i) H 2 (Θi ) = Ai B i b
(4.95)
Zat´ımco struktura rovnice (4.94) je stejn´a pro vˇsechny typy paraleln´ıch manipul´ator˚ u, struktura rovnice (4.95) z´ avis´ı striktnˇe na typu pouˇzit´ ych kinematick´ y ˇretˇezc˚ u. Inverzn´ı kinematick´ y probl´em pro obecn´ y paraleln´ı manipul´ator je pak d´an ˇreˇsen´ım soustavy rovnic:
(1) (1) H 1 (X) H 2 (Θ1 ) .. .. . . (i) (i) H 1 (X) = H 2 (Θi ) .. .. . . (n) (n) H (X) H (Θk ) | 1 {z } | 2 {z } H1 (X)
(4.96)
H2 (Θ)
´ zn´am´a a konstantn´ı Vzhledem k tomu, ˇze vektorov´ a funkce H1 (X) je v pˇr´ıpadˇe IKU a vektorov´a funkce H2 (Θ) m˚ uˇze b´ yt stanovena analogicky jako v pˇr´ıpadˇe s´eriov´ ych ´ pro s´eriov´e mamanipul´ator˚ u, viz Kapitola 4.6.1, jedn´a se v podstatˇe o ˇreˇsen´ı IKU nipul´atory z Kapitoly 4.6.2, kde bychom stanovili vˇsechny kloubov´e souˇradnice Θi (pasivn´ı + aktivn´ı) kaˇzd´eho i-t´eho kinematick´eho ˇretˇezce. Poznamenejme, ˇze kinematick´e ˇretˇezce tvoˇr´ıc´ı s´eriov´e manipul´ atory budou m´ıt v neredundantn´ım pˇr´ıpadˇe pr´avˇe 2 respektive 3 nez´ avisl´e kloubov´e souˇradnice (pasivn´ı + aktivn´ı) pro plan´arn´ı respektive prostorov´e manipul´ atory, nebot’ koncov´ y efektor tˇechto s´eriov´ ych manipul´ator˚ u je polohov´an, d´ıky pˇripojen´ı kloubem typu R respektive S, nez´avisle na sv´e orientaci (bod pˇripojen´ı kinematick´eho ˇretˇezce na koncov´ y efektor paraleln´ıho manipul´atoru je urˇcen −−−→ jen svou polohou, vektorem Ai B i ). Poˇcet aktivn´ıch a pasivn´ıch kloub˚ u vˇsech kinematick´ ych ˇretˇezc˚ u (bez uvaˇzov´ an´ı souˇradnic kloub˚ u pˇripojuj´ıc´ı koncov´ y efektor) je tedy roven L = 2 · n respektive L = 3 · n pro plan´arn´ı respektive prostorov´e manipul´atory. Rovnice (4.96) je tak soustavou L rovnic pro stejn´ y poˇcet nezn´am´ ych (pasivn´ı+aktivn´ı kloubov´e souˇradnice vˇsech d´ılˇc´ıch s´eriov´ ych manipul´ator˚ u). Je zˇrejm´e, ˇze v pˇr´ıpadˇe staticky urˇcen´eho neredundantn´ıho paraleln´ıho manipul´atoru je poˇcet N nez´ avisl´ ych aktivn´ıch kloubov´ y souˇradnic Θa roven poˇctu DoF koncov´eho efektoru manipul´ atoru a pˇri uzamˇcen´ı tˇechto N aktivn´ıch kloubov´ ych souˇradnic
156
4.7 Paraleln´ı manipul´ atory
(Θa = konst.) mus´ı existovat koneˇcn´ y poˇcet izolovan´ ych ˇreˇsen´ıch rovnice (4.96), odpov´ıdaj´ıc´ı nˇejak´e konfiguraci manipul´atoru. Lze tedy, podobnˇe jako v Kapitole 4.7.1, z´ıskat L − N nez´ avisl´ ych rovnic definuj´ıc´ı vztah mezi aktivn´ımi Θa a pasivn´ımi Θp kloubov´ ymi souˇradnicemi manipul´ atoru. V odborn´e literatuˇre se ˇcasto setk´av´ame s tvrzen´ım, ´ pro paraleln´ı manipul´atory je obecnˇe analyticky ˇreˇsiteln´a. Je zˇrejm´e, ˇze toto ˇze IKU tvrzen´ı neplat´ı, nebot’ pro sloˇzit´e kinematick´e ˇretˇezce je ˇreˇsen´ı rovnice (4.96) komplikovan´e V mnoha pˇr´ıpadech pr˚ umyslov´ ych manipul´ator˚ u jsou vˇsak kinematick´e ˇretˇezce spojuj´ıc´ı z´ akladnu a koncov´ y efektor manipul´atoru jednoduch´e. Velmi ˇcast´ ym pˇr´ıpad v praxi tvoˇr´ı prostorov´e paraleln´ı manipul´atory s kinematick´ ymi ˇretˇezci typu UPS (jedna ak´ d´ana pˇr´ımo ˇreˇsen´ım rovnice: tivn´ı kloubov´ a souˇradnice di ) pro kter´e je IKU −−−→ (i) di = kAi B i k = kH 1 (X)k
(4.97)
ˇ Casto se tak´e m˚ uˇzeme setkat s plan´arn´ımi manipul´atory s kinematick´ ymi ˇretˇezci typu ´ pro s´eriov´ RRR, RPR, ... pro kter´e je ˇreˇsen´ı rovnice (4.96) analogick´e s pˇr´ıpadem IKU y manipul´ ator typu PSM-RR z Pˇr´ıkladu (4.4). ´ pro PSZ) F Pˇ r´ıklad 4.8 (IKU Pˇredpokl´ adejme, ˇze zn´ ame zobecnˇen´e souˇradnice PSZ z Kapitoly 4.4.3. X=
h
α β γ
iT
Poloha i-t´eho kinematick´eho ˇretˇezce s pˇr´ıpojn´ ymi body B i , D i k z´akladnˇe a koncov´emu efektoru je tedy vyj´ adˇrena analogicky jako v rovnici (4.94), i = 1 . . . 3: −−−→ −−−→ −−−→ −−−→ −−−→ (i) H 1 (X) = B i D i b = B i D i b (X) = B i O b b + O b O e b + Rbe O e D i e kde
−−−→b B i O b = −B bi ,
−−−→e O e D i = D ei ,
iT −−−→b h Ob Oe = 0 0 v
A pˇr´ıpojn´e body z´ akladny B i a koncov´eho efektoru D i lze vyj´adˇrit pomoc´ı n´avrhov´ ych parametr˚ u ξ jako: B b1 =
h
√
3 6 a1
− 12 a1
0
iT
, B b2 =
h
√
3 6 a1
1 2 a1
0
iT
, B b3 =
vzhledem k s.s. Fb a h √ iT h √ iT h e e 3 3 1 D e1 = = = , D , D − 3 a2 0 0 2 3 6 a2 2 a2 0
157
h
√
√
−
3 6 a2
3 3 a1
0 0
− 12 a2
0
iT
iT
´ 4. UVOD DO ROBOTIKY
vzhledem k s.s. Fe . Matice rotace ud´ avaj´ıc´ı vz´ ajemnou orientaci koncov´eho efektoru vzhledem k z´akladnˇe manipul´ atoru lze vyj´ adˇrit jako: Rbe = Rbe (X) = R1 (α) · R2 (β) · R3 (γ) = 1 0 0 cos(β) 0 sin(β) cos(γ) − sin(γ) 0 = 0 cos(α) − sin(α) · 0 1 0 · sin(γ) cos(γ) 0 0 sin(α) cos(α) − sin(β) 0 cos(β) 0 0 1 ´ i-t´eho kinematick´eho ˇretˇezce lze pak vyj´adˇrit jako v rovnici (4.95): PKU −−−→ (i) ¯ b · u0 · l H 2 (Θ) = B i D i b = ubi · di + R i i −−−→ −−−→ kde ui = B i C i b /kB i C i b k je jednotkov´ y vektor smˇeru aktu´ator˚ u vzhledem k s.s. Fb (u1 = u2 = u3 = u), 1 0 0 cos ϑyi 0 sin ϑyi ¯b = R 1 0 0 cos ϑxi − sin ϑxi · 0 i 0 sin ϑxi cos ϑxi − sin ϑyi 0 cos ϑyi a ϑxi , ϑyi jsou Eulerovy u ´hly urˇcuj´ıc´ı natoˇcen´ı ramene C i D i podle sch´ematu XY vzhledem k s.s. Fb . Vektor kloubov´ ych souˇradnic manipul´atoru Θ je tedy tvoˇren pasivn´ımi (ϑxi , ϑyi ) a aktivn´ımi (di ) kloubov´ ymi souˇradnicemi. Rovnice (4.96) tak vede na soustavu 9 rovnic pro 9 nezn´ am´ ych (Θ). Zafixov´ an´ım aktivn´ıch kloubov´ ych souˇradnic di lze soustavu rovnic pˇrev´ezt na 6 nez´ avisl´ ych rovnic pro 6 pasivn´ıch kloubov´ ych souˇradnic (ϑxi , ϑyi ), jej´ıˇz ˇreˇsen´ı vede na vztah mezi pasivn´ımi a aktivn´ımi kloubov´ ymi souˇradnicemi manipul´atoru. Pokud vˇsak nepotˇrebujeme zn´at hodnoty pasivn´ıch kloubov´ ych souˇradnic, ´ pro PSZ m´a dvˇe lze s v´ yhodou vyuˇz´ıt metodu z Pˇr´ıkladu 4.6, ze kter´eho plyne, ˇze IKU r˚ uzn´a ˇreˇsen´ı. F ´ pro PPM) F Pˇ r´ıklad 4.9 (IKU Pro pˇr´ıpad PPM z Kapitoly 4.4.2 je ˇreˇsen´ı inverzn´ı kinematick´e u ´lohy zˇrejm´e, nebot’: iT +C −l3 0 h iT (2) H 1 (X) = − l3 0 +C (1)
H 1 (X) = −
h
´ PSM-RR z Pˇr´ıkladu 4.4. a ˇreˇsen´ı soustavy rovnic (4.96) je ekvivalentn´ı s ˇreˇsen´ım IKU Alternativn´ı pˇr´ıstup k ˇreˇsen´ı kinematiky je podrobnˇe pops´an v [95]. F
158
4.7 Paraleln´ı manipul´ atory
4.7.2.2
Pˇ r´ım´ y kinematick´ y probl´ em
Pˇr´ım´ a kinematick´ au ´loha pro paraleln´ı manipul´atory dodnes z˚ ust´av´a otevˇren´ ym probl´emem, nebot’ jej´ı ˇreˇsen´ı nen´ı v obecn´em pˇr´ıpadˇe jednoznaˇcn´e a vede na v´ıce moˇznost´ı, jak ”sloˇzit”paraleln´ı manipul´ ator pro dan´e hodnoty kloubov´ ych souˇradnic, tzv. assembly ´ paraleln´ıch manipul´ator˚ modes. Nejˇcastˇeji kladen´e ot´ azky pˇri vyˇsetˇrov´an´ı PKU u jsou ´ s´eriov´ tyto n´ asleduj´ıc´ı. Poznamenejme, ˇze stejn´e ot´azky vyvst´avaj´ı i pˇri ˇreˇsen´ı IKU ych manipul´ ator˚ u, vˇsak zejm´ena pro paraleln´ı manipul´atory s komplikovanˇejˇs´ı architekturou jsou tyto ot´ azky kl´ıˇcov´e pro d˚ ukladnou kinematickou anal´ yzu. ´ (poˇcet assembly modes)? • Jak´ y je poˇcet ˇreˇsen´ı PKU ´ nebo je nutno pouˇz´ıt numerick´ • Existuje analytick´e ˇreˇsen´ı PKU ych metod? • Nalezen´ı efektivn´ıho algoritmu (rychl´eho, numericky stabiln´ıho, pˇresn´eho,..) k v´ y´ poˇctu PKU? ´ z mnoˇziny pˇr´ıpustn´ • Jak rozhodnout o spr´ avn´em ˇreˇsen´ı PKU ych ˇreˇsen´ı? Vzhledem k faktu, ˇze architektury paraleln´ıch manipul´ator˚ u mohou b´ yt velmi roz´ pro obecn´ manit´e, je v podstatˇe nemoˇzn´e nal´ezt jeden univerz´aln´ı pˇr´ıstup k ˇreˇsen´ı PKU y pˇr´ıpad paraleln´ıho manipul´ atoru (numerick´e metody zat´ım ponechme stranou). Omez´ıme tedy naˇse zkoum´ an´ı pouze na vybran´e architektury paraleln´ıch manipul´ator˚ u a na pˇr´ıpad u ´pln´ ych paraleln´ıch manipul´ator˚ u1 Plan´ arn´ı manipul´ atory ´ pro paraleln´ı plan´arn´ı manipul´atory lze s v´ Vyˇsetˇrov´ an´ı PKU yhodou pˇrev´ezt na probl´em hled´ an´ı pr˚ useˇc´ık˚ u dvou rovinn´ ych kˇrivek. Z´akladn´ı ideou je pomysln´e rozpojen´ı manipul´ atoru v jednom z bod˚ u pˇripojen´ı nez´avisl´eho kinematick´eho ˇretˇezce na koncov´ y efektor. Pro dan´e aktivn´ı kloubov´e souˇradnice potom koncov´ y bod samostatn´eho odpojen´eho kinematick´eho ˇretˇezce opisuje jednu rovinnou kˇrivku (oznaˇcme ji d´ale jako Γ). Zb´ yvaj´ıc´ı ˇc´ ast koncov´eho efektoru (tzv. coupler ) opisuje bodem, ke kter´emu byl p˚ uvodnˇe pˇripojen oddˇelen´ y kinematick´ y ˇretˇezec, pak druhou rovinnou kˇrivku (tzv. coupler curve, 1
Upln´ y paraleln´ı manipul´ ator (fully parallel manipulator ) - neredundantn´ı paraleln´ı manipul´ ator (N DoF a N nez´ avisl´ ych aktivn´ıch kloubov´ ych souˇradnic), kde kaˇzd´ y kinematick´ y ˇretˇezec obsahuje pr´ avˇe jednu aktivn´ı kloubovou souˇradnici
159
´ 4. UVOD DO ROBOTIKY
´ oznaˇcme ji d´ ale Σ). Pr˚ useˇc´ıky rovinn´ ych kˇrivek Γ a Σ pak pˇr´ımo urˇcuj´ı ˇreˇsen´ı PKU ´ tedy poˇcet pr˚ (poˇcet ˇreˇsen´ı-assembly modes,...). Poˇcet ˇreˇsen´ı PKU, useˇc´ık˚ u rovinn´ ych kˇrivek lze za dan´ ych pˇredpoklad˚ u stanovit prostˇredn´ıctv´ım tzv. B´ezoutova teor´emu, viz [99]. Pozn´ amka 4.3 (B´ ezout˚ uv teor´ em) Algebraickou rovinnou kˇrivkou Γ rozum´ıme mnoˇzinu bod˚ u v rovinˇe, vyhovuj´ıc´ı rovnici fΓ (x, y) = 0, kde fΓ (x, y) je polynom stupnˇe m v promˇenn´ ych x, y s re´aln´ ymi koeficienty. Necht’ Γ respektive Σ jsou dvˇe algebraick´e rovinn´e kˇrivky stupnˇe m respektive n, kter´e nemaj´ı spoleˇcn´ y faktor, tzn. polynomy fΓ (x, y) a fΣ (x, y) lze faktorizovat na odliˇsn´e ireducibiln´ımi polynomy (pˇr´ıpad generick´ ych kˇrivek). Potom kˇrivky Γ a Σ maj´ı maxim´alnˇe m · n pr˚ useˇc´ık˚ u v rovinˇe xy zahrnuj´ıc´ı body v nekoneˇcnu a n´asobnost pr˚ useˇc´ık˚ u. ˇ Necht’ Π je pr˚ useˇc´ık kˇrivek Γ a Σ. Rekneme, ˇze pr˚ useˇc´ık Π je n´asobnosti jedna, pokud teˇcny kˇrivky Γ a Σ v tomto bodˇe jsou r˚ uzn´e, n´asobnosti dva, pokud teˇcny spl´ yvaj´ı. Vyˇsetˇrov´ an´ı pr˚ useˇc´ık˚ u vyˇsˇs´ıch n´asobnost´ı lze nal´ezt napˇr v [100]. F Pˇ r´ıklad 4.10 (Pr˚ useˇ c´ıky rovinn´ ych kˇ rivek) Uved’me nˇekolik jednoduch´ ych pˇr´ıklad˚ u pouˇzit´ı B´ezoutova teor´emu na z´akladn´ı typy algebraick´ ych rovinn´ ych kˇrivek. Pˇ r´ımka - Pˇ r´ımka: Pˇr´ımky popisuj´ı dvˇe polynomi´ aln´ı rovnice stupnˇe 1, tedy dle Bezoutova teor´emu dost´av´ame maxim´ alnˇe 1 pr˚ useˇc´ık, coˇz souhlas´ı s geometrickou pˇredstavou. Pokud jsou vˇsak pˇr´ımky rovnobˇeˇzn´e, je zn´ amo, ˇze jejich pr˚ useˇc´ık je opˇet jeden, a to v nekoneˇcnu. Toto tvrzen´ı lze snadno dok´ azat. Necht’ m´ ame dvˇe rovnobˇeˇzn´e pˇr´ımky Γ, Σ: Γ : fΓ (x, y) = a1 x + b1 y + c1 = 0
(m = 1)
Σ : fΣ (x, y) = a1 x + b1 y + c2 = 0 (n = 1) iT h iT h dost´av´ame: Zaveden´ım homogenn´ıch souˇradnic x y → wx wy w Γ : fΓ (x, y) = a1 x + b1 y + c1 w = 0
(4.98)
Σ : fΣ (x, y) = a1 x + b1 y + c2 w = 0 Lze snadno uk´ azat, ˇze soustavˇe (4.98) vyhovuje bod s homogenn´ımi souˇradnicemi iT h , tedy bod leˇz´ıc´ı v nekoneˇcnu (w = 0) ve smˇeru vyˇsetˇrovan´ ych pˇr´ı1 − ab11 0 mek.
160
4.7 Paraleln´ı manipul´ atory
Pˇ r´ımka-Kruˇ znice Kruˇznici popisuje polynomi´ aln´ı rovnice stupnˇe 2, tedy dle Bezoutova teor´emu dost´ av´ame maxim´ alnˇe dva pr˚ useˇc´ıky. Γ : fΓ (x, y) = a1 x + b1 y + c1 = 0
(m = 1)
Σ : fΣ (x, y) = (x + a2 )2 + (y + b2 )2 − c22 = 0
(4.99) (n = 2)
Lze uk´ azat, ˇze ˇreˇsen´ı soustavy (4.99) vede na polynom 2. stupnˇe v promˇenn´e x ˇci y a existuj´ı tedy bud’ dvˇe re´ aln´e ˇreˇsen´ı (pˇr´ımka je seˇcnou kruˇznice) dvˇe imagin´arn´ı ˇreˇsen´ı (pˇr´ımka neprot´ın´ a kruˇznici) nebo jedno ˇreˇsen´ı n´asobnosti dva (pˇr´ımka je teˇcnou ke kruˇznici, viz definice n´ asobnosti pr˚ useˇc´ık˚ u uveden´e v´ yˇse). Maxim´aln´ı poˇcet pr˚ useˇc´ık˚ u v rovinˇe xy je tedy 2. Kruˇ znice-Kruˇ znice Kruˇznice popisuj´ı dvˇe polynomi´aln´ı rovnice stupnˇe 2, tedy dle Bezoutova dost´av´ame maxim´ alnˇe 4 pr˚ useˇc´ıky, coˇz vˇsak nekoresponduje s geometrickou pˇredstavou, nebot’ je zn´amo, ˇze v takov´em pˇr´ıpadˇe existuj´ı pr˚ useˇc´ıky maxim´alnˇe dva. Γ : fΓ (x, y) = (x + a1 )2 + (y + b1 )2 − c21 = 0 2
2
Σ : fΣ (x, y) = (x + a2 ) + (y + b2 ) −
c22
=0
(m = 2) (n = 2)
Zaveden´ım homogenn´ıch souˇradnic dost´av´ame: Γ : fΓ (x, y) = (x + a1 w)2 + (y + b1 w)2 − c21 w2 = 0
(4.100)
Σ : fΣ (x, y) = (x + a2 w)2 + (y + b2 w)2 − c22 w2 = 0 Je zˇrejm´e, ˇze soustavˇe rovnic (4.100) vyhovuje dvojice bod˚ u S1 , S2 s homogenn´ımi h iT souˇradnicemi 1 ±i 0 , kter´e leˇz´ı na imagin´arn´ı kruˇznici x2 + y 2 = 0. Vzhledem k faktu, ˇze imagin´ arn´ı kruˇznice nez´avis´ı na parametrech vyˇsetˇrovan´ ych kruˇznic (ai , bi , ci ), body S1 , S2 reprezentuj´ı pr˚ useˇc´ıky kter´ ychkoliv dvou kruˇznic leˇz´ıc´ı v nekoneˇcnu (w = 0). Body S1 , S2 se v literatuˇre naz´ yvaj´ı jako imaginary circular points. Obecnˇe lze tedy ˇr´ıci, ˇze dvˇe generick´e kruˇznice maj´ı vˇzdy dva pr˚ useˇc´ıky reprezentovan´e imagin´arn´ımi body S1 , S2 , tedy maxim´alnˇe pouze dva pr˚ useˇc´ıky v rovinˇe xy. Poznamenejme, ˇze algebraick´e rovinn´e kˇrivky mohou obsahovat body S1 , S2 jako dvojn´asobn´e, trojn´ aˇ ık´ sobn´e, ... pr˚ useˇc´ıky. R´ ame, ˇze takov´e kˇrivky maj´ı circularity 2, 3,.... F
161
´ 4. UVOD DO ROBOTIKY
´ plan´arn´ıch paraleln´ıch manipul´ator˚ Praktick´e vyuˇzit´ı B´ezoutova teor´emu k ˇreˇsen´ı PKU u demonstruj´ı n´ asleduj´ıc´ı pˇr´ıklady. ´ pro PPM, viz 4.4.2) F Pˇ r´ıklad 4.11 (PKU Proved’me odpojen´ı jednoho kinematick´eho ˇretˇezce z koncov´eho efektoru manipul´atoru, tedy v bodˇe C, viz Obr´ azek 4.7(a). Zafixujeme-li kloubov´e souˇradnice Θ = h iT , m˚ uˇze se coupler pohybovat po kruˇznic´ıch Γ se stˇredem v bodˇe A1 a θA θB polomˇerem l2 a koncov´ y bod odpojen´eho kinematick´eho ˇretˇezce po kruˇznici Σ se stˇredem v bodˇe B 1 a polomˇerem l2 .
Obr´ azek 4.30: Coupler curve Σ (ˇcernˇe) a kˇrivka Γ odpojen´eho kinematick´eho ˇretˇezce PPM
Dost´av´ame tedy dvˇe algebraick´e rovinn´e kˇrivky, kter´e, pro zjednoduˇsen´ı n´asledn´eho ˇreˇsen´ı, zap´ıˇseme vzhledem k s.s. F1 = A1 − x1 y 1 z 1 , viz Obr´azek 4.30. Γ : fΓ (x, y) = x2 + y 2 − l22 = 0 Σ : fΣ (x, y) = (x −
B 11 [1])2
(m = 2)
+ (y −
B 11 [2])2
(4.101) −
l22
=0
(n = 2)
kde B 11 = B b1 − Ab1 a "
# " # −l l cos θ 3 1 A Ab1 = + , 0 l1 sin θA
162
" # " # l l cos θ 3 1 B B b1 = + 0 l1 sin θB
4.7 Paraleln´ı manipul´ atory
´ je Z B´ezoutova teor´emu m˚ uˇzeme tedy rovnou usuzovat, ˇze maxim´aln´ı poˇcet ˇreˇsen´ı PKU roven 2, viz Pozn´ amka 4.3. Soustavu rovnic (4.101) lze snadno upravit na jednu rovnici reprezentovanou polynomem stupnˇe 2 v promˇenn´e y: 1 (B 11 [1])2 l22 2 1 1 2 y − B 1 [2]y + kB 1 k − =0 (4.102) 4 kB 11 k2 Rovnice (4.102) reprezentuje, podobnˇe jako v Kapitole 4.6.2.3, charakteristick´ y poly´ y1 , y2 . Souˇradnici xi pak dopoˇcnom manipul´ atoru. Dost´ av´ ame tedy dvˇe ˇreˇsen´ı PKU teme ze vztahu: kB 11 k2 − 2B 11 [2]yi xi = (4.103) 2B 11 [1] Zobecnˇen´e souˇradnice manipul´atoru v s.s. F0 pak z´ısk´ame jako, viz Obr´azek 4.30: " # xi b C i = A1 + , i = 1, 2 (4.104) yi F F Pˇ r´ıklad 4.12 (3RRR plan´ arn´ı paraleln´ı manipul´ ator) Zaj´ımavˇejˇs´ı pˇr´ıpad nast´ av´ a pro tzv. 3RRR plan´arn´ı paraleln´ı manipul´ator, kde se kaˇzd´ y ze tˇr´ı nez´ avisl´ ych kinematick´ ych ˇretˇezc˚ u spojuj´ıc´ı z´akladnu s koncov´ ym efektorem skl´ ad´ a z trojice kloub˚ u typu R se vz´ajemnˇe rovnobˇeˇzn´ ymi osami rotace, viz Obr´ azek 4.31. Aktu´ atory manipul´atoru jsou um´ıstˇeny v z´akladnˇe.
Obr´ azek 4.31: 3RRR plan´arn´ı paraleln´ı manipul´ator
163
´ 4. UVOD DO ROBOTIKY
Kloubov´e souˇradnice: Θ=
h
Zobecnˇen´e souˇradnice: X= N´avrhov´e parametry: h ξ = lA1 lA2 lB 1
lB 2
iT
θ A θ B θC
lC 1
h
Φ Oe
lC 2
u
(4.105)
iT
v
(4.106)
w Ab B b C b
i
(4.107)
kde lA1 = kAA1 k, lA2 = kA1 A2 k, lB 1 = kBB 1 k, lB 2 = kB 1 B 2 k, lC 1 = kCC 1 k, lC 2 = kC 1 O e k Uzamˇcen´ım aktivn´ıch kloubov´ ych souˇradnic Θ jsou jednoznaˇcnˇe definov´any polohy kloub˚ u typu R um´ıstˇen´ ych v bodech A1 , B 1 , C 1 jako: " # " # " # l cos θ l cos θ l cos θ A B C A B C 1 1 1 Ab1 = Ab + , B b1 = B b + , C b1 = C b + (4.108) lA1 sin θA lB 1 sin θB lC 1 sin θC Provedeme-li nyn´ı rozpojen´ı manipul´atoru v bodˇe O e , dost´av´ame tzv. plan´ arn´ı 4ramenn´y mechanismus (planar four-bar mechanism), viz [98], [101], zn´azornˇen´ y na Obr´azku 4.32, kter´ y pˇredstavuje coupler manipul´atoru.
totožný směr s osou
Obr´ azek 4.32: Plan´ arn´ı 4-ramenn´ y mechanismus (coupler manipul´atoru)
Zaved’me nov´ y s.s. F0 = O 0 − x0 y 0 z 0 . Poloha mechanismu je pak jednoznaˇcnˇe defi0 nov´ana pozic´ı O e a orientac´ı φ vzhledem k s.s. F0 . Zobecnˇen´e souˇradnice manipul´atoru X lze se znalost´ı kloubov´ ych souˇradnic Θ zpˇet vypoˇc´ıtat jako: Φ = φ + ∆Φ,
O be = Ab1 + Rb0 · O 0e
164
(4.109)
4.7 Paraleln´ı manipul´ atory
´hel kde ∆Φ = atan2 (B b1 − Ab1 )[2], (B b1 − Ab1 )[1] a Rb0 je matice rotace kolem osy z o u ∆Φ. N´ avrhov´e parametry coupleru jsou tedy lA2 , lB 2 , lA1 B1 = kA1 B 1 k, u, v, w. Vyuˇzijemeli postupu z Kapitoly 4.7.2.1, lze ps´at vektory ramen coupleru jako: " # −u −−−→0 −−−→0 A1 A2 = O 0 O e + R0e · (4.110) −v " # w −−−−→0 −−−→0 −−−→0 B 1 B 2 = B 1 O 0 + O0 O e + R0e · −v kde R0e je matice rotace kolem osy z o u ´hel reprezentovan´ y souˇradnic´ı φ. Zˇrejmˇe plat´ı: −−−→ 2 kA1 A2 0 k − lA =0 2 −−−−→0 2 kB 1 B 2 k − lB =0 2
(4.111)
Kombinac´ı rovnic (4.110, 4.111) a vyuˇzit´ı substituce (4.80) T = tan
φ 2
⇒
sin φ = cos φ =
2·T 1+T 2 1−T 2 1+T 2
(4.112)
dost´ av´ ame dvˇe polynomi´ aln´ı rovnice 2. ˇr´adu: A1 · T 2 + B1 · T + C1 = 0
(4.113)
A2 · T 2 + B2 · T + C2 = 0 kde pro O 0e =
h
x y
iT
A1 = v 2 + 2 vy + x2 + y 2 + 2 ux − lA2 2 + u2 B1 = 4 vx − 4 uy C1 = u2 − lA2 2 + x2 + y 2 + v 2 − 2 vy − 2 ux A2 = x2 + 2 wlA1 B 1 − 2 wx + 2 vy + w2 + v 2 + lA1 B 1 2 − 2 xlA1 B 1 − lB 2 2 + y 2 B2 = 4 wy + 4 vx − 4 vlA1 B 1 C2 = x2 + y 2 − lB 2 2 + lA1 B 1 2 − 2 xlA1 B 1 − 2 vy + w2 + 2 wx − 2 wlA1 B 1 + v 2 Rovnice (4.113) lze opˇet ˇreˇsit dyalitickou eliminac´ı, tzn. pˇren´asoben´ım kaˇzd´e d´ılˇc´ı rovnice promˇennou T , dost´ av´ ame dvˇe pˇr´ıdavn´e rovnice, kter´e spoleˇcnˇe s rovnicemi p˚ uvod-
165
´ 4. UVOD DO ROBOTIKY
n´ımi lze ps´at v maticov´em tvaru: T3 2 T M · T =0 1 kde
A1 0 M = A 2 0
B1 A1 B2 A2
(4.114)
C1 0 B 1 C1 C2 0 B 2 C2
Rovnice (4.114) m´ a ˇreˇsen´ı pouze za pˇredpokladu: Γ:
det(M ) = 0
(4.115)
Rovnice (4.115) je polynom stupnˇe m = 6 v promˇenn´ ych x a y (tzv. sectic), kter´ y definuje algebraickou rovinnou kˇrivku Γ. Coupler manipul´atoru se tedy m˚ uˇze pohybovat (pro uzamˇcen´e aktivn´ı kloubov´e souˇradnice manipul´atoru) takov´ ym zp˚ usobem, ˇze jeho koncov´ y bod O e reprezentovan´ y souˇradnicemi x, y opisuje kˇrivku Γ. Je zˇrejm´e, ˇze koncov´ y bod odpojen´eho kinematick´eho ˇretˇezce manipul´atoru (CC 1 O e ) se m˚ uˇze pohybovat, pro uzamˇcen´e aktivn´ı kloubov´e souˇradnice, po algebraick´e rovinn´e kˇrivce Σ typu kruˇznice definovan´e polynomem stupnˇe n = 2 opˇet v promˇenn´ ych x, y. Σ:
2 (x − cx )2 + (y − cy )2 = lC 2
(4.116)
iT = C 01 = C b1 − Ab1 cx cy Z B´ezoutova teor´emu tedy vypl´ yv´a, ˇze maxim´aln´ı poˇcet pr˚ useˇc´ık˚ u je roven m · n = 12. Nicm´enˇe, zaveden´ım homogenn´ıch souˇradnic podobn´ ym zp˚ usobem jako v PoiT h iT h dost´av´ame pro w ˆ = 0 algebraickou rovinnou zn´amce 4.3 x y → wxˆ wyˆ w ˆ kˇrivku Γ ve tvaru: 3 Γ : −16 x2 + y 2 (u + w)2 = 0 (4.117) h iT Tedy dvojice bod˚ u S1 , S2 s homogenn´ımi souˇradnicemi 1 ±i 0 tvoˇr´ı trojn´asobn´e koˇreny polynomu definuj´ıc´ı algebraickou rovinnou kˇrivku Γ (body reprezentuj´ı koˇreny rovnice opˇet nez´ avis´ı na volbˇe parametr˚ u kˇrivky). Algebraick´a rovinn´a kˇrivka typu sectic m´a proto circularity rovno 3. Uv´aˇz´ıme-li ˇze imagin´arn´ı body S1 , S2 jsou zcela urˇcitˇe i koˇreny polynomu algebraick´e rovinn´e kˇrivky Σ, viz Pozn´amka 4.3, je zˇrejm´e, ˇze pro algebraick´e rovinn´e kˇrivky Γ a Σ existuje, nez´avisle na volbˇe jejich parametr˚ u, kde
h
166
4.7 Paraleln´ı manipul´ atory
vˇzdy pr´ avˇe 2 · 3 imagin´ arn´ıch pr˚ useˇc´ık˚ u (pr˚ useˇc´ıky v nekoneˇcnu, w ˆ = 0). Algebraick´e rovinn´e kˇrivky Γ a Σ maj´ı tak maxim´alnˇe 12 − 6 = 6 re´aln´ıch pr˚ useˇc´ık˚ u v rovinˇe xy.
Pˇr´ım´ y d˚ usledek takov´eho tvrzen´ı tedy dokazuje fakt, ˇze 3RRR plan´arn´ı paraleln´ı ´ manipul´ ator m´ a maxim´ alnˇe 6 ˇreˇsen´ı PKU.
Obr´ azek 4.33 ukazuje moˇzn´e ˇreˇsen´ı polohy coupleru 3RRR plan´arn´ıho paraleln´ıho manipul´ atoru pro n´ asleduj´ıc´ı hodnoty parametr˚ u (pˇredpokl´adejme, ˇze jiˇz zn´ame polohy bod˚ u A1 , B 1 , C 1 , kter´e jsou jsou jednoznaˇcnˇe urˇceny aktivn´ımi kloubov´ ymi souˇradnicemi, viz 4.108):
u = 1, v = 3, w = 1.5, lA1 B 1 = 5, lA2 = 3, lB 2 = 3, lC 2 = 3, C 01 =
h
iT
1.5 2 (4.118)
Pro algebraick´e rovinn´e kˇrivky plat´ı:
Γ:
− 100 x6 − 300 x4 y 2 − 300 x2 y 4 − 100 y 6 + 1400 x5 + 1200 x4 y + 2800 x3 y 2 + 2400 x2 y 3 + + 1400 xy 4 + 1200 y 5 − 7200 x4 − 12000 x3 y − 10400 x2 y 2 − 12000 xy 3 − 3200 y 4 + 18500 x3 − − 2100 x2 y + 18500 xy 2 − 2100 y 3 − 19450 x2 + 168000 xy − 31450 y 2 − 30900 x + 32700 y− − 4500 = 0
Σ:
(x − 1.5)2 + (y − 2)2 − 9 = 0
167
´ 4. UVOD DO ROBOTIKY
Obr´ azek 4.33: Coupler curve - kˇrivka Γ (ˇcervenˇe) a kˇrivka Σ (ˇcernˇe) s vyznaˇcenou poˇ arkovanˇe jsou lohou koncov´eho efektoru 3RRR plan´ arn´ıho paraleln´ıho manipul´atoru. C´ vyznaˇceny trajektorie pohybu ramen plan´arn´ıho 4-ramenn´eho mechanismu. Obr´azek de´ monstruje 6 r˚ uzn´ ych ˇreˇsen´ı PKU.
Uveden´ y postup zaloˇzen´ y na B´ezoutovˇe teor´emu n´am umoˇzn ˇuje stanovit nejen poˇcet ´ moˇzn´ ych ˇreˇsen´ı PKU, ale zejm´ena geometrick´ y n´ahled na jej´ı ˇreˇsen´ı. K v´ ypoˇctu konkr´etn´ıch hodnot polohy koncov´eho efektoru manipul´atoru je vˇsak postup nevhodn´ y, nebot’ vyˇzaduje nalezen´ı ˇreˇsen´ı soustavy polynomi´aln´ıch rovnic definuj´ıc´ı kˇrivky Γ, Θ.
Rozepiˇsme vˇsak rovnice (4.111) a (4.116), definuj´ıc´ı d´elky ramen A1 A2 , B 1 B 2 ,
168
4.7 Paraleln´ı manipul´ atory
C 1 Oe : u2 + v 2 − 2 ux cos (θ) − − 2 uy sin (θ) + 2 vx sin (θ) − 2 vy cos (θ) + x2 + y 2 − lA2 2 = 0
(4.119)
v 2 + w2 + 2 vx sin (θ) − − 2 vy cos (θ) − 2 vlA1 B 1 sin (θ) + 2 wx cos (θ) + 2 wy sin (θ) − 2 wlA1 B 1 cos (θ) + + x2 − 2 xlA1 B 1 + y 2 + lA1 B 1 2 − lB 2 2 = 0
(4.120)
x2 − 2 xcx + cx 2 + y 2 − 2 ycy + cy 2 − lC 2 2 = 0
(4.121)
Vhodn´ ym seˇcten´ım rovnic dost´av´ame: x2 − 2 xcx + cx 2 + y 2 − 2 ycy + cy 2 − lC 2 2 = 0
(4.122)
(4.120)-(4.119) :
Z + Rx + Sy = 0
(4.123)
(4.121)-(4.119) :
V y + Ux + W = 0
(4.124)
(4.121) :
kde R = 2 u cos (θ) + 2 w cos (θ) − 2 lA1 B 1 S = 2 u sin (θ) + 2 w sin (θ) Z = −2 vlA1 B 1 sin (θ) − 2 wlA1 B 1 cos (θ) − u2 + w2 + lA1 B 1 2 + lA2 2 − lB 2 2 U = −2 cx − 2 sin (θ) v + 2 u cos (θ) V = 2 u sin (θ) − 2 cy + 2 cos (θ) v W = −lC 2 2 + cx 2 − v 2 + cy 2 − u2 + lA2 2 Rovnice (4.123) a (4.124) jsou line´arn´ı v promˇenn´ ych x, y. Lze tedy nal´ezt jejich ˇreˇsen´ı x=
−ZV + SW , RV − U S
y=−
RW − U Z RV − U S
(4.125)
a dosadit zpˇet do rovnice (4.122). Z´ıskan´a rovnice je tak z´avisl´a pouze na souˇradnici φ. Dosazen´ım substituce (4.112) tak dost´av´ame polynom stupnˇe 6 v promˇenn´e T (charakteristick´ y polynom manipul´atoru f(T )): f(T ) = k6 T 6 + k5 T 5 + k4 T 4 + k3 T 3 + k2 T 2 + k1 T + k0 = 0
169
(4.126)
´ 4. UVOD DO ROBOTIKY
kde koeficienty ki z´ avis´ı pouze na parametrech manipul´atoru (4.118). ˇ sen´ım charakteristick´eho polynomu (4.126) dost´av´ame opˇet maxim´alnˇe 6 re´alReˇ n´ ych koˇren˚ u Ti . Zpˇetnou substituc´ı z rovnice (4.112) dost´av´ame hodnoty souˇradnice φ h iT a z ˇreˇsen´ı (4.125) pak hodnoty souˇradnic O 0e = x y , viz Tabulka ??. Hodnoty zobecnˇen´ ych souˇradnic 3RRR manipul´atoru lze pak dopoˇc´ıtat z (4.109). Charakteristick´ y polynom manipul´ atoru z pˇr´ıkladu na Obr´azku 4.33:
f(T ) = 23850 T 6 − 11430 T 5 − 24899 T 4 + 2732 T 3 + 3932 T 2 − 238 T − 119
ˇreˇsen´ı i
x
y
φ
1 2 3 4 5 6
1.9042e+000 -7.2024e-001 -2.6528e-001 4.0200e+000 3.2735e+000 4.4214e+000
4.9726e+000 -1.7555e-002 -4.2565e-001 3.7222e-001 4.4197e+000 2.6824e+000
4.6834e-001 6.8600e-001 1.7408e+000 -3.3028e-001 -7.7052e-001 -1.2619e+000
(4.127)
ˇ sen´ı PKU ´ 3RRR plan´ Tabulka 4.6: Reˇ arn´ıho paraleln´ıho manipul´atoru (souˇradnice polohy a orientace plan´ arn´ıho 4-ramenn´eho mechanismu).
F
Protoˇze uvaˇzujeme tˇr´ıdu neredundantn´ıch u ´pln´ ych plan´arn´ıch paraleln´ıch maipul´ator˚ u, je zˇrejm´e, ˇze poˇcet DoF kaˇzd´eho nez´avisl´eho plan´arn´ıho kinematick´eho ˇretˇezce je roven 3 (translace ve smˇeru osy x a y a rotace kolem osy z), obsahuje tedy 3 nez´avisl´e klouby s 1DoF typu P ˇci R, z nichˇz je pr´avˇe jeden kloub aktu´atorem. V takov´em pˇr´ıpadˇe lze uk´ azat, ˇze moˇzn´e kombinace kinematick´ ych ˇretˇezc˚ u jsou, viz Obr´azek 4.34:
RRR, RPR, RRP, RPP, PRR, PPR, PRP
170
4.7 Paraleln´ı manipul´ atory
Obr´ azek 4.34: Moˇzn´e kombinace kinematick´ ych ˇretˇezc˚ u u ´pln´eho neredundantn´ıho plan´ arn´ıho paraleln´ıho manipul´ atoru
Kombinace PPP se neuvaˇzuje nebot’ obecnˇe 3 klouby takov´eho typu nejsou nez´avisl´ ymi (pouze 2DoF kinematick´eho ˇretˇezce). Jako aktivn´ı kloub kinematick´eho ˇretˇezce lze vybrat kloub, pˇri jehoˇz uzamˇcen´ı nedostaneme kinematick´ y ˇretˇezce typu PP. V opaˇcn´em se m˚ uˇze koncov´ y bod takov´eho kinematick´eho ˇretˇezce pohybovat neomezenˇe v rovinˇe xy. Koncov´e efektor manipul´ atoru z´ısk´av´a tak neˇriditeln´e stupnˇe volnosti. Moˇzn´e kombinace kinematick´ ych ˇretˇezc˚ uu ´pln´ ych plan´arn´ıch neredundantn´ıch manipul´ator˚ u pak vystihuje n´ asleduj´ıc´ı tabulka: RRR
RRR
RRR
RPR
RPR
RPR
RPP
RPP
PRR
PRR
PRR
PRP
PRP
PPR
PPR
RRP
RRP
RRP
Tabulka 4.7: Moˇzn´e kombinace kinematick´ ych ˇretˇezc˚ uu ´pln´ ych plan´arn´ıch neredundantn´ıch manipul´ ator˚ u.
ˇ sen´ı PKU ´ pro manipul´ Reˇ ator se tˇremi identick´ ymi kinematick´ ymi ˇretˇezci typu RRR byl podrobnˇe uk´ az´ an v Pˇr´ıkladu 4.12. Pro ostatn´ı kombinace kinematick´ ych ˇretˇezc˚ u ´ nal´ezt v [102]. z Tabulky ?? lze ˇreˇsen´ı PKU
171
´ 4. UVOD DO ROBOTIKY
Prostorov´ e manipul´ atory ´ daleko sloˇzitˇejˇs´ı a dodnes tato problemaPro prostorov´e manipul´ atory je ˇreˇsen´ı PKU tika z˚ ust´av´a otevˇren´ ym probl´emem. Omez´ıme se tedy v n´asleduj´ıc´ım textu pouze na nˇekter´e zn´am´e vybran´e architektury. Obecnˇe plat´ı, ˇze pro prostorov´e manipul´atory ´ ˇreˇsit pomˇernˇe jednoduˇse. Pˇr´ıkladem takov´e skupiny pouze s translaˇcn´ımi DoF lze PKU manipul´ator˚ u m˚ uˇze b´ yt napˇr. zn´ am´ y Delta robot, viz Obr´azek 4.35.
aktuátory R
základna
end-effector
Obr´ azek 4.35: Delta robot, CAD model (vlevo) a skuteˇcn´ y v´ yrobek firmy Abb.
Je zˇrejm´e, ˇze pro uzamˇcen´e aktu´ atory se body M i nach´azej´ı v konstatn´ı poloze. Body B i se mohou pohybovat po kruˇznic´ıch se stˇredem v bodˇe M i a polomˇerem ri definovan´ ym d´elkou ramen M i B i . Protoˇze ramena M i B i tvoˇr´ı tzv. parallelogram, −−−→ mus´ı koncov´ y efektor udrˇzovat konstantn´ı orientaci (B i C = konst.). Je tedy zˇrejm´e, ˇze bod C koncov´eho efektoru se mus´ı nal´ezat na pr˚ useˇc´ık˚ u tˇr´ı kruˇznic s polomˇerem ri − −− → a posunut´ ym stˇredem M i + B i C. Dost´av´ame tak dvojici ˇreˇsen´ı pro polohu koncov´eho efektoru, symetrickou podle z´ akladny manipul´atoru. Podstatnˇe sloˇzitˇejˇs´ı situace nast´ av´ a v pˇr´ıpadˇe manipul´ator˚ u kde koncov´ y efektoru m˚ uˇze mˇenit nejen svou translaˇcn´ı polohu, ale i orientaci (v obecn´em pˇr´ıpadˇe mluv´ıme o paraleln´ıch manipul´ atorech se vˇsemi 6 DoF). V pr˚ ubˇehu zkoum´an´ı mnoha r˚ uzn´ ych typ˚ u paraleln´ıch manipul´ ator˚ u se uk´ azalo, ˇze cel´a ˇrada jejich architektur lze pˇrev´ezt na zjednoduˇsen´e architektury zn´ azornˇen´e na Obr´azku 4.36.
172
4.7 Paraleln´ı manipul´ atory
(a) Simplified Symmetric Manipula- (b) Triangular Simplified Symmetric (c) Minimal Simplified Symmetric tor (SSM) Manipulator (TSSM) Manipulator (MSSM)
Obr´ azek 4.36: Nejzn´ amˇejˇs´ı zjednoduˇsen´e architektury (mechanismy) prostorov´ ych paraleln´ıch manipul´ ator˚ u SSM, TSSM a jeho speci´aln´ı pˇr´ıpad MSSM.
Zab´ yvejme se podrobnˇeji pouze mechanismem typu TSSM. V Pˇr´ıkladu 4.13 poz´ pro TSSM lze pouˇz´ıt, s m´ırnou modifikac´ı, i pro dˇeji uk´ aˇzeme, ˇze postup ˇreˇsen´ı PKU ´ PSZ z Kapitoly 4.4.3. Z´akladn´ı myˇslenka ˇreˇsen´ı PKU ´ pro TSSM spoˇc´ıv´ ˇreˇsen´ı PKU a v urˇcen´ı dovolen´eho pohybu bod˚ u B i ud´avaj´ıc´ıch m´ısto pˇripojen´ı nez´avisl´ ych kinematick´ ych ˇretˇezc˚ u ke koncov´emu efektoru manipul´atoru. Jelikoˇz plat´ı, ˇze d´elky kinematick´ ych ˇretˇezc˚ u jsou jednoznaˇcnˇe urˇceny hodnotami aktivn´ıch kloubov´ ych souˇradnic di = kAi B i k, lze vypozorovat, ˇze pˇr´ıpojn´ y bod B 1 troj´ uheln´ıku A1 A2 B 1 se mus´ı pohybovat po kruˇznici, se stˇredem S 1 leˇz´ıc´ım na pˇr´ımce definovan´e stranou A1 A2 a polomˇerem R1 urˇcen´ ym v´ yˇskou troj´ uheln´ıku na tuto stranu. Analogicky lze nal´ezt takov´e kruˇznice i pro vˇsechny ostatn´ı pˇr´ıpojn´e body koncov´eho efektoru. Z TSSM mechanismu pak dost´ av´ ame mechanismus se tˇremi ekvivalentn´ımi kinematick´ ymi ˇretˇezci typu RS, viz Obr´ azek 4.37(a). Zobecnˇen´ım postupu zaloˇzen´eho na B´ezoutovˇe teor´emu z Pozn´ amky 4.3, lze urˇcit poˇcet moˇzn´ ych ˇreˇsen´ı pro TSSM, nebot’ rozpojen´ım jednoho kinematick´eho ˇretˇezce od koncov´eho efektoru dost´av´ame tentokr´at prostorov´ y 4-ramenn´ y mechanismus, viz Obr´azek 4.37(b).
173
´ 4. UVOD DO ROBOTIKY
kloub S kloub S
kloub S
kloub R
kloub R kloub R
(a) 3-RS ekvivalentn´ı mechanismus
(b) Prostorov´ y 4-ramenn´ y mechanismus
Obr´ azek 4.37: Dekompozice TSSM mechanismu
Hunt [103] uk´ azal, ˇze bod B 3 se m˚ uˇze pohybovat po ploˇse, kter´a m´a s kruˇznic´ı, po kter´e se m˚ uˇze pohybovat koncov´ y bod odpojen´eho kinematick´eho ˇretˇezce S 3 B 3 ´ pro maxim´alnˇe 16 re´ aln´ ych pr˚ useˇc´ık˚ u v prostoru xyz. Maxim´aln´ı poˇcet ˇreˇsen´ı PKU TSSM je tedy roven 16. Z omezuj´ıc´ıch podm´ınek, na konstantn´ı vzd´alenost bod˚ u Bi lze sestavit n´asleduj´ıc´ı rovnice: kB 1 B 2 k = l12 ,
kB 2 B 3 k = l23 ,
kB 3 B 1 k = l31
(4.128)
ˇ sen´ı soustavy rovnic (4.128) vede na ˇreˇsen´ı soustavy polynomi´aln´ıch rovnic, viz Pˇr´ıReˇ klad 4.13. ´ pro PSZ) F Pˇ r´ıklad 4.13 (PKU ´ pro TSSM je zˇrejm´e, ˇze osy kruˇznic, po kter´ V pˇr´ıpadˇe ˇreˇsen´ı PKU ych se mohou pohybovat pˇr´ıpojn´e body B i koncov´eho efektoru leˇz´ı v jedn´e rovinˇe. Tento fakt vˇsak nen´ı nikterak omezuj´ıc´ı, protoˇze rovnice (4.128) na tomto pˇredpokladu nez´avis´ı (osy ´ pro TSSM lze tedy kruˇznic mohou zauj´ımat libovolnou polohu). Postup ˇreˇsen´ı PKU ´ PSZ, nebot’ i v tomto pˇr´ıpadˇe se mohou pˇr´ıpojn´e body koncov´eho vyuˇz´ıt pro ˇreˇsen´ı PKU efektoru (v naˇsem pˇr´ıpadˇe oznaˇcen´e jako D i ), d´ıky pasivn´ımu stabilizaˇcn´ımu elementu O b O e , pohybovat opˇet po kruˇznic´ıch (oznaˇcme je d´ale jako ki ), jejichˇz osy sd´ıl´ı spoleˇcn´ y pr˚ useˇc´ık, viz Obr´ azek 4.38.
174
4.7 Paraleln´ı manipul´ atory
Obr´ azek 4.38: Ekvivalence mezi PSZ a TSSM
Kruˇznice ki vzniknou jako pr˚ useˇc´ık dvou kulov´ ych ploch Ke , Ki zn´azornˇen´ ych na Obr´ azku 4.40. Za u ´ˇcelem zjednoduˇsen´ı nalezen´ı takov´ ych kruˇznic, zaved’me nov´ y s.s. Fi = Ci −xi y i z i . Vztah mezi s.s. Fi a s.s. z´akladny manipul´atoru Fb je d´an matic´ı rotace Rbi a vektorem translace C bi . Vektor C bi je d´an pouze translac´ı bod˚ u B bi z Pˇr´ıkladu 4.8 ve smˇeru osy z o hodnotu danou kloubov´ ymi souˇradnicemi di : C b1 =
h
√
3 6 a1
− 12 a1 d1
iT
, C b2 =
h
√
3 6 a1
1 2 a1
d2
iT
, C b3 =
h
√
−
3 3 a1
0 d3
iT
Matice rotace Rbi je urˇcena postupnou rotac´ı okolo osy z b o u ´hel ϕzi a okolo novˇe vznikl´e osy y 0 o u ´hel ϕyi , viz Obr´azek 4.39. Rotace okolo osy z b o u ´hel ϕzi : cos ϕzi − sin ϕzi 0 z b Ri = sin ϕzi cos ϕzi 0 0 0 1 kde ϕ¯z1 = − π3 , ϕ¯z2 = π3 , ϕ¯z3 = π a ϕzi = ϕ¯zi + π. Tedy: i
sin ϕzi
cos ϕz1
1
3 2√
− 12
0
1
2 3
√
−
3 2
175
− 12
(4.129)
´ 4. UVOD DO ROBOTIKY
Rotace okolo novˇe vznikl´e osy y 0 o u ´hel ϕyi : cos ϕyi 0 sin ϕyi y b Ri = 0 1 0 − sin ϕyi 0 cos ϕyi
kde
(4.130)
√
cos ϕyi
=q
3 3 a1
1 2 3 a1
,
+ (v − di )2
sin ϕyi = − q
v − di 1 2 3 a1
+ (v − di )2
V´ ysledn´a matice rotace mezi s.s. Fb a Fi : Rbi = zRbi · yRbi
Obr´ azek 4.39: Odvozen´ı matice rotace Rbi , ρ1 =
(4.131)
√
3 3 a1 ,
kC i O e k =
p ρ21 + (v − di )2
√
Obr´ azek 4.40: Pr˚ useˇc´ık koul´ı Ki a Ke definuj´ıc´ı kruˇznice ki , ρ2 =
176
3 3 a2
4.7 Paraleln´ı manipul´ atory
Kruˇznice ki lze nal´ezt ˇreˇsen´ım n´asleduj´ıc´ı soustavy rovnic, pˇredstavuj´ıc´ı rovnice koul´ı Ki a Ke v s.s. Fi : Ki : Ke :
x2 + y 2 + z 2 = l2 2 q 2 2 x − ρ1 + (v − di ) + y 2 + z 2 = ρ22
(4.132) (4.133)
Dosazen´ım rovnice (4.132) do (4.133) a vyj´adˇren´ım x-ov´e souˇradnice dost´av´ame vzd´ alenost ri stˇredu S i kruˇznice ki ve smˇeru osy xi od poˇc´atku C i s.s. Fi :
ri = x =
l2 − ρ22 + ρ21 + (v − di )2 p 2 ρ21 + (v − di )2
(4.134)
Dosazen´ım v´ ysledku (4.134) zpˇet do rovnice (4.132) dost´av´ame pˇredpis pro rovnici ki : y 2 + z 2 = Ri2
ki :
(4.135)
kde Ri2
2
=l −
l2 − ρ22 + ρ21 + (v − di )2 p 2 ρ21 + (v − di )2
!2
Souˇradnice bod˚ u D i , pohybuj´ıc´ıch se po kruˇznic´ıch ki tedy ps´at s pomoc´ı novˇe zaveden´e u ´hlov´e promˇenn´e θi jako (θi parametrizuje trajektorii moˇzn´eho pohybu bod˚ u D i ): V s.s. Fi : D ii =
h
ri Ri cos θi Ri sin θi
V s.s. Fb : D bi = C bi + Rbi · D ii
iT
(4.136) (4.137)
Nyn´ı lze analogicky jako v (4.128) sestavit omezuj´ıc´ı podm´ınky na vz´ajemn´e vzd´alenosti pˇr´ıpojn´ ych bod˚ u D bi jako: kD b1 D b2 k − a22 = 0
(4.138)
kD b2 D b3 k − a22 = 0
(4.139)
kD b1 D b3 k
(4.140)
−
177
a22
=0
´ 4. UVOD DO ROBOTIKY
Rovnice (4.138-4.140) lze upravit na tvar:
h
h
h
(12)
k1
(23)
k1
(13)
k1
(12)
k2
(23)
k2
(13)
k2
(12)
k3
(23)
k3
(13)
k3
(12)
k4
(23)
k4
(13)
k4
(12)
k5
(23)
k5
(13)
k5
(12)
k6
(23)
k6
(13)
k6
(12)
k7
(23)
k7
(13)
k7
(12)
k8
(23)
k8
(13)
k8
cos (θ1 ) cos (θ2 ) cos (θ1 ) sin (θ2 ) sin (θ1 ) cos (θ2 ) sin (θ ) sin (θ ) 1 2 i (12) =0 · sin (θ1 ) k9 sin (θ2 ) cos (θ1 ) cos (θ2 ) 1 (4.141) cos (θ2 ) cos (θ3 ) cos (θ2 ) sin (θ3 ) sin (θ2 ) cos (θ3 ) sin (θ2 ) sin (θ3 ) i (23) =0 · sin (θ ) k9 2 sin (θ3 ) cos (θ2 ) cos (θ3 ) 1 (4.142) cos (θ1 ) cos (θ3 ) cos (θ1 ) sin (θ3 ) sin (θ1 ) cos (θ3 ) sin (θ1 ) sin (θ3 ) i (13) =0 · sin (θ ) k9 1 sin (θ3 ) cos (θ ) 1 cos (θ3 ) 1 (4.143)
(?)
kde konstanty kj
j = 1 . . . 9 z´ avis´ı pouze na n´avrhov´ ych parametrech manipul´atoru ξ
178
4.7 Paraleln´ı manipul´ atory
a na hodnot´ ach aktivn´ıch kloubov´ ych souˇradnic di . Dosazen´ım substituce (4.80)
Ti = tan
θi 2
⇒
sθi = cθi =
2Ti 1+Ti2 1−Ti2 1+Ti2
,
pro i = 1, 2, 3
(4.144)
lze soustavu rovnic (4.141-4.143), po u ´prav´ach, pˇrepsat na soustavu polynomi´aln´ıch rovnic: T12 T22 T1 T22 T 2 T2 1 2 T h i 1 (12) (12) (12) (12) (12) (12) (12) (12) (12) · T22 (4.145) l1 l2 l3 l4 l5 l6 l7 l8 l9 =0 T1 T2 T1 T2 1 T22 T32 T2 T32 T 2 T3 2 2 T h i 2 (23) (23) (23) (23) (23) (23) (23) (23) (23) · T32 (4.146) l1 l2 l3 l4 l5 l6 l7 l8 l9 =0 T2 T3 T2 T 3 1 T12 T32 T1 T32 T 2 T3 1 2 T i h 1 (13) (13) (13) (13) (13) (13) (13) (13) (13) (4.147) · T32 l1 l2 l3 l4 l5 l6 l7 l8 l9 =0 T1 T3 T1 T3 1 (?)
kde konstanty lj
(?)
z´ avis´ı pouze na konstant´ach kj .
179
´ 4. UVOD DO ROBOTIKY
Je zˇrejm´e, ˇze ze soustavy rovnic (4.145-4.147) lze vˇzdy vybrat jednu dvojici rovnic, kter´a bude line´ arn´ı vzhledem k promˇenn´ ym Ti2 a Ti . Napˇr. pro i = 2, m˚ uˇzeme ps´at rovnici (4.145) a (4.146) jako: A · T22 + B · T2 + C = 0
(4.148)
R · T22 + S · T2 + Q = 0
(4.149)
kde (12)
(12)
(12)
T12 + l6
(12)
T12 + l7
(12)
T32 + l3
B = B(T1 ) = l3 C = C(T1 ) = l4 R = R(T3 ) = l1
(12)
S = S(T3 ) = l2
T1 + l8
(12)
T1 + l9
(12)
T3 + l4
(12)
(12)
T1 + l5
(12)
T32 + l6
Q = Q(T3 ) = l5
(12)
T12 + l2
A = A(T1 ) = l1
(12)
(12)
(12)
(12)
T3 + l7
(12)
T32 + l8
(12)
T3 + l9
Dyalitickou eleminac´ı soustavy (4.148-4.149) dost´av´ame:
A B 0 A det R S 0 R
C B Q S
0 C =0 0 Q
(4.150)
Zat´ım nepouˇzit´ a rovnice (4.147) a novˇe vznikl´a rovnice (4.150), kter´e obˇe z´avis´ı pouze na promˇenn´ ych T1 a T3 , lze zapsat ve tvaru: (4.143) : D · T32 + E · T3 + F = 0
(4.151)
(4.150) : G · T34 + M · T33 + N · T32 + U · T3 + V = 0
(4.152)
kde (13)
D = D(T1 ) = l1
(13)
T12 + l2
(13)
T12 + l6
(13)
T12 + l7
E = E(T1 ) = l3 F = F (T1 ) = l4
(13)
T1 + l8
(13)
(13)
T1 + l9
a G, M , N , U , V jsou polynomy ˇr´ adu 4 v promˇenn´e T1 .
180
(13)
T1 + l5
(13)
4.7 Paraleln´ı manipul´ atory
Dyalitickou eliminac´ı soustavy (4.151-4.152) dost´av´ame charakteristick´ y polynom manipul´ atoru: G M N U V 0 0 G M N U V D E F 0 0 0 f(T1 ) = det (4.153) =0 0 D E F 0 0 0 0 D E F 0 0 0 0 D E F Lze uk´ azat, ˇze charakteristick´ y polynom manipul´atoru je ˇr´adu 16, tedy existuje maxim´ alnˇe 16 re´ aln´ ych koˇren˚ u T1 . Zpˇetn´ y v´ ypoˇcet souˇradnic θi je pak d´an n´asleduj´ıc´ım postupem: • Vypoˇcti koˇreny charakteristick´eho polynomu f(T1 ), viz (4.153) • Vypoˇcti konstanty D, E, F , G, M , N , U , V • Vypoˇcti koˇreny (4.151) a vyber jeden z koˇren˚ u, vyhovuj´ıc´ı rovnici (4.152) ⇒ T3 • Vypoˇcti konstanty A, B, C, R, S, Q • Vypoˇcti koˇreny (4.148) a vyber jeden z koˇren˚ u, vyhovuj´ıc´ı rovnici (4.149) ⇒ T2 • Z hodnot T1 , T2 , T3 vypoˇcti zpˇetnou substituc´ı (4.144) u ´hlov´e promˇenn´e θi . • Vypoˇcti polohy pˇr´ıpojn´ ych bod˚ u D ii v s.s. Fi a pˇrepoˇc´ıtej je do s.s. z´akladny manipul´ atoru Fb , viz (4.136, 4.137). ´ pro PSZ pro n´asleduj´ıc´ı hodnoty parameObr´ azek 4.41 ukazuje moˇzn´a ˇreˇsen´ı PKU tr˚ u a aktivn´ıch kloubov´ ych souˇradnic: a1 = 3, a2 = 2, v = 3, l = 3, d1 = 0.98496, d2 = 0.44993, d3 = 1.118 Charakteristick´ y polynom manipul´atoru: f(T1 ) = −0.00003075 T1 16 + 0.005980 T1 15 − 0.2293 T1 14 − 5.981 T1 13 − 29.51 T1 12 + + 20.01 T1 11 + 75.18 T1 10 − 27.72 T1 9 − 64.06 T1 8 + 18.97 T1 7 + 20.65 T1 6 − − 5.991 T1 5 − 2.002 T1 4 + 0.6826 T1 3 − 0.06630 T1 2 + 0.002512 T1 − 0.00003270 Poznamenejme, ˇze hled´ an´ı koˇren˚ u polynom˚ u vyˇsˇs´ıch ˇr´ad˚ u, v naˇsem pˇr´ıpadˇe 16, je problematick´e s ohledem na stabilitu a pˇresnost ˇreˇsen´ı. Bylo experiment´alnˇe ovˇeˇreno, ˇze numerick´e chyby pˇri ˇreˇsen´ı charakteristick´eho polynomu manipul´atoru v programov´em
181
´ 4. UVOD DO ROBOTIKY
bal´ıku Matlab jsou silnˇe z´ avisl´e na n´ avrhov´ ych parametrech manipul´atoru ξ i na aktu´aln´ıch hodnot´ ach kloubov´ ych souˇradnic di . Pˇresnost ˇreˇsen´ı je v mnoha pˇr´ıpadech velice ´ ˇspatn´a (nalezen´e koˇreny nenuluj´ı charakteristick´ y polynom) a u ´loha hled´an´ı ˇreˇsen´ı PKU prostˇrednictv´ım hled´ an´ı koˇren˚ u charakteristick´eho polynomu je pro praktick´e aplikace t´emˇeˇr nepouˇziteln´ a. Alternativn´ı pˇr´ıstup vhodn´ y pro re´aln´e aplikace, kter´ y je modifi´ kac´ı uveden´eho postupu ˇreˇsen´ı PKU lze nal´ezt v [92]. Experiment´alnˇe bylo ovˇeˇreno, ˇze pˇresto, ˇze charakteristick´ y polynom manipul´atoru je 16 stupnˇe, mˇelo by tedy exis´ ve skuteˇcnosti vˇsak dost´av´ame maxim´alnˇe pouze 8 tovat maxim´ alnˇe 16 ˇreˇsen´ı PKU, ˇreˇsen´ı, nebot’ charakteristick´ y polynom manipul´atoru vykazuje maxim´alnˇe 8 re´aln´ ych ˇ sen´ı PKU ´ prostˇrednictv´ım ekvivalence k TSSM r˚ uzn´ ych p´ol˚ u (kaˇzd´ y n´ asobnosti 2). Reˇ tedy nemus´ı vˇzdy v´ezt na minim´ aln´ı charakteristick´ y polynom manipul´atoru.
182
4.7 Paraleln´ı manipul´ atory
ˇ sen´ı PKU ´ pro PSZ, ˇcervenˇe jsou vyznaˇceny moˇzn´e trajektorie pˇr´ıpojn´ Obr´ azek 4.41: Reˇ ych bod˚ u Di
183
´ 4. UVOD DO ROBOTIKY
F ´ pro TSSM nelze vyuˇz´ıt v pˇr´ıpadˇe mechanismu Bohuˇzel, uveden´ y postup ˇreˇsen´ı PKU typu SSM (v obecn´em pˇr´ıpadˇe pˇr´ıpojn´e body na z´akladnˇe a koncov´em efektoru manipul´atoru nemus´ı leˇzet ani v jedn´e rovinˇe). V odborn´e literatuˇre lze nal´ezt metody pro ´ speci´ v´ ypoˇcet PKU aln´ıch pˇr´ıpad˚ u SSM, kde kinematick´e ˇretˇezce typu UPS spojuj´ıc´ı koncov´ y efektor a z´ akladnu manipul´ atoru maj´ı ztotoˇznˇen´e pˇr´ıpojn´e body (manipul´atory typu m − n, kde m oznaˇcuje poˇcet pˇr´ıpojn´ ych bod˚ u na z´akladnˇe a n poˇcet pˇr´ıpojn´ ych bod˚ u na koncov´em efektoru manipul´ atoru, viz Obr´azek 4.42).
(a) 6-5 manipul´ ator
(b) 6-4 manipul´ ator
(c) 5-5 manipul´ ator
Obr´ azek 4.42: Speci´aln´ı pˇr´ıpady SSM
Bylo dok´ az´ ano, ˇze maxim´ aln´ı poˇcet ˇreˇsen´ı obecn´eho 6-6 paraleln´ıho manipul´atoru s 6 ´ v pˇr´ıpadˇe kinematick´ ymi ˇretˇezci typu UPS je 40. Efektivn´ı metody pro v´ ypoˇcet PKU takov´eho obecn´eho paraleln´ıho manipul´atoru dodnes z˚ ust´avaj´ı pˇredmˇetem intenzivn´ıho zkoum´an´ı. Poznamenejme, ˇze nejˇcastˇeji pouˇz´ıvan´e metody jsou zaloˇzeny na principech: • homotopie: Generov´ an´ı syst´emu algebraick´ ych rovnic, jehoˇz ˇreˇsen´ı se s dan´ ymi ´ hodnoty argument˚ hodnotami argument˚ u bl´ıˇz´ı ˇreˇsen´ı IKU, u pak povaˇzujeme za ´ ˇreˇsen´ı PKU. • dyalitick´ e eliminace Viz Kapitola 4.6.2.3, Pˇr´ıklad 4.12.
184
4.7 Paraleln´ı manipul´ atory
• Gr¨ obnerovy b´ aze: Umoˇzn ˇuj´ı algoritmizovat eliminaˇcn´ı proces z pˇredchoz´ıho bodu v koneˇcn´em poˇctu krok˚ u. Kaˇzdou soustavu polynomi´aln´ıch rovnic lze tedy pˇrev´ezt na ekvivalentn´ı soustavu polynomi´aln´ıch rovnic, kde v jedn´e rovnici vystupuje polynom pouze v jedn´e promˇenn´e (charakteristick´ y polynom manipul´atoru). V´ıce o Gr¨ obnerov´ ych b´ az´ıch lze nal´ezt napˇr. v [93], [104]. ´ v podobˇe interval˚ • intervalov´ e anal´ yzy: Hled´an´ı ˇreˇsen´ı PKU u, ve kter´ ych se m˚ uˇze nal´ezat. Intervalov´a anal´ yza se pˇredevˇs´ım dnes uplatˇ nuje k ˇreˇsen´ı u ´loh v robotice za u ´ˇcelem zajiˇstˇen´ı spolehlivosti v´ ypoˇct˚ u (odstranˇen´ı n´achylnosti k numerick´ ym chyb´ am). V´ıce lze nal´ezt napˇr. v [105]. ´ na jednoduˇs´ı u • ad-hoc metody: Pˇreveden´ı ˇci aproximace ˇreˇsen´ı PKU ´lohu se zn´ am´ ym ˇreˇsen´ım. ´ pro paraleln´ı manipul´atory vˇcetnˇe pˇreUcelen´ y pˇrehled o problematice ˇreˇsen´ı PKU hledov´e tabulky shrnuj´ıc´ı dnes ˇreˇsiteln´e architektury manipul´ator˚ u lze nal´ezt v [98]. ˇ sen´ı PKU ´ r˚ Reˇ uzn´ ych architektur manipul´ator˚ u pak napˇr. v [106], [107], [108].
185
´ 4. UVOD DO ROBOTIKY
4.8
Kinematika manipul´ ator˚ u: Z´ avislosti rychlost´ı a zrychlen´ı
Doposud jsme se zab´ yvali pouze z´ avislostmi mezi polohami kloubov´ ych souˇradnic Θ a zobecnˇen´ ych souˇradnic X. Z hlediska u ´pln´eho kinematick´eho popisu manipul´atoru je vˇsak nutn´e nal´ezt tak´e vztahy mezi rychlostmi a zrychlen´ımi kloubov´ ych a zobecnˇen´ ych souˇradnic. Pˇredpokl´ adejme definici pˇr´ım´e a inverzn´ı kinematick´e u ´lohy, viz kapitoly 4.6.1 a 4.6.2 pomoc´ı funkce geometrick´ ych omezen´ı G(?), viz (4.46), (4.47): X = G(Θ)
´ (PKU),
Θ = G−1 (X)
´ (IKU)
(4.154)
´ lze potom ps´at n´asleduj´ıc´ım zp˚ Pˇr´ım´a okamˇzit´ a kinematick´ au ´loha (POKU) usobem1 : ˙ = ∂G(Θ) · Θ ˙ = J A (Θ) · Θ ˙ X ∂Θ ¨ = J˙ A (Θ) · Θ ˙ + J A (Θ) · Θ ¨ X
(4.155) (4.156)
´ u Inverzn´ı okamˇzit´ a kinematick´ a (IOKU) ´loha pak jako: ˙ = ∂G(X) · X ˙ ˙ = J −1 (X) · X Θ A ∂X ¨ ¨ = J˙ −1 (X) · X ˙ + J −1 (X) · X Θ A
kde J A (Θ) =
∂G(Θ) ∂Θ
A
respektive J −1 A (X) =
∂G(X) ∂X
(4.157) (4.158)
naz´ yv´ame analytick´ym respektive
inverzn´ım analytick´ym jakobi´ anem. Poznamenejme, ˇze analytick´ y jakobi´an z´avis´ı na aktu´aln´ım uspoˇr´ ad´ an´ı (poloze) manipul´atoru X (a odpov´ıdaj´ıc´ımu Θ). ˇ Casov´ e derivace kloubov´ ych souˇradnic Θ a zobecnˇen´ ych souˇradnic X lze pro obecn´ y manipul´ator s n klouby vyj´ adˇrit jako, viz (4.45): ϑ˙ 1 ϑ˙ 2 . . . ϑn
T
ϑ¨1 ϑ¨2 . . . ϑ¨n i ˙ = R ˙b O ˙b X e e h i ¨ = R¨b O ¨b X e e
T
˙ = Θ
¨ = Θ
h
(4.159)
(4.160)
Na prvn´ı pohled je zˇrejm´e, ˇze analytick´ y jakobi´an J A lze stanovit pˇr´ımou derivac´ı funkce G(?) podle kloubov´ ych souˇradnic Θ. Nicm´enˇe vzhledem ke sloˇzitosti funkce 1
˙ = Necht’ X
dX , dt
¨ = X
d2 X dt2
186
4.8 Kinematika manipul´ ator˚ u: Z´ avislosti rychlost´ı a zrychlen´ı
(posloupnost v´ ypoˇct˚ u s ˇradou meziv´ ysledk˚ u), je jej´ı pˇr´ım´a derivace velice nepraktick´a a vede na sloˇzit´e a mnohaˇclenn´e vztahy. Ty je sice moˇzn´e vˇetˇsinou nal´ezt pomoc´ı n´astroj˚ u pro symbolick´e v´ ypoˇcty (Maple, Matlab, Mathematica,...), nicm´enˇe pouˇzit´ı takov´ ych vztah˚ u v technick´e praxi, zejm´ena pak v implementaci do algoritm˚ u ˇr´ızen´ı, je v´ıcem´enˇe komplikovan´e. Moˇznou alternativou pro v´ ypoˇcet rychlostn´ıch z´avislost´ı manipul´atoru lze nal´ezt napˇr´ıklad v [80], kter´ a pˇredkl´ad´a systematickou geometrickou metodologii v´ ypoˇctu ˙e a u translaˇcn´ı rychlosti O ´hlov´e rychlosti ω e koncov´eho efektoru v z´avislosti na rych˙ manipul´atoru. Vztah mezi derivac´ı matice rotace R ˙e a losti kloubov´ ych souˇradnic Θ vektorem u ´hlov´e rychlosti ω e a zrychlen´ı ω˙ e je pops´an v Kapitole 4.5.1.2. Uvaˇzujme proto nad´ ale rychlosti a zrychlen´ı koncov´eho efektoru m´ısto (4.160) jako: " ˙ = X " ¨ = X
˙b O e ω be
#
¨b O e ω˙ be
#
(4.161)
´ lze potom pro obecn´ POKU y manipul´ator s n klouby ps´at jako:
˙ = J (Θ) · Θ ˙ X
(4.162)
¨ = J˙ (Θ) · Θ ˙ + J (Θ) · Θ ¨ X
(4.163)
´ jako: A IOKU
˙ = J −1 (X) · X ˙ Θ
(4.164)
˙ (X) · X ¨ = J −1 ˙ + J −1 (X) · X ¨ Θ
(4.165)
kde J (Θ) respektive J −1 (X) naz´ yv´ame kinematick´ym respektive inverzn´ım kinematick´ym jakobi´ anem.
187
´ 4. UVOD DO ROBOTIKY
4.8.1
Z´ avislosti rychlosti pro s´ eriov´ e manipul´ atory
˙0 a Uvaˇzujme prozat´ım, ˇze T b0 = T ne = I (bez kompenzac´ı). Translaˇcn´ı rychlost O i u ´hlovou rychlost ω 0i i-t´eho s.s. Fi vzhledem k s.s. F0 lze ps´at jako: ˙ ϑ1 ϑ˙ 2 " # .. p p p p 0 ˙ j1 j2 . . . jj . . . ji . O i = o o o o j1 j2 . . . jj . . . ji ϑ˙ j ω 0i | {z } . .. J i (Θ) ϑ˙ i
(4.166)
Tedy: ˙ 0 = j p · ϑ˙ 1 + j p · ϑ˙ 2 + · · · + j p · ϑ˙ j + · · · + j p · ϑ˙ i O i 2 1 j i
(4.167)
ω 0i = j o1 · ϑ˙ 1 + j o2 · ϑ˙ 2 + · · · + j oj · ϑ˙ j + · · · + j oi · ϑ˙ i p j kde j = 1 . . . i a sloupcov´e subvektory jo , j pj ∈ R3 respektive j oj ∈ R3 kinematick´eho jj jakobi´anu J i (Θ), zprostˇredkov´ avaj´ı pˇr´ıspˇevek j-t´eho kloubu ϑj do celkov´e translaˇcn´ı respektive u ´hlov´e rychlosti s.s. Fi vzhledem k s.s. F0 . Z Obr. 4.12 a Obr. 4.15 je zˇrejm´e, ˇze pˇr´ıspˇevek translaˇcn´ı rychlosti v 0j,i a u ´hlov´e rychlosti ω 0j,i do s.s. Fi (vyj´ adˇrenou vzhledem k s.s. F0 ) zp˚ usobenou pohybem j-t´eho kloubu, lze pro konkr´etn´ı typy kloub˚ u a zaveden´ı popisu s.s. vyj´adˇrit jako: • Joint j je typu P (ϑj = dj ): Pro D-H u ´mluvu: ω 0j,i = 0 v 0j,i = d˙j · z 0j−1
Pro K-K u ´mluvu: (4.168) ω 0j,i = 0 v 0j,i = d˙j · z 0j
(4.169)
• Joint j je typu R (ϑj = θj ): Pro D-H u ´mluvu: 0 ˙ ω j,i = θj · z 0j−1 v 0j,i
=
ω 0j,i
×
r 0j−1,i
K-K u ´mluvu: (4.170) ω 0j,i = θ˙j · z 0j = θ˙j ·
z 0j−1
×
r 0j−1,i
v 0j,i
=
ω 0j,i
×
(4.171) r 0j,i
= θ˙j ·
z 0j
×
r 0j,i
kde z 0j je osa z s.s. Fj a r 0j,i = O 0i − O 0j je vz´ajemn´a translaˇcn´ı poloha s.s. Fj a Fi . Pˇri znalosti matic pˇrechodu T 0j je zˇrejm´e, ˇze: z 0j = T 0j [1 : 3, 3]
a
r 0j,i = T 0i [1 : 3, 4] − T 0j [1 : 3, 4]
188
(4.172)
4.8 Kinematika manipul´ ator˚ u: Z´ avislosti rychlost´ı a zrychlen´ı
Sloupcov´e subvektory j pj a j oj lze tedy s ohledem na (4.167) ps´at jako: • Joint j je typu P (ϑj = dj ): Pro D-H u ´mluvu: 0 j pj z j−1 o = 0 jj
Pro K-K u ´mluvu: p 0 jj zj (4.173) o = 0 jj
(4.174)
• Joint j je typu R (ϑj = θj ): Pro D-H ´mluvu: pu 0 z j−1 × r 0j−1,i jj = z 0j−1 j oj
Pro K-Ku ´mluvu: 0 z j × r 0j,i j pj (4.175) = z 0j j oj
(4.176)
V´ ysledn´ y kinematick´ y jakobi´ an J i (Θ) je tedy pˇr´ımo d´an rovnicemi (4.173 - 4.176), (4.172) a lze urˇcit pomoc´ı d´ılˇc´ıch matic pˇrechodu T i−1 s.s. manipul´atoru. i ˙iau ´ pro Pro efektivn´ı v´ ypoˇcet translaˇcn´ı rychlosti O ´hlov´e rychlosti ω i (tedy POKU libovoln´ y s.s. v kinematick´em ˇretˇezci manipul´atoru) lze nast´ınˇen´ y postup modifikovat na n´ asleduj´ıc´ı zobecnˇen´ y rekurzivn´ı algoritmus pro libovoln´ y typ kloub˚ u. Definujme pomocnou promˇennou σi , pro kterou plat´ı: σj = 0
pokud Joint j
je typu R (ϑj = θj )
σj = 1
pokud Joint j
je typu P (ϑj = dj )
σ ¯j = 1 − σj Algoritmus 4.1 (Rekurzivn´ı alg. v´ ypoˇ ctu rychlost´ı pro D-H u ´ mluvu) Z rovnic (4.167), (4.173) a (4.175) vypl´ yv´a n´asleduj´ıc´ı rekurzivn´ı sch´ema: Pro u ´ hlov´ e rychlosti plat´ı: ω 01 = z 00 σ ¯1 ϑ˙ 1 ¯2 ϑ˙ 2 = ω 01 + z 01 σ ¯2 ϑ˙ 2 ω 02 = z 00 σ ¯ ϑ˙ +z 01 σ | {z1 }1 ω 01
ω 03 = z 00 σ ¯ ϑ˙ + z 0 σ ¯ ϑ˙ +z 02 σ ¯3 ϑ˙ 3 = ω 02 + z 02 σ ¯3 ϑ˙ 3 | 1 1 {z 1 2 }2 ω 02
189
´ 4. UVOD DO ROBOTIKY
Pro translaˇ cn´ı rychlosti plat´ı: 0 0 ˙ ˙ 0 = ((z 0 × r 0 )¯ O ¯ ϑ˙ ×r 00,1 ) + z 00 σ1 ϑ˙ 1 = ω 01 × r 00,1 + z 00 σ1 ϑ˙ 1 1 0 0,1 σ1 + z 0 σ1 )ϑ1 = (z 0 σ | {z1 }1 ω 01
0 0 0 ˙ ˙ 0 = ((z 0 × r 0 )¯ O σ2 + z 01 σ2 )ϑ˙ 2 = 2 0 0,2 σ1 + z 0 σ1 )ϑ1 + ((z 1 × r 1,2 )¯
= ((z 00 × (r 00,1 + r 01,2 ))¯ σ1 + z 00 σ1 )ϑ˙ 1 + (z 01 × r 01,2 )¯ σ2 ϑ˙ 2 + z 01 σ2 ϑ˙ 2 = σ1 ϑ˙ 1 + (z 01 × r 01,2 )¯ σ2 ϑ˙ 2 + z 01 σ2 ϑ˙ 2 = = ((z 00 × r 00,1 )¯ σ1 + z 00 σ1 )ϑ˙ 1 +(z 00 × r 01,2 )¯ | {z } ˙0 O 1
˙ 0 + (z 0 σ =O ¯ ϑ˙ + z 0 σ ¯ ϑ˙ ) ×r 01,2 + z 01 σ2 ϑ˙ 2 = 1 | 0 1 1 {z 1 2 2} ω 02
˙ 0 + ω 0 × r 0 + z 0 σ2 ϑ˙ 2 =O 1 2 1,2 1 0 0 0 ˙ ˙ 0 = ((z 0 × r 0 )¯ O σ2 + z 01 σ2 )ϑ˙ 2 + ((z 02 × r 02,3 )¯ σ3 + z 02 σ3 )ϑ˙ 3 = 3 0 0,3 σ1 + z 0 σ1 )ϑ1 + ((z 1 × r 1,3 )¯
= ((z 00 × (r 00,2 + r 02,3 ))¯ σ1 + z 00 σ1 )ϑ˙ 1 + ((z 01 × (r 01,2 + r 02,3 ))¯ σ2 + z 01 σ2 )ϑ˙ 2 + + (z 02 × r 02,3 )¯ σ3 ϑ˙ 3 + z 02 σ3 ϑ˙ 3 = σ1 ϑ˙ 1 + (z 01 × r 02,3 )¯ σ2 ϑ˙ 2 + = ((z 00 × r 00,2 )¯ σ1 + z 00 σ1 )ϑ˙ 1 + ((z 01 × r 01,2 )¯ σ2 + z 01 σ2 )ϑ˙ 2 +(z 00 × r 02,3 )¯ {z } | ˙0 O 2
+ (z 02 × r 02,3 )¯ σ3 ϑ˙ 3 + z 02 σ3 ϑ˙ 3 = ˙ 0 + (z 0 σ =O ¯ ϑ˙ + z 01 σ ¯ ϑ˙ + z 02 σ ¯3 ϑ˙ 3 ) ×r 02,3 + z 02 σ3 ϑ˙ 3 = 2 {z2 2 } | 0 1 1 ω 03
0
˙ + ω 0 × r 0 + z 0 σ3 ϑ˙ 3 =O 2 3 2,3 2 Tedy rekurzivn´ı algoritmus vzhledem k s.s. F0 lze ps´at: ω 0j = ω 0j−1 + z 0j−1 σ ¯j ϑ˙ j
(4.177)
˙0 O j
(4.178)
=
˙0 O j−1
+ ω 0j × r 0j−1,j + z 0j−1 σj ϑ˙ j
Lze uk´azat, ˇze rekurzivn´ı v´ ypoˇcet lze v´ yraznˇe urychlit vyj´adˇren´ım translaˇcn´ıch a u ´hlov´ ych rychlost´ı koncov´eho efektoru v algoritmu vˇzdy vzhledem k aktu´aln´ımu s.s. Fj . j−1 ω jj = ω jj−1 + z jj−1 σ ¯j ϑ˙ j = Rjj−1 (ω j−1 + z j−1 ¯j ϑ˙ j ) j−1 σ j j j−1 j j ˙ ˙ ˙j =O ˙ j + ωj × rj ˙ j−1 O j j−1 j j−1,j + z j−1 σj ϑj = Rj−1 (O j−1 + z j−1 σj ϑj ) + ω j × r j−1,j
Z´aroveˇ n zˇrejmˇe plat´ı z j−1 j−1 =
h
0 0 1
iT
.
190
4.8 Kinematika manipul´ ator˚ u: Z´ avislosti rychlost´ı a zrychlen´ı
V´ ysledn´ y rekurzivn´ı algoritmus pro postupn´ y v´ ypoˇcet translaˇcn´ı a u ´hlov´e rychlosti koncov´eho efektoru pro D-H u ´mluvu pro popis s.s. lze tak ps´at jako:
ω jj = Rjj−1 (ω j−1 j−1 +
h
˙ j = R j (O ˙ j−1 + O j j−1 j−1
h
0 0 1 0 0 1
iT
σ ¯j ϑ˙ j )
(4.179)
iT
σj ϑ˙ j ) + ω jj × r jj−1,j
(4.180)
˙ ? = 0 (poloha s.s. F0 je pevn´a) kde j = 1 . . . i, ω0? = 0, O 0
T r jj−1,j = O jj − O jj−1 = −O jj−1 = −(T j−1 j ) [1 : 3, 4] T Rjj−1 = (T j−1 j ) [1 : 3, 1 : 3]
Algoritmus 4.2 (Rekurzivn´ı alg. v´ ypoˇ ctu rychlost´ı pro K-K u ´ mluvu) Z rovnic (4.167), (4.174) a (4.176) vypl´ yv´a n´asleduj´ıc´ı rekurzivn´ı sch´ema: Pro u ´ hlov´ e rychlosti plat´ı:
ω 01 = z 01 σ ¯1 ϑ˙ 1 ω 02 = z 01 σ ¯ ϑ˙ +z 02 σ ¯2 ϑ˙ 2 = ω 01 + z 02 σ ¯2 ϑ˙ 2 | {z1 }1 ω 01
ω 03 = z 01 σ ¯ ϑ˙ + z 0 σ ¯ ϑ˙ +z 03 σ ¯3 ϑ˙ 3 = ω 02 + z 03 σ ¯3 ϑ˙ 3 | 1 1 {z 2 2 }2 ω 02
191
´ 4. UVOD DO ROBOTIKY
Pro translaˇ cn´ı rychlosti plat´ı: 0 0 ˙ ˙ ˙ 0 = ((z 0 × r 0 )¯ O 1 1 1,1 σ1 + z 1 σ1 )ϑ1 = z 1 σ1 ϑ1 |{z} 0
˙0 O 2
=
(z 01 σ ¯1 ϑ˙ 1 ×r 01,2 )
×
r 01,2 )¯ σ1
+ z 01 σ1 )ϑ˙ 1 + ((z 02 × r 02,2 )¯ σ2 + z 02 σ2 )ϑ˙ 2 = |{z}
=
((z 01
0
+
| {z }
z 01 σ1 ϑ˙ 1 +z 02 σ2 ϑ˙ 2
=
| {z }
ω 01
˙0 O 1
0
˙ + ω 0 × r 0 + z 0 σ2 ϑ˙ 2 =O 1 1 1,2 2 0 0 0 ˙ ˙ 0 = ((z 0 × r 0 )¯ O σ2 + z 02 σ2 )ϑ˙ 2 + ((z 03 × r 03,3 )¯ σ3 + z 03 σ3 )ϑ˙ 3 = 3 1 1,3 σ1 + z 1 σ1 )ϑ1 + ((z 2 × r 2,3 )¯ |{z} 0
=
((z 01
×
(r 01,2
+
r 02,3 ))¯ σ1
z 01 σ1 )ϑ˙ 1
+
+
((z 02
×
r 02,3 )¯ σ2
+
z 02 σ2 )ϑ˙ 2
+ z 03 σ3 ϑ˙ 3 =
= ((z 01 × r 01,2 )¯ σ1 + z 01 σ1 )ϑ˙ 1 + z 02 σ2 ϑ˙ 2 +(z 01 × r 02,3 )¯ σ1 ϑ˙ 1 + (z 02 × r 02,3 )¯ σ2 ϑ˙ 2 + z 03 σ3 ϑ˙ 3 = | {z } ˙0 O 2
˙ 0 + (z 0 σ =O ¯ ϑ˙ + z 0 σ ¯ ϑ˙ ) ×r 02,3 + z 03 σ3 ϑ˙ 3 = 2 | 1 1 1 {z 2 2 2} ω 02
0
˙ + ω 0 × r 0 + z 0 σ3 ϑ˙ 3 =O 2 2 2,3 3 Tedy rekurzivn´ı algoritmus vzhledem k s.s. F0 lze ps´at: ω 0j = ω 0j−1 + z 0j σ ¯j ϑ˙ j
(4.181)
˙0 O j
(4.182)
=
˙0 O j−1
+ ω 0j−1 × r 0j−1,j + z 0j σj ϑ˙ j
Ze stejn´eho d˚ uvodu jako v Algoritmu 4.1 provedeme vyj´adˇren´ı rekurzivn´ıho algoritmu v s.s. Fj : j ω jj = ω jj−1 + z jj σ ¯j ϑ˙ j = Rjj−1 ω j−1 ¯j ϑ˙ j j−1 + z j σ j j j−1 j−1 j ˙ ˙ ˙ j−1 ˙j =O ˙ j + ωj × rj O j j−1 j−1 j−1,j + z j σj ϑj = Rj−1 (O j−1 + ω j−1 × r j−1,j ) + z j σj ϑj
Opˇet plat´ı z jj =
h
0 0 1
iT
.
V´ ysledn´ y rekurzivn´ı algoritmus pro postupn´ y v´ ypoˇcet translaˇcn´ı a u ´hlov´e rychlosti koncov´eho efektoru pro K-K u ´mluvu pro popis s.s. lze tak ps´at jako: h iT j−1 ω jj = Rjj−1 ω j−1 + 0 0 1 σ ¯j ϑ˙ j (4.183) h iT ˙ j = R j (O ˙ j−1 + ω j−1 × r j−1 ) + 0 0 1 σj ϑ˙ j (4.184) O j j−1 j−1 j−1 j−1,j
192
4.8 Kinematika manipul´ ator˚ u: Z´ avislosti rychlost´ı a zrychlen´ı
˙ ? = 0 (poloha s.s. F0 je pevn´a) kde j = 1 . . . i, ω0? = 0, O 0 j−1 j−1 r j−1,j = O j−1 − O j−1 = T j−1 j j−1 = O j j [1 : 3, 4] T Rjj−1 = (T j−1 j ) [1 : 3, 1 : 3]
Kompenzace rychlost´ı ˙ z (4.161) s uvaˇzov´an´ım Zab´ yvejme se nyn´ı v´ yslednou rychlost´ı koncov´eho efektoru X kompenzaˇcn´ıch matic T b0 a T ne . Vzhledem k faktu, ˇze s.s. Fn je n´asledov´an s.s. Fe , lze ps´ at rekurzivn´ı algoritmus pro D-H (4.179, 4.180) i K-K (4.183, 4.184) u ´mluvu pro j = {n, e} jako: Pro D-H u ´mluvu:
ω ee = Ren ω nn +
0 0 1 {z |
T
·σ ¯e ϑ˙ e }
(4.185)
=0 (ϑe neexistuje)
˙e O e
=
˙n Ren (O n
+
0 0 1 | {z
T
=0
· σe ϑ˙ e ) + ω ee × r en,e }
Pro K-K u ´mluvu:
ω ee = Ren ω nn +
0 0 1 {z |
T
=0
˙e O e
=
˙n Ren (O n
+
ω nn
σ ¯e ϑ˙ e }
× r nn,e )] +
(4.186)
0 0 1 | {z 0
T
σe ϑ˙ e }
Jednoduch´ ymi u ´pravami lze uk´azat, ˇze rovnice (4.185) a (4.186) vedou na stejn´ y vztah:
ω ne = ω nn
(4.187)
˙n=O ˙ n + ωn × rn O e n n n,e
193
´ 4. UVOD DO ROBOTIKY
˙ viz (4.161), lze vypoˇc´ıtat jako: Rychlost zobecnˇen´ ych souˇradnic manipul´atoru X, " # n b n b n ) ˙ ˙ ˙b I −S(r R 0 R 0 O O O 3×3 3×3 3×3 n,e n n e n = ˙ e X= = · = · · b b n b 03×3 I 3×3 03×3 Rn 0 Rn ωe ω nn ωe 0 T b " 0# ˙ I 3×3 −S(r nn,e ) (Rn ) 03×3 Rn 03×3 O n = · · = 0 T · b 0 (Rn ) 03×3 I 3×3 0 Rn ω 0n b " 0# ˙ R0 −Rb0 · R0n · S(r nn,e ) · (R0n )T O n = · b 03×3 R0 ω 0n {z } | J comp (Θ)
(4.188) kde ´ viz (4.42)) Rb0 = T b0 [1 : 3, 1 : 3], R0n = T 0n [1 : 3, 1 : 3] (z ˇreˇsen´ı PKU, 0 −r nn,e [3, 1] r nn,e [2, 1] n n 0 −r nn,e [1, 1] , r nn,e = T ne [1 : 3, 4] S(r n,e ) = r n,e [3, 1] −r nn,e [2, 1] r nn,e [1, 1] 0 Kombinac´ı rovnice (4.166) pro i = n a rovnice (4.188) dost´av´ame v´ ysledn´ y vztah pro v´ ypoˇcet rychlost´ı koncov´eho efektoru manipul´atoru: ˙ = J comp (Θ) · J n (Θ) · Θ ˙ X
(4.189)
kde J comp (Θ) je kompenzaˇcn´ı matice a J n (Θ) je kinematick´ y jakobi´an pro n kloubov´ ych souˇradnic bez uvaˇzovan´ ych kompenzac´ı (T b0 = T ne = I). Porovn´ame-li v´ ysledek (4.189) s rovnic´ı (4.162) je zˇrejm´e, ˇze kinematick´ y jakobi´an (vˇcetnˇe kompenzac´ı polohy z´ akladny a koncov´eho efektoru) je d´an jako: J (Θ) = J comp (Θ) · J n (Θ).
4.8.2
(4.190)
Z´ avislosti zrychlen´ı pro s´ eriov´ e manipul´ atory 0
¨ au ¨ 0i ´hlov´e zrychlen´ı ω Nejprve uvaˇzujme opˇet, ˇze T b0 = T ne = I. Translaˇcn´ı zrychlen´ı O i s.s. Fi vzhledem k s.s. F0 lze stanovit pˇr´ımou ˇcasovou derivac´ı z rovnice (4.166) jako: ¨ ˙ ϑ1 ϑ1 ϑ¨2 ϑ˙ 2 " # .. .. 0 ¨ O i =J ˙ i (Θ) · . + J i (Θ) · . (4.191) 0 ϑ¨j ˙ ω˙ i ϑj . . .. .. ˙ ϑ¨i ϑi
194
4.8 Kinematika manipul´ ator˚ u: Z´ avislosti rychlost´ı a zrychlen´ı
Tedy: ¨ 0 = j p · ϑ¨1 + j p · ϑ¨2 + · · · + j p · ϑ¨j + · · · + j p · ϑ¨i + j˙ p · ϑ˙ 1 + j˙ p · ϑ˙ 2 + · · · + j˙ p · ϑ˙ j + · · · + j˙ p · ϑ˙ i O i 1 2 j i 1 2 j i (4.192) o o o o ω˙ 0 = j o · ϑ¨1 + j o · ϑ¨2 + · · · + j o · ϑ¨j + · · · + j o · ϑ¨i + j˙ · ϑ˙ 1 + j˙ · ϑ˙ 2 + · · · + j˙ · ϑ˙ j + · · · + j˙ · ϑ˙ i i
1
2
j
i
1
2
j
i
ˇ Casov´ a derivace kinematick´eho jakobi´anu J˙ i (Θ) je d´ana ˇcasov´ ymi derivacemi jeho d´ılˇc´ıch prvk˚ u, tedy jeho sloupc˚ u, viz rovnice (4.173 - 4.176):
• Joint j je typu P (ϑj = dj ): Pro K-K u ´mluvu: " p# j˙ j z˙ 0j (4.193) o = 0 j˙ j
Pro D-H u ´"mluvu: # p j˙ j z˙ 0j−1 o = 0 j˙ j
(4.194)
• Joint j je typu R (ϑj = θj ): Pro ´mluvu: " pD-H # u ´mluvu: 0 Pro" K-K # u ˙j 0 p z˙ j−1 × r 0j−1,i + z 0j−1 × r˙ 0j−1,i j ˙ z˙ j × r 0j,i + z 0j × r˙ 0j,i jj o = 0 (4.196) o = z˙ j−1 j˙ j z˙ 0j j˙ j (4.195) ˙ 0 − O˙ 0 a z 0 = T 0 [1 : 3, 4]. kde r˙ 0j,i = O i j i i 0 ˙ au Translaˇcn´ı rychlosti O ´hlovou rychlost ω 0i lze vyi poˇc´ıtat jako v rovnici (4.166) ˇci pomoc´ı rekurzivn´ıho Algoritmu 4.1 respektive Algoritmu 4.2. ˙ 0 a ω 0 staˇ Casovou derivaci osy z˙ 0i lze se znalost´ı O i i novit dle Obr´ azku 4.43 jako: 0
0
˙ + ω0 × z0 − O ˙ = ω 0 × z 0 (4.197) z˙ 0i = v 2 − v 1 = O i i i i i i Obr´ azek 4.43: Odvozen´ı
Stejnˇe jako v Kapitole 4.8.1 lze opˇet nal´ezt vhodn´ y rychlosti z˙ i rekurzivn´ı algoritmus k v´ ypoˇctu translaˇcn´ıho zrychlen´ı ¨ Oi a u ´hlov´eho zrychlen´ı ω˙ i . Rekurzivn´ı sch´ema lze odvodit pˇr´ımou ˇcasovou derivac´ı rekurzivn´ıho algoritmu pro v´ ypoˇcet translaˇcn´ı a u ´hlov´e rychlosti koncov´eho efektoru z Algoritmu 4.1 respektive z Algoritmu 4.2.
195
´ 4. UVOD DO ROBOTIKY
Algoritmus 4.3 (Rekurzivn´ı alg. v´ ypoˇ ctu zrychlen´ı pro D-H u ´ mluvu) Pro u ´ hlov´ e zrychlen´ı plat´ı: ˇ Casovou derivac´ı rovnice (4.177) a substituc´ı (4.197) dost´av´ame: ω ˙ 0j = ω ˙ 0j−1 + z˙ 0j−1 σ ¯j ϑ˙ j + z 0j−1 σ ¯j ϑ¨j = ω ˙ 0j−1 + (ω 0j−1 × z 0j−1 )¯ σj ϑ˙ j + z 0j−1 σ ¯j ϑ¨j = =ω ˙ 0j−1 + ω 0j−1 σ ¯j ϑ˙ j × z 0j−1 + z 0j−1 σ ¯j ϑ¨j Pro translaˇ cn´ı zrychlen´ı plat´ı: ˇ Casovou derivac´ı rovnice (4.178), substituc´ı (4.197) a substituc´ı: 0
0
0 0 0 ˙ ˙ −O ˙ r˙ 0j−1,j = O j j−1 = ω j × r j−1,j + z j−1 σj ϑj
(viz rovnice (4.178))
dost´av´ame: 0 ¨0 = O ¨ 0 + ω˙ 0 × r 0 ˙ 0j−1,j + z˙ 0j−1 σj ϑ˙ j + z 0j−1 σj ϑ¨j = O j j−1 j j−1,j + ω j × r 0 0 0 0 0 0 0 ˙ ˙ ¨ ¨ 0 + ω˙ 0 × r 0 =O j−1 j j−1,j + ω j × (ω j × r j−1,j + z j−1 σj ϑj ) + (ω j−1 × z j−1 )σj ϑj + z j−1 σj ϑj = 0
¨ ˙ 0j × r 0j−1,j + ω 0j × (ω 0j × r 0j−1,j ) + ω 0j × z 0j−1 σj ϑ˙ j + (ω 0j−1 × z 0j−1 )σj ϑ˙ j + z 0j−1 σj ϑ¨j =O j−1 + ω Tedy rekurzivn´ı algoritmus vzhledem k s.s. Fj lze ps´at: j−1 j−1 ω ˙ jj = ω ˙ jj−1 + ω jj−1 σ ¯j ϑ˙ j × z jj−1 + z jj−1 σ ¯j ϑ¨j = Rjj−1 (ω ˙ j−1 ¯j ϑ˙ j × z j−1 ¯j ϑ¨j ) j−1 + ω j−1 σ j−1 + z j−1 σ j j j j j j j j ˙ ˙ ¨ ¨j = O ¨ j + ω˙ j × r j O j j−1 j j−1,j + ω j × (ω j × r j−1,j ) + ω j × z j−1 σj ϑj + (ω j−1 × z j−1 )σj ϑj + z j−1 σj ϑj = j−1
j−1 j−1 j−1 ˙ ¨ ¨ ˙ jj × r jj−1,j + ω jj × (ω jj × r jj−1,j )+ = Rjj−1 (O j−1 + ω j−1 × z j−1 σj ϑj + z j−1 σj ϑj ) + ω j−1 +ω jj × Rjj−1 z j−1 σj ϑ˙ j 0 j−1 Opˇet plat´ı z j−1 = 0 .
1 V´ ysledn´ y rekurzivn´ı algoritmus pro postupn´ y v´ ypoˇcet translaˇcn´ıho a u ´hlov´eho zrychlen´ı koncov´eho efektoru pro D-H u ´mluvu pro popis s.s. lze tak ps´at jako: 0 0 ¨ j j j−1 j−1 ˙ ω ˙ j = Rj−1 (ω ˙ j−1 + σ ¯j (ω j−1 ϑj × 0 + 0 ϑj )) (4.198)
1
1
0 0 j j j j j ¨ j = Rj (O ¨ j−1 + σj (ω j−1 × O 0 ϑ˙ j + 0 ϑ¨j )) + ω˙ j × r j−1,j + ω j × (ω j × r j−1,j )+ j j−1 j−1 j−1 1 1 0 ˙ j j + ω j × Rj−1 0 σj ϑj (4.199) 1
196
4.8 Kinematika manipul´ ator˚ u: Z´ avislosti rychlost´ı a zrychlen´ı
¨ ? = 0 (poloha s.s. F0 je pevn´a) kde j = 1 . . . i, ω˙ ?0 = 0, O 0 T r jj−1,j = O jj − O jj−1 = −O jj−1 = −(T j−1 j ) [1 : 3, 4] T Rjj−1 = (T j−1 j ) [1 : 3, 1 : 3] j a ω j−1 ame z Algoritmu 4.1. j−1 , ω j zn´
Algoritmus 4.4 (Rekurzivn´ı alg. v´ ypoˇ ctu zrychlen´ı pro K-K u ´ mluvu) Pro u ´ hlov´ e zrychlen´ı plat´ı: ˇ Casovou derivac´ı rovnice (4.181) a substituc´ı (4.197) dost´av´ame: ω˙ 0j = ω˙ 0j−1 + z˙ 0j σ ¯j ϑ˙ j + z 0j σ ¯j ϑ¨j = ω˙ 0j−1 + (ω 0j × z 0j )¯ σj ϑ˙ j + z 0j σ ¯j ϑ¨j = = ω˙ 0j−1 + ω 0j σ ¯j ϑ˙ j × z 0j + z 0j σ ¯j ϑ¨j Pro translaˇ cn´ı zrychlen´ı plat´ı: ˇ Casovou derivac´ı rovnice (4.182), substituc´ı (4.197) a substituc´ı: 0
0
0 0 0 ˙ ˙ −O ˙ r˙ 0j−1,j = O j j−1 = ω j−1 × r j−1,j + z j σj ϑj
(viz rovnice (4.182))
dost´ av´ ame: 0 ¨0 = O ¨ 0 + ω˙ 0 × r 0 ˙ 0j−1,j + z˙ 0j σj ϑ˙ j + z 0j σj ϑ¨j = O j j−1 j−1 j−1,j + ω j−1 × r 0 0 0 0 0 0 0 ˙ ˙ ¨ ¨ 0 + ω˙ 0 × r 0 =O j−1 j−1 j−1,j + ω j−1 × (ω j−1 × r j−1,j + z j σj ϑj ) + (ω j × z j )σj ϑj + z j σj ϑj = 0 0 0 0 0 0 0 0 ˙ ˙ ¨ ¨ 0 + ω˙ 0 × r 0 =O j−1 j−1 j−1,j + ω j−1 × (ω j−1 × r j−1,j ) + ω j−1 × z j σj ϑj + ω j × z j σj ϑj + z j σj ϑj
Tedy rekurzivn´ı algoritmus vzhledem k s.s. Fj lze ps´at: j ω˙ jj = ω˙ jj−1 + ω jj σ ¯j ϑ˙ j × z jj + z jj σ ¯j ϑ¨j = Rjj−1 ω˙ j−1 ¯j ϑ˙ j × z jj + z jj σ ¯j ϑ¨j j−1 + ω j σ j j j j j j j j ˙ ˙ ¨ ¨j = O ¨ j + ω˙ j × r j O j j−1 j−1 j−1,j + ω j−1 × (ω j−1 × r j−1,j ) + ω j−1 × z j σj ϑj + ω j × z j σj ϑj + z j σj ϑj = j−1
j−1 j−1 j−1 j−1 j j−1 j ˙ ¨ ˙ j−1 = Rjj−1 (O × r j−1 j−1 + ω j−1,j + ω j−1 × (ω j−1 × r j−1,j )) + Rj−1 ω j−1 × z j σj ϑj +
+ω jj × z jj σj ϑ˙ j + z jj σj ϑ¨j
0 Opˇet plat´ı z jj = 0 . 1 V´ ysledn´ y rekurzivn´ı algoritmus pro postupn´ y v´ ypoˇcet translaˇcn´ıho a u ´hlov´eho zrychlen´ı
197
´ 4. UVOD DO ROBOTIKY
koncov´eho efektoru pro K-K u ´mluvu pro popis s.s. lze tak ps´at jako: 0 0 ¨ j−1 j ˙ j j ¯j (ω j ϑj × 0 + 0 ϑj ) ω˙ j = Rj−1 ω˙ j−1 + σ 1 1 ¨ j = Rj (O ¨ j−1 + ω˙ j−1 × r j−1 + ω j−1 × (ω j−1 × r j−1 ))+ O j j−1 j−1 j−1 j−1,j j−1 j−1 j−1,j 0 0 ˙ ¨ j j−1 j + σj ((Rj−1 ω j−1 + ω j ) × 0 ϑj + 0 ϑj ) 1
(4.200)
(4.201)
1
¨ ? = 0 (poloha s.s. F0 je pevn´a) kde j = 1 . . . i, ω˙ ?0 = 0, O 0 j−1 j−1 r j−1,j = O jj−1 − O j−1 = T j−1 j−1 = O j j [1 : 3, 4] T Rjj−1 = (T j−1 j ) [1 : 3, 1 : 3] j a ω j−1 ame z Algoritmu 4.2. j−1 , ω j zn´
Poznamenejme, ˇze rekurzivn´ı algoritmy Algoritmus 4.1-4.4 je moˇzn´e jednoduch´ ymi u ´pravami pˇrev´ezt na tvary uveden´e v literatuˇre [80] a [81]. Kompenzace zrychlen´ı ˙ z (4.161) s uvaˇzov´an´ım Zab´ yvejme se opˇet v´ ysledn´ ym zrychlen´ım koncov´eho efektoru X kompenzaˇcn´ıch matic T b0 a T ne . Vzhledem k faktu, ˇze s.s. Fn je n´asledov´an s.s. Fe , lze ps´at rekurzivn´ı algoritmus pro D-H (4.199, 4.198) i K-K (4.201, 4.200) u ´mluvu pro j = {n, e} jako: Pro D-H u ´mluvu:
0 0 e e n n˙ ω ˙ e = R n (ω ˙n+σ ¯e (ω n ϑe × 0 + 0 ϑ¨e ) 1 1 {z } |
(4.202)
=0 (θe neexistuje)
0 0 e e ¨n n ˙ ¨ O e = Rn (O n + σe (ω n × 0 ϑe + 0 ϑ¨e )) + ω˙ ee × r en,e + ω ee × (ω ee × r en,e )+ 1 1 | {z } =0 0 e e + ω e × Rn 0 σe ϑ˙ e 1 | {z } =0
198
4.8 Kinematika manipul´ ator˚ u: Z´ avislosti rychlost´ı a zrychlen´ı
Pro K-K u ´mluvu:
0 0 e e n e˙ ω˙ e = Rn ω˙ n + σ ¯e (ω e ϑe × 0 + 0 ϑ¨e ) 1 1 {z } |
(4.203)
=0
¨ e = R e (O ¨ n + ω˙ n × r n + ω n × (ω n × r n ))+ O e n n n n,e n n n,e 0 0 e n e ˙ + σe ((Rn ω n + ω e ) × 0 ϑe + 0 ϑ¨e ) 1 1 {z } | =0
Jednoduch´ ymi u ´pravami lze uk´azat, ˇze rovnice (4.202) a (4.203) vedou na stejn´ y vztah: ω˙ ne = ω˙ nn ¨n O e
=
¨n O n
(4.204) + ω˙ nn × r nn,e + ω nn × (ω nn × r nn,e ) {z } | n )·S(ω n )·r n S(ωn n n,e
¨ viz 4.161, lze vypoˇc´ıtat jako: Zrychlen´ı zobecnˇen´ ych souˇradnic manipul´atoru X, " # n b n 2 n b n ) n ¨ ¨ ¨b I −S(r S (ω ) · r R 0 R 0 O O O 3×3 3×3 3×3 n,e n n,e n n e n ¨ = e X = · = · · + 03×3 I 3×3 0 03×3 Rbn 0 Rbn ω˙ ne ω˙ nn ω˙ be " # b 0 T b 2 n n ¨0 I 3×3 −S(r nn,e ) Rn 03×3 (Rn ) 03×3 O n + Rn · S (ω n ) · r n,e = = · · · 0 (R0n )T 03×3 I 3×3 0 0 Rbn ω˙ 0n " # b b ¨0 R0 −Rb0 · R0n · S(r nn,e ) · (R0n )T R0 · R0n · S 2 (ω nn ) · r nn,e O n · + = 0 03×3 Rb0 ω˙ 0n | {z } {z } | ˙ J add (Θ,Θ)
J comp (Θ)
(4.205) kde ´ viz (4.42)) Rb0 = T b0 [1 : 3, 1 : 3], R0n = T 0n [1 : 3, 1 : 3] (z ˇreˇsen´ı PKU, 0 −r nn,e [3, 1] r nn,e [2, 1] n 0 −r nn,e [1, 1] , r nn,e = T ne [1 : 3, 4] S(r n,e ) = r nn,e [3, 1] −r nn,e [2, 1] r nn,e [1, 1] 0 a ω nn zn´ ame z Kapitoly 4.8.1. Kombinac´ı rovnice (4.191) pro i = n a rovnice (4.205) dost´av´ame v´ ysledn´ y vztah pro v´ ypoˇcet zrychlen´ı koncov´eho efektoru manipul´atoru: ¨ = J comp (Θ) · (J˙ n (Θ) · Θ ˙ + J n (Θ) · Θ) ¨ + J add (Θ, Θ) ˙ X
199
(4.206)
´ 4. UVOD DO ROBOTIKY
kde kompenzaˇcn´ı matice J comp (Θ) je shodn´a jako kompenzaˇcn´ı matice z rovnice (4.189), ˙ je aditivn´ı sloˇzka zrychlen´ı zp˚ matice J add (Θ, Θ) usoben´a rotac´ı koncov´eho efektoru a J n (Θ) respektive J˙ n (Θ) je kinematick´ y jakobi´an, respektive jeho ˇcasov´a derivace pro n kloubov´ ych souˇradnic bez uvaˇzovan´ ych kompenzac´ı (T b0 = T ne = I). Porovn´ame-li v´ ysledek (4.206) s rovnic´ı (4.163), dost´av´ame: ¨ = J comp (Θ) · J˙ n (Θ) · Θ ˙ + J add (Θ, Θ) ˙ + J comp (Θ) · J n (Θ) ·Θ ¨ X | {z } | {z } ˙ ˙ J(Θ)· Θ
4.8.3
(4.207)
J(Θ)
Z´ avislosti rychlosti a zrychlen´ı pro paraleln´ı manipul´ atory
Na probl´em nalezen´ı z´ avislosti rychlost´ı a zrychlen´ı pro paraleln´ı manipul´atory lze opˇet nahl´ıˇzet dvˇema zp˚ usoby. V pˇr´ıpadˇe dekompozice paraleln´ıho mechanismu na nez´avisl´e ´ kaˇzd´eho rozpouzavˇren´e kinematick´e ˇretˇezce, viz Kapitola 4.7.1 je moˇzn´e zapsat POKU jen´eho s´eriov´eho kinematick´eho ˇretˇezce, viz Obr´azek 4.27, vyjadˇruj´ıc´ı stejnou rychlost v rozpojen´em kloubu Joint k (podm´ınka na uzavˇren´ı kinematick´e smyˇcky): ˙ 1 = J k+B (Θk+B ) · Θ ˙2 J k (Θk ) · Θ
(4.208)
kde Θ1 respektive Θ2 vyjadˇruje rychlosti aktivn´ıch i pasivn´ıch kloubov´ ych souˇradnic v kaˇzd´em rozpojen´em kinematick´em ˇretˇezci a J k (Θ1 ) respektive J k+b (Θ2 ) jejich kinematick´e jakobi´ any. Jelikoˇz ze zn´ am´e polohy aktivn´ıch kloubov´ ych souˇradnic m˚ uˇzeme dopoˇc´ıtat hodnoty souˇradnic pasivn´ıch, rovnice (4.208) vede na line´arn´ı soustavu rovnic, ze kter´e lze pro kaˇzdou polohu manipul´ atoru dopoˇc´ıtat vztah mezi pasivn´ımi a aktivn´ımi rychlostmi kloubov´ ych souˇradnic. Analogick´ y v´ ysledek lze z´ıskat pˇr´ımou ˇcasovou derivac´ı rovnice (4.90). Rychlost koncov´eho efektoru manipul´atoru lze potom z´ıskat jako ˇreˇsen´ı ´ s´eriov´eho ˇretˇezce vedouc´ıho od z´akladny manipul´atoru ke koncov´emu efektoru, POKU nebot’ vˇsechny polohy i rychlosti pasivn´ıch i aktivn´ıch kloubov´ ych souˇradnic jsou nyn´ı zn´amy. Postup lze analogicky rozˇs´ıˇrit i pro v´ ypoˇcet zrychlen´ı koncov´eho efektoru. V pˇr´ıpadˇe dekompozice paraleln´ıho mechanismu na s´eriov´e manipul´atory, viz Ka´ stanoven´ım rychlosti pˇr´ıpojn´ pitola 4.7.2, lze ˇreˇsit POKU ych bod˚ u koncov´eho efektoru k jednotliv´ ym kinematick´ ym ˇretˇezc˚ um. Z Obr´azku 4.29 je zˇrejm´e, ˇze pro danou polohu koncov´eho efektoru, lze stanovit translaˇcn´ı rychlosti jeho pˇr´ıpojn´ ych bod˚ u Bi z translaˇcn´ı O be a u ´hlov´e ω be rychlosti koncov´eho efektoru jako: −−→ ˙b=O ˙ b + ωb × − B Oe B i b i e e
200
(4.209)
4.8 Kinematika manipul´ ator˚ u: Z´ avislosti rychlost´ı a zrychlen´ı −−→ ˙ba− ´ IKU). ´ kde O O e B i b jsou urˇceny pro danou polohu koncov´eho efektoru (ˇreˇsen´ı PKU, e Hodnoty rychlost´ı aktivn´ıch i pasivn´ıch kloubov´ ych souˇradnic lze pak nal´ezt ˇreˇsen´ım z´avislost´ı rychlost´ı pro kaˇzd´ y d´ılˇc´ı kinematick´ y ˇretˇezec manipul´atoru, viz Kapitola 4.8.1. Vzhledem k tomu, ˇze vˇetˇsina paraleln´ıch manipul´ator˚ u m´a pouze jednoduch´e kine´ stanovit jednoduˇse jako matick´e ˇretˇezce s aktivn´ım kloubem typu P, lze ˇcasto IOKU projekci rychlost´ı pˇr´ıpojn´ ych bod˚ u B i do smˇeru translaˇcn´ı osy aktivn´ı kloubov´e souˇradnice, jak ukazuje n´ asleduj´ıc´ı pˇr´ıklad:
´ pro PSZ ) F Pˇ r´ıklad 4.14 (IOKU Rychlosti pˇr´ıpojn´ ych bod˚ u D i koncov´eho efektoru PSZ jsou d´any, analogicky k rov˙ b = 0): nici (4.209) jako (translaˇcn´ı rychlost koncov´eho efektoru je nulov´a ⇒ O e
−−→ ˙ b = ωb × − D Oe Di b i e
(4.210)
pro i = 1 . . . 3. Z Obr´ azku 4.44 je zˇrejm´e, ˇze projekce rychlosti aktivn´ı kloubov´e souˇradnice d˙i do ˙ i do smˇeru ramene C i D i mus´ı b´ yt totoˇzn´a, jako projekce rychlosti pˇr´ıpojn´eho bodu D stejn´eho smˇeru.
Obr´ azek 4.44: Projekce rychlost´ı v kinematick´em ˇretˇezci PSZ (u je jednotkov´ y smˇerov´ y vektor aktu´ ator˚ u)
Vyuˇzijeme-li rovnici (4.210) a vlastnost´ı vektorov´eho a skal´arn´ıho souˇcinu, dosta-
201
´ 4. UVOD DO ROBOTIKY
neme: −−−→ −−−→ − ˙b u ) = (C i D i b )T · D (C i D i b )T · (d˙i → i −−−→ −−−→b T (C i D i ) · (ω be × O e D i b ) ˙ di = −−−→ − u (C i D i b )T · → −−−→b −−−→b T (O e D i × C i D i ) · ω be d˙i = −−−→ − (C i D i b )T · → u ´ pro PSZ lze tedy ps´ IOKU at jako: d˙1 ˙ −1 b d2 = J (Θ) · ω e d˙3 kde inverzn´ı kinematick´ y jakobi´ an J −1 (Θ) je: −−−−→b
−−−−→ (O e D 1 ×C 1 D 1 b )T −−−−→b T − (C 1 D 1 ) · → u
(4.211)
(4.212)
. . . . . . . . . . . . . . . . −−−−→b −−−−→b T 2 ×C 2 D 2 ) J −1 (Θ) = (Oe D −−−−→ → u (C 2 D2 b )T ·− . . . . . . . . . . . . . . . . −−−−→b −−−−→b T (O e D 3 ×C 3 D 3 ) −−−−→ → (C 3 D 3 b )T ·− u
h iT a Θ = d1 d2 d3 . Pro v´ ypoˇcet rychlosti koncov´eho efektoru, definovan´eho derivacemi zobecnˇen´ ych h iT b ˙ souˇradnic X = α˙ β˙ γ˙ , viz Kapitola 4.4.3, z u ´hlov´e rychlosti ω e pouˇzijeme Eulerovy kinematick´e rovnice, viz rovnice (4.31). Anal´ yza z´ avislost´ı zrychlen´ı by vedla opˇet na nutnost v´ ypoˇctu ˇcasov´e derivace J −1 , kterou je moˇzn´e z´ıskat podobnˇe jako v Kapitole 4.8.2. F
202
4.8 Kinematika manipul´ ator˚ u: Z´ avislosti rychlost´ı a zrychlen´ı
4.8.4
Singul´ arn´ı polohy manipul´ atoru
Singul´ arn´ı polohy manipul´ ator˚ u jsou specifick´e polohy koncov´eho efektoru, kter´e v´ yznamnˇe ovlivˇ nuj´ı jeho kinematick´e vlastnosti. Tuto skuteˇcnost je tˇreba br´at vˇzdy v u ´vahu pˇri synt´eze, anal´ yze i ˇr´ızen´ı manipul´ator˚ u. Z Kapitoly 4.8 vypl´ yv´ a, ˇze mezi rychlostmi kloubov´ ych a zobecnˇen´ ych souˇradnic manipul´ atoru existuje, pro danou konstantn´ı polohu manipul´atoru, line´ arn´ı z´ avislost dan´ a kinematick´ ym ˇci analytick´ ym jakobi´anem, d´ale jen jakobi´anem, jako:
˙ = J (Θ) · Θ ˙ X
(4.213)
Pˇredpokl´ adejme d´ ale obecn´ y pˇr´ıpad redundantn´ıch manipul´ator˚ u, tedy manipul´ator˚ u, kde poˇcet DoF koncov´eho efektoru M je menˇs´ı neˇz poˇcet nez´avisl´ ych aktivn´ıch kloubov´ ych souˇradnic N . V pˇr´ıpadˇe neredundantn´ıch manipul´ator˚ u diskutovan´ ych v pˇredchoz´ıch kapitol´ ach plat´ı M = N . Jakobi´an J (Θ) = J bude tedy reprezentovan´ y matic´ı tvaru [M × N ] s hodnost´ı rank(J ) = r.
Singularity pro s´ eriov´ e manipul´ atory Singul´arn´ı polohou s´eriov´eho manipul´ atoru, tzv. s´eriovou singularitou rozum´ıme takovou polohu koncov´eho efektoru, pro kterou jakobi´ an J ztr´ ac´ı svou hodnost r < min (M, N ). Ze z´avislosti mezi rychlostmi (4.213) lze tedy usuzovat, ˇze existuje nenulov´a rychlost kloubov´ ych souˇradnic ˙ 6= 0 pro kterou se koncov´ ˙ = 0. Z´aroveˇ Θ y efektor nem˚ uˇze pohybovat X n plat´ı, ˇze v bl´ızkosti takov´ ych poloh je pro mal´e rychlosti koncov´eho efektoru zapotˇreb´ı velk´ ych rychlost´ı kloubov´ ych souˇradnic. Pˇr´ıklad singul´arn´ı polohy pro antropomorfn´ı manipul´ator se sf´erick´ ym z´ apˇest´ım z Kapitoly 4.4.1 ukazuje Obr´azek 4.45.
203
´ 4. UVOD DO ROBOTIKY
(a) Singul´ arn´ı poloha AM - rychlost kloubov´ ych souˇradnic φ1 6= 0, φ2 = φ3 = 0 generuje nulovou rychlost koncov´eho efektoru
(b) Singul´ arn´ı poloha SZ - rychlost kloubov´ ych souˇradnic φ4 = −φ6 , φ5 = 0 generuje nulovou rychlost koncov´eho efektoru
Obr´ azek 4.45: S´eriov´e singul´arn´ı polohy AM+SZ
ˇ Singularity pro paraleln´ı manipul´ atory Casovou derivac´ı vztahu (4.96) ˇreˇs´ıc´ı ´ pro paraleln´ı manipul´ IKU atory lze odvodit form´aln´ı tvar inverzn´ıho jakobi´anu jako: ˙ =B·X ˙ A·Θ ˙ = A−1 · B ·X ˙ Θ | {z } J −1 −1
˙ ˙ = B · A ·Θ X | {z }
(4.214)
J
Singul´arn´ı polohy pro paraleln´ı manipul´atory lze tedy dˇelit na tˇri typy: • Matice A nem´ a plnou hodnost ⇒ s´eriov´ a singularita (jakobi´an J nem´a plnou hodnost). Tento typ singularity koresponduje se s´eriovou singul´arn´ı polohou s´eriov´ ych manipul´ ator˚ u reprezentuj´ıc´ı nez´avisl´e kinematick´e ˇretˇezce manipul´atoru. Tedy v takov´emto pˇr´ıpadˇe opˇet existuje nenulov´a rychlost kloubov´ ych souˇradnic ˙ 6= 0 pro kterou se koncov´ ˙ = 0. Tento typ singuΘ y efektor nem˚ uˇze pohybovat X larity b´ yv´ a ˇcasto chybnˇe zamˇen ˇov´an s hranic´ı pracovn´ıho prostoru manipul´atoru, pro kterou plat´ı to sam´e (obecnˇe vˇsak s´eriov´a singularita na hranici pracovn´ıho prostoru nemus´ı nast´ avat).
204
4.8 Kinematika manipul´ ator˚ u: Z´ avislosti rychlost´ı a zrychlen´ı
• Matice B nem´ a plnou hodnost ⇒ paraleln´ı singularita (inverzn´ı jakobi´an J −1 ˙ = nem´ a plnou hodnost). V takov´em pˇr´ıpadˇe existuje nenulov´a rychlost X 6 0 ˙ = 0 aktivn´ıch kloubov´ koncov´eho efektoru generuj´ıc´ı nulovou rychlost Θ ych souˇradnic. Koncov´ y efektor tedy z´ısk´av´a tzv. neˇriditeln´e stupnˇe volnosti. V praxi je nezbytnˇe nutn´e se okol´ı takov´ ych poloh vyvarovat, nebot’ manipul´ator se v nich st´ av´ a v podstatˇe neˇriditeln´ ym.
• Matice A ani B nem´ a plnou hodnost. Nast´avaj´ı oba typy singularit souˇcasnˇe.
F Pˇ r´ıklad 4.15 (Singul´ arn´ı polohy PSZ) Z rovnice pro v´ ypoˇcet inverzn´ıho kinematick´eho jakobi´anu (4.211) lze odvodit tvar matic A a B: ˙ = B · ωb A·Θ e
⇒
˙ = A−1 · B ·ω b Θ | {z } e J
(4.215)
−1
kde −−−−→ −−−−→ (O e D 1 b × C 1 D 1 b )T −−−−→b T → . . . . . . . . . . . . . . . . . . . . (C 1 D 1 ) · − u 0 0 −−−−→b −−−−→b T −−−−→ − A= ( 0 (C 2 D 2 b )T · → u 0 a B= Oe D2 × C 2 D2 ) −−−−→b T → 0 0 (C 3 D 3 ) · − u . . . . . . . . . . . . . . . . . . . . −−−−→ −−−−→ (O e D 3 b × C 3 D 3 b )T (4.216) Z matice A vypl´ yv´ a, ˇze PSZ se nach´az´ı v s´eriov´e singul´arn´ı poloze pokud je alespoˇ n −−−→ → − jedna dvojice vektor˚ u C i D i a u vz´ajemnˇe kolm´a, viz Obr´azek 4.47. Z matice B vypl´ yv´ a, ˇze PSZ se nach´ az´ı v paraleln´ı singul´arn´ı poloze pokud je alespoˇ n jeden vektor −−−→b −−−→b −−−→b −−−→b (O e D i × C i D i ) nulov´ ym vektorem (O e D i a C i D i jsou vz´ajemnˇe rovnobˇeˇzn´e), nebo −−−→b −−−→b jsou vektory (O e D i × C i D i ) line´arnˇe z´avisl´e, viz Obr´azek 4.46 a Obr´azek 4.47.
205
´ 4. UVOD DO ROBOTIKY
(a) α = 0, β = 0, γ = − 32 π
(b) α = 0, β = 0, γ = 31 π
Obr´ azek 4.46: Paraleln´ı singul´ arn´ı polohy PSZ, vektory O e D i b × C i D i b jsou line´arnˇe z´ avisl´e (leˇz´ı v jedn´e rovinˇe se vz´ ajemn´ ym pootoˇcen´ım o 23 π)
Obr´ azek 4.47: PSZ souˇcasnˇe v s´eriov´e i paraleln´ı singul´arn´ı poloze, α = β = γ = 0, n´ avrhov´e parametry ξ jsou odliˇsn´e od pˇredchoz´ıho pˇr´ıpadu, vektory (O e D i b × C i D i b ) jsou line´ arnˇe z´ avisl´e, vektory C i D i a u jsou kolm´e
F
206
4.9 Numerick´ y pˇ r´ıstup ˇ reˇ sen´ı kinematiky manipul´ ator˚ u
4.9
Numerick´ y pˇ r´ıstup ˇ reˇ sen´ı kinematiky manipul´ ator˚ u
V pˇredchoz´ıch kapitol´ ach byl uveden obecn´ y pˇr´ıstup k ˇreˇsen´ı kinematiky manipul´ator˚ u. Pˇr´ım´ a kinematika pro s´eriov´e manipul´ atory a inverzn´ı kinematika pro paraleln´ı manipul´ atory (uvaˇzujeme jednoduch´e kinematick´e ˇretˇezce) je relativnˇe jednoduch´a a ˇreˇsiteln´ a analyticky. Jejich implementace do ˇr´ıdic´ıch algoritm˚ u manipul´ator˚ u proto vˇetˇsinou neˇcin´ı vˇetˇs´ı pot´ıˇze a v´ ypoˇcetn´ı n´ aroˇcnost takov´ ych u ´loh je uspokojiv´a vzhledem k nasazen´ı v souˇcasn´ ych pr˚ umyslov´ ych poˇc´ıtaˇc´ıch. Opaˇcn´e kinematick´e u ´lohy (inverzn´ı pro s´eriov´e manipul´ atory a pˇr´ım´ a pro paraleln´ı manipul´ atory) lze uspokojivˇe ˇreˇsit pouze ve specifick´ ych pˇr´ıpadech a obecnˇe s sebou pˇrin´aˇsej´ı ˇradu probl´em˚ u pˇri jejich implementaci do ˇr´ıdic´ıch algoritm˚ u. Mezi nejv´ yznamnˇejˇs´ı m˚ uˇzeme oznaˇcit tyto n´asleduj´ıc´ı: • v obecn´em pˇr´ıpadˇe neexistuje analytick´e ˇreˇsen´ı • metody zaloˇzen´e na nalezen´ı charakteristick´eho polynomu manipul´atoru (dyalitick´ a eliminace, Gr¨ obnerovy b´aze) pˇrin´aˇsej´ı nemal´e probl´emy vzhledem ke stabilitˇe a pˇresnosti ˇreˇsen´ı (hled´an´ı koˇren˚ u polynom˚ u vysok´ ych ˇr´ad˚ u) • v´ ypoˇcetn´ı n´ aroˇcnost metod (intervalov´a anal´ yza, homotopie, dosazov´an´ı do komplikovan´ ych algebraick´ ych vztah˚ u) je pˇr´ıliˇs velik´a pro implemenatci do ˇr´ıdic´ıch algoritm˚ u (v re´ aln´em ˇcase) • nalezen´ı v´ıce moˇzn´ ych ˇreˇsen´ı kinematick´ ych u ´loh vyˇzaduje implementovat metody pro rozhodov´ an´ı o aktu´ aln´ım korektn´ım ˇreˇsen´ı ´ pro V n´ asleduj´ıc´ım textu se omezme pouze na numerick´e metody v´ ypoˇctu IKU s´ eriov´ e manipul´ atory. Metoda m˚ uˇze b´ yt ekvivalentnˇe snadno rozˇs´ıˇrena i na ma´ m´ame k dispozici polohy, rychlosti a zrychlen´ı nipul´ atory paraleln´ı. Pˇri ˇreˇsen´ı IKU kloubov´ ych souˇradnic: Θ=
ϑ1 ϑ2 . . . ϑ N
T
D´ ale pak line´ arn´ı vztah mezi rychlostmi aktivn´ıch kloubov´ ych a zobecnˇen´ ych souˇradnic pro konstantn´ı polohu koncov´eho efektoru, viz rovnice (4.213): ˙ = J (Θ) · Θ ˙ X
207
(4.217)
´ 4. UVOD DO ROBOTIKY
Zab´ yvejme se d´ ale nˇekter´ ymi vlastnostmi jakobi´anu J .Provedeme-li singul´arn´ı dekompozici matice J , jej´ıˇz hodnost je rovna r, dost´av´ame: J =U ·Σ·VT
(4.218)
kde matice U M ×M respektive V N ×N jsou unit´arn´ı matice jejichˇz sloupce vyjadˇruj´ı lev´ y respektive prav´ y singul´ arn´ı vektor matice J . Matice ΣM ×N je ve tvaru: Σ=
S r×r
0r×(N −r)
0(M −r)×r 0(M −r)×(N −r)
(4.219)
kde S je diagon´ aln´ı matice s diagon´ aln´ımi prvky σ1 , σ2 , . . . σr , pro kter´e plat´ı σ1 ≥ σ2 ≥ · · · ≥ σr ≥ 0 reprezentuj´ıc´ı singul´ arn´ı ˇc´ısla matice J . Rozeps´an´ım rovnice (4.218) s pomoc´ı (4.219) dostaneme: ˙ =U ·Σ·VT ·Θ ˙ X r X ˙ = ˙ X σi · U [:, i] · V [:, i]T · Θ i=1
˙ = σ1 · U [:, 1] · V [:, 1]T · Θ ˙ + σ2 · U [:, 2] · V [:, 2]T · Θ ˙ + · · · + σr · U [:, r] · V [:, r]T · Θ ˙ X (4.220) Nutnˇe tedy plat´ı n´ asleduj´ıc´ı tvrzen´ı, grafick´e zn´azornˇen´ı viz Obr´azek 4.48: ˙ = a, kde a = a1 . . . ai . . . aN T a ai ∈ R, plat´ı • Oznaˇc´ıme-li V T · Θ ˙ = V · a, tedy sloupce V [:, 1] aˇz V [:, r] matice V generuj´ı prostor kloubov´ Θ ych ⊥ ˙ tzv. doplnˇek null space N(J ), oznaˇcen´ rychlost´ı Θ, y jako N (J ), viz n´asleduj´ıc´ı bod. dim(N⊥ (J )) = r • Zˇrejmˇe plat´ı, ˇze zb´ yvaj´ıc´ı sloupce V [:, r + 1] aˇz V [:, N ] generuj´ı prostor takov´ ych ˙ pro kter´ rychlost´ı kloubov´ ych souˇradnic Θ, y je rychlost koncov´eho efektoru vˇ zdy ˙ = 0, oznaˇcme jej jako tzv. null space N(J ). nulov´a X dim(N(J )) = N − r
208
4.9 Numerick´ y pˇ r´ıstup ˇ reˇ sen´ı kinematiky manipul´ ator˚ u
• Sloupce U [:, 1] aˇz U [:, r] matice U potom generuj´ı prostor dosaˇ ziteln´ ych rych˙ oznaˇcme jej jako tzv. range space R(J ). lost´ı koncov´eho efektoru X, dim(R(J )) = r • Zˇrejmˇe plat´ı, ˇze zb´ yvaj´ıc´ı sloupce U [:, r + 1] aˇz U [:, M ] generuj´ı prostor nedo˙ tzv. doplnˇek range space R(J ), saˇ ziteln´ ych rychlost´ı koncov´eho efektoru X, oznaˇcen´ y jako R⊥ (J ). dim(R⊥ (J )) = M − r
Obr´ azek 4.48: Prostory generovan´e vlastn´ımi vektory jakobi´anu J
4.9.1
´ pro neredundantn´ı manipul´ IKU atory
´ pro neredundantn´ı manipul´atory, Diskutujme nejprve pˇr´ıpad numerick´eho ˇreˇsen´ı IKU tedy pro pˇr´ıpad, kdy poˇcet DoF koncov´emu efektoru pˇresnˇe odpov´ıd´a poˇctu nez´avisl´ ych aktivn´ıch kloubov´ ych souˇradnic manipul´atoru (M = N ). Pˇredpokl´adejme, ˇze jakobi´ an J nen´ı singul´ arn´ı matic´ı, tzn. jeho hodnost je pln´a, r = M = N . Z pˇredchoz´ı anal´ yzy matice jakobi´ anu J vypl´ yv´ a, ˇze neexistuj´ı ˇz´adn´e nenulov´e hodnoty rychlost´ı aktivn´ıch ˙ ˙ = 0, kloubov´ ych souˇradnic Θ, kter´e by generovaly nulov´ y pohyb koncov´eho efektoru, X nebot’ dim(N(J )) = N − r = N − N = 0.
209
´ 4. UVOD DO ROBOTIKY
Inverz´ı rovnice (4.217), zaveden´ım diskr´etn´ıch ˇcasov´ ych okamˇzik˚ u tk , tk+1 . . . , ˇca˙ dost´av´ame: sov´eho pˇr´ır˚ ustku ∆t = tk+1 − tk a n´ aslednou aproximac´ı derivace Θ ˙ = J −1 (Θ) · X ˙ Θ Θ(tk+1 ) − Θ(tk ) ˙ k) = J −1 (Θ(tk )) · X(t ∆t ˙ k ) · ∆t Θ(tk+1 ) = Θ(tk ) + J −1 (Θ(tk )) · X(t
(4.221)
´ dan´ Bohuˇzel, v´ ypoˇcet IKU y rovnic´ı (4.221), vyuˇz´ıvaj´ıc´ı pˇr´ıstup klasick´e numerick´e integrace rychlosti koncov´eho efektoru, v praktick´ ych aplikac´ı nelze pouˇz´ıt, nebot’ vlivem nepˇresnost´ı doch´ az´ı k nasˇc´ıt´ av´ an´ı numerick´ ych chyb v ˇcase. Alternativn´ım pˇr´ıstupem je zaveden´ı chyby e mezi poˇzadovanou X d a aktu´aln´ı X polohou zobecnˇen´ ych souˇradnic, z´ıskanou z dostupn´ ych hodnot namˇeˇren´ ych kloubov´ ych ´ Snadno tedy vyj´adˇr´ıme, pomoc´ı rovnice (4.217), i souˇradnic Θ prostˇrednictv´ım PKU. jej´ı ˇcasovou derivaci: e = X d − X = X d − G(Θ)
(4.222)
˙ d−X ˙ e˙ = X ˙ d − J (Θ) · Θ ˙ e˙ = X
(4.223)
´ viz rovnice (4.46), z Kapitoly 4.6.1. kde G(Θ) oznaˇcuje ˇreˇsen´ı PKU, Rovnice (4.223) reprezentuje v´ yvoj chyby v´ ypoˇctu zobecnˇen´ ych souˇradnic X v ˇcase. Vhodnou volbou z´ avislosti mezi e˙ a X lze zajistit konvergenci e k nule v exponenci´aln´ım ˇcase. Poloˇzme proto e˙ K-maticov´emu n´asobku z´apornˇe vzat´e odchylky e: !
e˙ = −K · e ˙ d − J (Θ) · Θ ˙ = −K[X d − G(Θ)] X
(4.224) (4.225)
Je zˇrejm´e, ˇze rovnice (4.224) reprezentuje line´arn´ı syst´em chyby e. Pokud je matice K volena jako pozitivnˇe definitn´ı, line´arn´ı syst´em chyby e je asymptoticky stabiln´ı, chyba e → 0, tedy G(Θ) → X d a tedy Θ konverguje ke skuteˇcn´ ym hodnot´am kloubov´ ych souˇradnic odpov´ıdaj´ıc´ım aktu´ aln´ı poloze koncov´eho efektoru. Rovnice (4.225) lze pˇrepsat do tvaru: ˙ = J −1 (Θ) X ˙ d + K[X d − G(Θ)] Θ
210
(4.226)
4.9 Numerick´ y pˇ r´ıstup ˇ reˇ sen´ı kinematiky manipul´ ator˚ u
˙ v (4.226) dost´av´ame pˇredpis pro korektn´ı numerick´e ˇreˇsen´ı IKU ´ Aproximac´ı derivace Θ neredundantn´ıch s´eriov´ ych manipul´ator˚ u: ˙ d + K[X d − G(Θ(tk ))] ∆t Θ(tk+1 ) = Θ(tk ) + J −1 (Θ(tk )) X
(4.227)
˙ d = 0 odpov´ıd´a Newtonovu algoritmu numericPoznamenejme, ˇze vztah (4.227) pro X k´eho hled´ an´ı ˇreˇsen´ı Θ rovnice X d − G(Θ) = 0.
4.9.2
´ pro redundantn´ı manipul´ IKU atory
´ pro neredundantn´ı manipul´atory, kdy poˇcet DoF V pˇr´ıpadˇe numerick´eho ˇreˇsen´ı IKU koncov´emu efektoru je menˇs´ı neˇz poˇcet nez´avisl´ ych aktivn´ıch kloubov´ ych souˇradnic ma˙ kter´e nipul´ atoru (M < N ), existuj´ı takov´e rychlosti aktivn´ıch kloubov´ ych souˇradnic Θ, ˙ Tyto rychlosti kloubov´ generuj´ı nulov´ y pohyb koncov´eho efektoru X. ych souˇradnic leˇz´ı v prostoru N(J ), jehoˇz dimenze je dim(N(J )) = N − r = N − M . Opˇet pˇredpokl´ad´ame, ˇze jakobi´ an J m´ a plnou hodnost, tedy r = min (M, N ) = M . V takov´em pˇr´ıpadˇe lze ˇr´ıci, ˇze pokud rychlosti kloubov´ ych souˇradnic Θ˙ ? jsou ˇreˇsen´ım rovnice (4.217), potom jsou ˇreˇsen´ım i rychlosti (rovnice m´a nekoneˇcnˇe mnoho ˇreˇsen´ı): ˙ =Θ ˙ ?+P ·Θ ˙0 Θ
(4.228)
kde P je matice [N, N ], jej´ıˇz sloupce generuj´ı null space jakobi´anu J , tedy plat´ı R(P ) = ˙ 0 ∈ RN je libovoln´ N(J ). Θ y vektor. Toto tvrzen´ı lze snadno dok´azat dosazen´ım ˇreˇsen´ı (4.228) do rovnice (4.217). ˙ = J · (Θ ˙ ?+P ·Θ ˙ 0) = J · Θ ˙ ?+J ·P ·Θ ˙ =X ˙ J ·Θ | {z }0 =0
Jin´ ymi slovy m˚ uˇzeme ˇr´ıci, ˇze pro redundantn´ı manipul´atory lze nal´ezt takov´e rych˙ = P ·Θ ˙ 0 , kter´e negeneruj´ı rychlost (tedy ani zmˇenu polosti kloubov´ ych souˇradnic Θ ˙ a zp˚ lohy) koncov´eho efektoru manipul´atoru X usobuj´ı pouze ”vnitˇrn´ı pohyb”mechanick´e konstrukce manipul´ atoru. Toho lze vyuˇz´ıt pro pˇrekonfigurov´an´ı manipul´atoru pˇri konstantn´ı poloze koncov´eho efektoru za u ´ˇcelem splnˇen´ı ˇci optimalizov´an´ı nˇejak´eho pˇr´ıdavn´eho krit´eria, napˇr. udrˇzov´ an´ı manipul´atoru co nejd´al od moˇzn´ ych singul´arn´ıch poloh, od limitn´ıch poloh aktu´ ator˚ u ˇci od pˇrek´aˇzek v pracovn´ım prostoru. ˙ koncov´eho efektoru redunPˇredpokl´ adejme, ˇze chceme doc´ılit zadan´eho pohybu X ˙ = J ·Θ ˙ (prim´arn´ı krit´erium - vazebn´ı dantn´ıho manipul´ atoru, tedy splnˇen´ı podm´ınky X
211
´ 4. UVOD DO ROBOTIKY
podm´ınka, kter´ a mus´ı b´ yt vˇzdy splnˇena) a z´aroveˇ n budeme poˇzadovat, aby se rychlosti ˙ co nejv´ıce bl´ıˇzily poˇzadovan´e hodnotˇe Θ ˙ 0 (dan´e sekund´arn´ım, kloubov´ ych souˇradnic Θ pˇr´ıdavn´ ym krit´eriem). Definujme tedy kriteri´ aln´ı funkci: ˙ = 1 (Θ ˙ −Θ ˙ 0 )T · (Θ ˙ −Θ ˙ 0) Q(Θ) 2
(4.229)
Prostˇrednictv´ım Lagrangeov´ ych multiplik´ator˚ u λ ∈ RM zavedeme rozˇs´ıˇrenou kriteri´aln´ı funkci zohledˇ nuj´ıc´ı vazebn´ı podm´ınku: ˙ λ) = 1 (Θ ˙ −Θ ˙ 0 )T · (Θ ˙ −Θ ˙ 0 ) + λT (X ˙ − J · Θ) ˙ Q(Θ, 2
(4.230)
Z nutn´e podm´ınky existence extr´emu dost´av´ame: ˙ λ) ∂Q(Θ, ˙ −Θ ˙ 0 − JT · λ = 0 =Θ ˙ ∂Θ ˙ =Θ ˙ 0 + JT · λ Θ
(4.231)
Vyn´asoben´ım rovnice (4.231) zleva J s respektov´an´ım rovnice (4.217) a vyj´adˇren´ım multiplik´atoru λ dost´ av´ ame: ˙ =J ·Θ ˙ 0 + J · JT · λ J ·Θ | {z } ˙ X
˙ −J ·Θ ˙ 0) λ = (J · J T )−1 (X
(4.232)
Dosazen´ım rovnice (4.232) zpˇet do (4.231) dost´av´ame v´ ysledn´e rychlosti kloubov´ ych ˙ splˇ souˇradnic Θ nuj´ıc´ı prim´ arn´ı a minimalizuj´ıc´ı sekund´arn´ı krit´erium. ˙ =Θ ˙ 0 + J T · (J · J T )−1 (X ˙ −J ·Θ ˙ 0) Θ ˙ = J† · X ˙ + (I − J † · J ) · Θ ˙0 Θ
(4.233)
kde J † = J T · (J · J T )−1 je prav´ a zobecnˇen´a inverze jakobi´anu J . ˙ 0 = 0 rovnice (4.233) pˇrejde na tvar standartn´ı zobecnˇen´e inverze Θ ˙ = J† ·X ˙ Pro Θ ˙ kloubov´ a sekund´arn´ı krit´erium tak minimalizuje velikosti rychlost´ı kΘk ych souˇradnic ˙ Z porovn´ ˙ pˇredstavuje Θ. an´ı v´ ysledku (4.233) s rovnic´ı (4.228) je zˇrejm´e, ˇze ˇclen J † · X ˙ ? a ˇclen (I − J † · J ) matici P generuj´ıc´ı null space jakobi´anu J prostˇrednictv´ım ˇreˇsen´ı Θ ˙ 0. souˇradnic Θ
212
4.9 Numerick´ y pˇ r´ıstup ˇ reˇ sen´ı kinematiky manipul´ ator˚ u
˙ 0 d´ Necht’ je vektor Θ an gradientem sekund´arn´ı kriteri´aln´ı funkce w(Θ) jako: ∂w(Θ) ˙ Θ0 = k0 · (4.234) ∂Θ kde k0 ∈ R je konstanta zes´ılen´ı. ˙ aΘ ˙ 0 z krit´eria (4.229) se tedy optim´alnˇe bl´ıˇz´ıme Minimalizov´ an´ım vzd´ alenosti Θ ke gradientu sekund´ arn´ı kriteri´aln´ı funkce w(Θ), tedy manipul´ator se pohybuje takov´ ym zp˚ usobem, ˇze kriteri´ aln´ı funkce w(Θ) je maximalizov´ana pˇri souˇcasn´em dodrˇzen´ı poˇzadovan´e trajektorie pohybu koncov´eho efektoru (prim´arn´ı krit´erium). Mezi nejˇcastˇejˇs´ı volby kriteri´aln´ı funkce w(Θ) patˇr´ı n´asleduj´ıc´ı: • Vzd´ alenost od singul´ arn´ıch poloh: w(Θ) =
q det J · J T
(4.235)
kde kriteri´ aln´ı funkce pˇredstavuje odmocninu ze souˇcinu singul´arn´ıch ˇc´ısel σi jakobi´ anu J . Maximalizov´an´ım kriteri´aln´ı funkce tedy rekonfigurujeme mechaniku manipul´ atoru takov´ ym zp˚ usobem, aby se nach´azel co nejd´ale od sv´ ych singul´ arn´ıch poloh (σi = 0). • Vzd´ alenost od limitn´ıch poloh n aktu´ator˚ u: n 1 X ϑi − ϑ¯i w(Θ) = − 2n ϑmax − ϑmin i i i=1 kde ϑ¯i =
ϑmax −ϑmin i i 2
(4.236)
a ϑmax respektive ϑmin oznaˇcuje maxim´aln´ı respektive minii i
m´ aln´ı polohu aktu´ atoru. Maximalizac´ı kriteri´aln´ı funkce rekonfigurujeme mechaniku manipul´ atoru takov´ ym zp˚ usobem, aby se aktu´atory manipul´atoru udrˇzovaly co nejd´ ale od sv´ ych limitn´ıch poloh. ´ (4.225) zobecnˇenou inverz´ı Nahrad´ıme-li nyn´ı inverzi J −1 v algoritmu v´ ypoˇctu IKU se zahrnut´ ym sekund´ arn´ım krit´eriem (4.233) dost´av´ame: ˙ = J † (Θ) X ˙ d + K[X d − G(Θ)] + I − J † (Θ) · J (Θ) · Θ ˙0 Θ
(4.237)
˙ v (4.237) dost´av´ame pˇredpis pro korektn´ı numerick´e ˇreOpˇet aproximac´ı derivace Θ ´ redundantn´ıch s´eriov´ ˇsen´ı IKU ych manipul´ator˚ u se sekund´arn´ım krit´eriem definovan´ ym funkc´ı w(Θ): h ˙ d + K[X d − G(Θ(tk ))] + Θ(tk+1 ) = Θ(tk ) + J † (Θ(tk )) X i ˙ 0 (tk ) ∆t + I − J † (Θ(tk )) · J (Θ(tk )) · Θ
213
(4.238)
´ 4. UVOD DO ROBOTIKY
4.9.3
´ pro polohy manipul´ IKU atoru v bl´ızkosti singularit
´ Dosud jsme se zab´ yvali pouze probl´emem nalezen´ı numerick´eho algoritmu v´ ypoˇctu IKU v pˇr´ıpadˇe, kdy se s´eriov´ y manipul´ ator nenal´ez´a v s´eriov´e singul´arn´ı poloze. Tzn. jakobi´an J m´a plnou hodnost r = min (M, N ). Nen´ı-li tento pˇredpoklad splnˇen (manipul´a´ pro neredundantn´ı, tor se nach´az´ı v singul´ arn´ı poloze) nast´ av´a probl´em s algoritmy IKU viz Kapitola 4.9.1 i redundantn´ı manipul´atory, viz Kapitola 4.9.2: • Neredundantn´ı manipul´ atory (M = N ): Jakobi´ an J je singul´ arn´ı matic´ı, neexistuje tedy jeho inverze J −1 . V bl´ızkosti ´ singul´arn´ı polohy je pak operace inverze ˇspatnˇe podm´ınˇen´a a algoritmus IKU vede na velk´e hodnoty rychlost´ı kloubov´ ych souˇradnic (pˇr´ımo v singul´arn´ı poloze na nekoneˇcnˇe velk´e hodnoty). • Redundantn´ı manipul´ atory (M < N ): Matice J · J T je singul´ arn´ı, neexistuje tedy jej´ı inverze (J · J T )−1 a tedy ani zobecnˇen´ a inverze J † . V bl´ızkosti singul´arn´ıch poloh nast´av´a stejn´ y probl´em jako v pˇredchoz´ım bodˇe. ˇ sen´ı pˇrin´aˇs´ı vyuˇzit´ı metody tlumen´ Reˇ ych nejmenˇs´ıch ˇctverc˚ u (neredundantn´ı manipul´atory) ˇci tlumen´e pseudoinverze (redundantn´ı manipul´atory). Obˇe metody vych´azej´ı z pˇredpokladu, ˇze v singul´ arn´ı poloze manipul´atoru nen´ı moˇzn´e, z v´ yˇse uveden´ ych d˚ u˙ = J ·Θ ˙ a vod˚ u, pˇresnˇe splnit vztah mezi zobecnˇen´ ymi a kloubov´ ymi rychlostmi X ˙ v pˇrijateln´ z´aroveˇ n udrˇzet rychlosti kloubov´ ych souˇradnic Θ ych mez´ıch. Jako kompromisn´ı ˇreˇsen´ı budeme tedy minimalizovat sm´ıˇsen´e krit´erium typu: ˙ = 1 · (X ˙ − J · Θ) ˙ T · (X ˙ − J · Θ) ˙ + α2 · 1 · Θ ˙ T ·Θ ˙ Q(Θ) 2 2
(4.239)
kde α ∈ R je v´ ahov´ y koeficient. Vˇetˇs´ı hodnoty α vedou na niˇzˇs´ı hodnoty rychlost´ı klou˙ na u bov´ ych souˇradnic Θ ´kor nepˇresnosti sledov´an´ı poˇzadovan´e trajektorie koncov´ ym efektorem a naopak. Neredundantn´ı manipul´ atory (M = N ) Z nutn´e podm´ınky existence extr´emu dost´av´ame krit´eria (4.239): ˙ ∂Q(Θ) ˙ + α2 · Θ ˙ − JT · X ˙ =0 = JT · J · Θ ˙ Θ ˙ = J T · J + α2 · I −1 · J T · X ˙ Θ
214
(4.240)
4.9 Numerick´ y pˇ r´ıstup ˇ reˇ sen´ı kinematiky manipul´ ator˚ u
´ v Kapitole 4.9.1 je tedy nahraInverze jakobi´ anu J −1 v algoritmech v´ ypoˇctu IKU zena v bl´ızkosti singularit tlumenou verz´ı s koeficientem tlumen´ı α > 0: J −1
⇒
˙ = J −1 · X ˙ Θ
⇒
J T · J + α2 · I
−1
· JT
(4.241)
Tedy: ˙ = J T · J + α2 · I Θ
−1
˙ · JT · X
(4.242)
Redundantn´ı manipul´ atory (M < N ) Analogicky lze z´ıskat tlumenou verzi pseudoinverze J † , viz [109]. Pseudoinverze jako´ v Kapitole 4.9.2 je tedy nahrazena v bl´ızkosti ypoˇctu IKU bi´anu J † v algoritmech v´ singularit tlumenou verz´ı s koeficientem tlumen´ı α > 0: J † = J T · (J · J T )−1
⇒
J T · (J · J T + α2 · I)−1
(4.243)
Provedeme-li nyn´ı singul´ arn´ı dekompozici jakobi´anu J , viz (4.218), v tlumen´e verzi inverze (4.241), dost´ av´ ame: J T · J + α2 · I
−1
· J T = (V · Σ · U T · U · Σ · V T + α2 · I)−1 · V · Σ · U T
J T · J + α2 · I
−1
· J T = (V · (Σ2 + α2 · I) · V T )−1 · V · Σ · U T
J T · J + α2 · I
−1
· J T = (V · (Σ2 + α2 · I)−1 · V T ) · V · Σ · U T
J T · J + α2 · I
−1
· J T = V · (Σ2 + α2 · I)−1 · Σ ·U T | {z }
(4.244)
¯ Σ
kde ¯ Σ=
σ1 σ12 +α2
0 .. . 0
0
...
0
0 ... .. . 0 ...
0 .. .
0
σ2 σ22 +α2
0
σr σr2 +α2
Rozeps´ an´ım rovnice (4.242) pomoc´ı (4.244) dost´av´ame d˚ ukaz, ˇze singul´arn´ı jakobi´ an J (alespoˇ n jedno jeho singul´ arn´ı ˇc´ıslo σi = 0) nevede na nekoneˇcnˇe velk´e rychlosti ˙ kloubov´ ych souˇradnic Θ: ˙ = Θ
r X i=1
σi ˙ 9 ∞ · V [:, i] · U [:, i]T · X σi →0 σi2 + α2
215
(4.245)
´ 4. UVOD DO ROBOTIKY
˙ zp˚ Chyba eΘ˙ ve v´ ypoˇctu rychlost´ı kloubov´ ych souˇradnic Θ usoben´a tlumen´ım lze vyj´adˇrit jako: eΘ˙ = =
r X i=1 r X i=1
r
X 1 σi ˙ = ˙ − · V [:, i] · U [:, i]T · X · V [:, i] · U [:, i]T · X 2 2 σi σi + α i=1 −α2 ˙ · V [:, i] · U [:, i]T · X (σi2 + α2 )σi
(4.246)
Poznamenejme, ˇze stejn´ y v´ ysledek lze z´ıskat analogicky i pro tlumenou pseudoinverzi z rovnice (4.243). V praktick´ ych aplikac´ıch ˇr´ızen´ı se koeficient tlumen´ı α ˇcasto mˇen´ı dynamicky, podle m´ıry vzd´ alenosti koncov´eho efektoru manipul´atoru k singul´arn´ı poloze. ´ s´eriov´ Postupy popsan´e v t´eto kapitole pro numerick´e ˇreˇsen´ı IKU ych manipul´ator˚ u ´ manipul´ator˚ lze analogicky aplikovat na ˇreˇsen´ı PKU u paraleln´ıch, proto se jimi jiˇz nebudeme d´ ale zab´ yvat. V´ıce o numerick´ ych pˇr´ıstupech ˇreˇsen´ı kinematiky manipul´ator˚ u lze nal´ezt v [110], [111], [112], [113].
4.9.4
Shrnut´ı
Pr´ace se zab´ yv´ a probl´emem kinematiky robotick´ ych architektur, tedy z´avislostmi mezi polohami, rychlostmi a zrychlen´ımi v mechanick´ ych konstrukc´ıch robot˚ u bez ohledu na s´ıly a silov´e momenty, kter´ ymi jsou vyvol´any. Dynamika robot˚ u proto nen´ı n´apln´ı pr´ace, avˇsak d˚ ukladn´ a kinematick´ a anal´ yza je nezbytnou podm´ınkou k jej´ımu zkoum´an´ı. Pˇrestoˇze pojem robot je dneˇsn´ı spoleˇcnost´ı ch´ap´an sp´ıˇse jako humanoidn´ı bytost schopn´a autonomn´ıho ˇzivotn´ıho cyklu (pohyb, rozhodov´an´ı, atd.) v pr´aci jsou diskutov´any robotick´e struktury spadaj´ıc´ı v´ yhradnˇe do oblasti robotick´ ych manipul´ator˚ u, kter´e hraj´ı v´ yznamnou roli nejen v pr˚ umyslu ale stejnˇe tak i v l´ekaˇrsk´ ych oborech, arm´adn´ıch aplikac´ıch, kosmick´em v´ yzkumu atd. C´ılem pr´ ace je pˇredloˇzit ucelen´ y pˇrehled st´avaj´ıc´ıch metod k ˇreˇsen´ı kinematiky manipul´ator˚ u zaloˇzen´ y na souˇcasn´em stavu v t´eto oblasti. Pˇrestoˇze, ˇrada jednoduˇsˇs´ıch probl´em˚ u v kinematice manipul´ ator˚ u m˚ uˇze b´ yt ˇreˇsena intuitivnˇe postupy, vyuˇz´ıvaj´ıc´ı pˇr´ıstupy stˇredoˇskolsk´e matematiky (analytiky, geometrie, fyziky), v pr´aci je kladen d˚ uraz pˇredevˇs´ım na moˇznost algoritmizace pˇredloˇzen´ ych metod za u ´ˇcelem jejich implementace do automatick´ ych algoritm˚ u. Tyto algoritmy by pak mˇely st´at z´akladem vytvoˇren´ı softwarov´eho n´ astroje pro intuitivn´ı automatick´e generov´an´ı model˚ u manipul´ator˚ u pro vˇedeck´e i studijn´ı u ´ˇcely, viz Kapitola 4.3. Pˇr´ıklady takov´ ych softwarov´ ych n´astroj˚ u lze nal´ezt napˇr. v [114], [115], [116].
216
4.9 Numerick´ y pˇ r´ıstup ˇ reˇ sen´ı kinematiky manipul´ ator˚ u
Probl´emy kinematiky jsou diskutov´any pro dva z´akladn´ı typy architektur manipul´ ator˚ u - s´eriov´e a paraleln´ı. Porovn´an´ı vlastnost´ı tˇechto dvou z´akladn´ıch skupin lze nal´ezt v Kapitole 4.2. V Kapitole 4.5.2 jsou pˇredstaveny dvˇe nejzn´amˇejˇs´ı u ´mluvy pro popis kinematick´ ych architektur manipul´ator˚ u, kter´e umoˇzn ˇuj´ı automatick´e rekurzivn´ı generov´ an´ı souˇradn´ ych syst´em˚ u definuj´ıc´ı polohy jednotliv´ ych ramen manipul´ator˚ u. Transformaˇcn´ı vztahy mezi souˇradn´ ymi syst´emy lze pak povaˇzovat za kinematick´ y model manipul´ atoru, jehoˇz n´ avrhov´e parametry jsou tvoˇreny sadou geometrick´ ych parametr˚ u, kter´e jednoznaˇcnˇe ud´ avaj´ı uspoˇr´ad´an´ı mechanick´e konstrukce manipul´atoru (tvar ramen, typy a um´ıstˇen´ı kloub˚ u). Kapitola 4.6 je vˇenov´ ana z´avislostem mezi kloubov´ ymi souˇradnicemi manipul´atoru a zobecnˇen´ ymi souˇradnicemi koncov´eho efektoru. Rozliˇsujeme dvˇe z´akladn´ı kinematick´e ´ tedy z´avislost polohy zobecnˇen´ u ´lohy - pˇr´ımou kinematickou u ´lohu (PKU), ych souˇrad´ nic na poloh´ ach souˇradnic kloubov´ ych a zpˇetnou (inverzn´ı) kinematickou u ´lohu (IKU), tedy z´ avislost polohy kloubov´ ych souˇradnic na souˇradnic´ıch zobecnˇen´ ych. V textu je uk´az´ ano, ˇze v´ ypoˇcetn´ı n´ aroˇcnost kinematick´ ych u ´loh jednoznaˇcnˇe z´avis´ı na typu manipul´ atoru. ´ ˇreˇsiteln´a vˇzdy analyticky a jej´ı v´ V pˇr´ıpadˇe s´eriov´ ych manipul´ator˚ u je PKU ypoˇcet ´ pro s´eriov´e manipulze z´ıskat jednoduˇse z kinematick´eho modelu manipul´atoru. PKU ´ pro s´eriov´e manipul´atory s sebou pˇrin´ l´atory vede vˇzdy na pr´ avˇe jedno ˇreˇsen´ı. IKU aˇs´ı ˇradu probl´em˚ u, nebot’ obecnˇe vede na v´ ypoˇcet soustavy transcendentn´ıch rovnic s v´ıce moˇzn´ ymi ˇreˇsen´ımi. V jednoduch´ ych pˇr´ıpadech lze tyto rovnice ˇreˇsit intuitivnˇe, viz Kapitola 4.6.2.1, takov´e postupy jsou vˇsak obt´ıˇznˇe algoritmizovateln´e. Nˇekdy je moˇzn´e dekomponovat mechanickou konstrukci manipul´atoru na dvˇe vz´ajemnˇe nez´avisl´e ˇc´asti ´ ˇreˇsit oddˇelenˇe pro kaˇzdou ˇc´ast zvl´aˇst’, viz Kapitola 4.6.2.2. Poznamenejme, ˇze a IKU tento pˇr´ıpad nast´ av´ a pro drtivou vˇetˇsinu dneˇsn´ıch pr˚ umyslov´ ych manipul´ator˚ u, kde je ´ tak ˇreˇsen´ı IKU v´ yraznˇe zjednoduˇseno (lze nal´ezt analyticky). Kapitola 4.6.2.3 analyzuje ´ obecn´eho s´eriov´eho manipul´atoru se 6DoF. Pˇredstaven´a metoda moˇznost ˇreˇsen´ı IKU vˇsak vyˇzaduje relativnˇe sloˇzit´e symbolick´e pˇredzpracov´an´ı ˇreˇsen´ ych rovnic, jejichˇz u ´pravami lze z´ıskat tzv. kinematick´ y polynom manipul´atoru (polynom v jedn´e promˇenn´e). ´ Poznamenejme, Nalezen´ı koˇren˚ u kinematick´eho polynomu potom urˇcuje ˇreˇsen´ı IKU. ´ ˇze kinematick´ y polynom je v tomto pˇr´ıpadˇe stupnˇe 16, tedy nelze nal´ezt ˇreˇsen´ı IKU v uzavˇren´em (analytick´em) tvaru. Hled´an´ı koˇren˚ u polynom˚ u vysok´ ych stupˇ n˚ u pˇrin´ aˇs´ı tak´e probl´emy se stabilitou a pˇresnost´ı numerick´ ych v´ ypoˇct˚ u.
217
´ 4. UVOD DO ROBOTIKY
´ ani Pro paraleln´ı manipul´ atory obecnˇe plat´ı, ˇze analyticky ˇreˇsiteln´a nen´ı ani PKU ´ viz Kapitola 4.7. Vˇsak vzhledem k faktu, ˇze kinematick´e ˇretˇezce spojuj´ıc´ı koncov´ IKU, y efektor se z´akladnou manipul´ atoru jsou vˇetˇsinou relativnˇe jednoduch´e, lze analyticky ´ PKU ´ pro paraleln´ı manipul´atory vˇsak dodnes z˚ nal´ezt ˇreˇsen´ı IKU. ust´av´a otevˇren´ ym probl´emem. V pr´ aci jsou zahrnuty uk´ azky v´ ypoˇctu pro nˇekter´e varianty manipul´ator˚ u (plan´arn´ı paraleln´ı manipul´ atory, paraleln´ı sf´erick´e z´apˇest´ı a jeho ekvivalence k zn´am´e zjednoduˇsen´e variantˇe paraleln´ıch manipul´ator˚ u). I zde vˇsak nest´avaj´ı nemal´e probl´emy ´ obecn´eho 6DoF paras numerick´ ymi algoritmy. Efektivn´ı algoritmy pro v´ ypoˇcet PKU leln´ıho manipul´ atoru z˚ ust´ avaj´ı tak i nad´ale pˇredmˇetem intenzivn´ıho zkoum´an´ı. Kapitola 4.8 je vˇenov´ ana anal´ yze rychlost´ı a zrychlen´ım mezi kloubov´ ymi a zo´ a zpˇetn´a becnˇen´ ymi souˇradnicemi, tzv. pˇr´ım´ a okamˇzit´a kinematick´a u ´loha (POKU) ´ Moˇznost prost´eho derivov´an´ı rovnic (inverzn´ı) okamˇzit´ a kinematick´ a u ´loha (IOKU). ´ ˇci ´IKU) nen´ı v textu uvaˇzov´ano. Algoritmizace automaticpolohov´ ych z´ avislost´ı (PKU k´eho symbolick´eho derivov´ an´ı je sice moˇzn´a, vˇetˇsinou vˇsak vede na mnohaˇclen´e sloˇzit´e vztahy zcela nevhodn´e pro implementaci do re´aln´ ych algoritm˚ u ˇr´ızen´ı ˇci jako vstupy pro tvorbu dynamick´ ych model˚ u manipul´ator˚ u. M´ısto toho jsou odvozeny z´avislosti rychlost´ı a zrychlen´ıch pomoc´ı geometrie mechaniky manipul´atoru a d´ale pak uk´az´any efektivn´ı rekurzivn´ı algoritmy v´ ypoˇctu rychlost´ı a zrychlen´ı libovoln´eho kloubu manipul´atoru. V kapitole je d´ ale zahrnut struˇcn´ y pˇrehled pojedn´avaj´ıc´ı o singul´arn´ıch poloh´ach koncov´eho efektoru a jejich dopad˚ um na kinematick´e chov´an´ı manipul´atoru. ´ pro s´eriov´e manipuV Kapitole 4.9 je analyzov´ an numerick´ y pˇr´ıstup k ˇreˇsen´ı IKU ´ pro manipul´atory paraleln´ı. l´atory, kter´ y m˚ uˇze b´ yt snadno aplikov´ an i na ˇreˇsen´ı PKU Uveden´ y numerick´ y pˇr´ıstup je modifikac´ı z´akladn´ıho Newtonova algoritmu pro hled´an´ı ˇreˇsen´ı neline´ arn´ıch rovnic. Mezi v´ yhody numerick´eho pˇr´ıstupu jednoznaˇcnˇe patˇr´ı jeho snadn´a implementace do re´ aln´ ych algoritm˚ u ˇr´ızen´ı a moˇznost odstranˇen´ı numerick´ ych chyb. Bohuˇzel, i zde existuje nebezpeˇc´ı, ˇze numerick´ y algoritmus bude konvergovat k ji´ neˇz je poˇzadov´ n´emu ˇreˇsen´ı IKU, ano, z d˚ uvodu existence v´ıce takov´ ych ˇreˇsen´ı v bl´ızkosti aktu´aln´ı polohy koncov´eho efektoru. Pomoc v takov´ ych pˇr´ıpadech nab´ız´ı napˇr. metody intervalov´e anal´ yzy. V kapitole je d´ ale uk´az´ana moˇznost vyuˇzit´ı redundance manipul´ator˚ u (vˇetˇs´ı poˇcet nez´ avisl´ ych aktu´ ator˚ u neˇz poˇcet DoF koncov´eho efektoru) k v´ ypoˇctu kloubov´ ych souˇradnic zajiˇst’uj´ıc´ıch poˇzadovan´ y pohyb koncov´eho efektoru manipul´atoru a z´aroveˇ n optimalizuj´ıc´ı dan´e sekund´arn´ı krit´erium (vzd´alenost od singul´arn´ıch
218
4.9 Numerick´ y pˇ r´ıstup ˇ reˇ sen´ı kinematiky manipul´ ator˚ u
poloh, udrˇzov´ an´ı hodnot aktu´ator˚ u od sv´ ych limitn´ıch poloh, atd.). Na z´avˇer je zm´ınˇena moˇzn´ a modifikace numerick´ ych algoritm˚ u v´ ypoˇctu pro manipul´atory nach´azej´ıc´ı se v bl´ızkosti singul´ arn´ıch poloh.
219
´ 4. UVOD DO ROBOTIKY
220
5
Z´ avˇ er Tady chyb´ı z´ avˇer skripta.
221
´ ER ˇ 5. ZAV
222
Literatura [1] R. Isermann. On the Design and Control of Mechatronic Systems - A Survey. IEEE Transactions on industrial electronics, 43, 1996. 1, 3, 8 [2] R. Isermann. Modeling and Design Methodology for Mechatronic Systems. IEEE Transactions on Mechatronics, 1, 1996. 1, 8 [3] R. Isermann. Mechatronic systems - A challenge for control engineering. Proceedings of the American Control Conference, Albuquerque, New Mexico, 1997. 1, 3, 8, 11 [4] N. Kyura and H. Oho. Mechatronics - An Industrial Perspective. IEEE Transactions on Mechatronics, 1, 1996. 1, 3 [5] B. A. Gregory. An Introduction to Electrical Instrumentation and Measurment Systems. MacMillan, New York, 1981. 8 [6] A. S. Morris. Principles of Measurment and Instrumentation. Prentice Hall, London, 1988. 8 [7] J. G. Webster. The Measurement, Instrumentation and Sensors Handbook. CRC Press, 1999. 8 [8] D. G. Luenberger. An Introduction to Observers. IEEE Transactions on Automatic Control, 6, 1971. 11 [9] R. E. Kalman. New methods nad results in linear prediction and filtering theory. Research institute for Advanced Studies, Baltimore, 1961. 11 [10] B. D. O. Anderson and J. B. Moore. Optimal Control Linear Quadratic Methods. Prentice Hall, 2007. 11
223
LITERATURA
[11] K. J. ˚ Astr¨ om. Feedback systems. Princeton University Press, 2009. 11 [12] K. J. ˚ Astr¨ om. Introduction to Stochastic Control Theory. Dover, New York, 2006. 11 ˇek. Elektrick´e motory a pohony. BEN, Praha, 2004. 13 [13] O. Roub´ıc [14] T. Kenjo and S. Nagamori. Brushless Motors - Advanced Theory and Modern Applications. Sogo Electronics Press, 2003. 13 [15] H. A. Toliyat and S. G. Campbell. DSP-based Electromechanical Motion Control. CRC Press, 2004. 13, 15 [16] R. Isermann. Supervision, Fault Detection and Fault Diagnosis Methods. Control Engineering Practice, 1997. 18 [17] R. Siegwart and I. R. Nourbakh. Introduction to Autonomous Mobile Robots. MIT Press, 2004. 18 [18] H. Choset. Principles of Robot Motion: Theory, Algorithms, and Implementations. MIT Press, 2005. 18 [19] T. R. Kurfess. Robotics and Automation Handbook. CRC Press, 2004. 18, 20, 24, 28, 52, 55, 58 [20] R. Kelly et al. Control of Robot Manipulators in Joint Space. Springer, 2005. 18, 20, 24, 52, 55 [21] K. Erkorkmaz. Optimal Trajectory Generation and Precision Tracking Control for Multi-axis Machines. University of British Columbia, 2004. 18, 30, 31, 73, 76, 77, 84, 85, 86 [22] B. Sencer. Smooth Trajectory Generation and Precision Control of 5-Axis CNC Machine Tools. University of British Columbia, 2009. 18, 30, 31, 77, 84 [23] J.E. Bobrow, S. Dubowsky, and J.S. Gibson. Time-optimal control of robotic manipulators along specified paths. International Journal of Robotic Research, 1985. 19, 78, 87, 88
224
LITERATURA
[24] Z. Shiller and H.H. Lu. Computation of path constrained time optimal motions with dynamic singularities. ASME Journal of Dynamic Systems, 114:34–40, 1992. 19, 78, 87, 88 [25] K.G. Shin and N.D. McKay. Minimum-time control of robotic manipulators with geometric constraints. IEEE Transactions on Automatic Control, AC-30(6):531–541, 1985. 19, 78, 88 [26] B. Cao, G.I. Doods, and G.W. Irwin. Time-optimal and smooth constrained path planning for robot manipulators. IEEE International Conference on Robotics and Automation, 1994. 19, 78, 89 [27] S. Macfarlane and S. Croft. Jerk-bounded manipulator trajectory planning: design for real-time applications. IEEE International Conference on Robotics and Automation. 19, 78, 89 [28] O. J. M. Smith. Posicast Control of Damped Oscillatory Systems. Proceedings of the IRE, 45/9:1249–1255, 1957. 19 [29] W. E. Singhose. Command Generation for Flexible Systems, PhD thesis. MIT , Dept. of Mechanical Engineering, 1997. 19, 23 [30] N. C. Singer and W.P. Seering. Preshaping Comman Inputs to Reduce System Vibration. A.I. Memo, 1027:1–23, 1988. 19, 23 [31] J. R. Huey, K.L. Sorensen, and W. E. Singhose. Usefull Applications of Closed-loop Shaping Controllers. Control Engineering Practice, 16:836–846, 2007. 19, 23 [32] S. S. G¨ urley¨ uk and S. Cinal. Robust Three-impulse Sequence Input Shaper Design. Journal of Vibration and Control, 13:1807–1818, 2007. 19, 23 [33] PLCopen. Function blocks for motion control, Version 1.1. PLCopen, Technical Committee 2, 2005. 19, 20, 52, 53, 56 [34] M. Mandal and T. K. Naskar. Introduction of control points in splines for synthesis of optimized cam motion program. Mechanism and Machine Theory, Elsevier, 2008. 20, 34, 40
225
LITERATURA
[35] Qiu et al. A universal optimal approach to cam curve design and its applications. Mechanism and Machine Theory, Elsevier, 2005. 20, 34, 44 [36] A. Iglesias. B-Splines and NURBS curves and surfaces. Lectures in computer-aided geometric design and surfaces, University of Cantabria, 2001. 20, 40 [37] M. Heng. Smooth and time optimal trajectory generation for high-speed machine tools. University of Waterloo, 2008. 20, 31, 34, 40, 62, 73, 77, 84, 86 [38] F. Jeˇ zek. Geometrick´e a poˇc´ıtaˇcov´e modelov´ an´ı. Z´apadoˇcesk´a univerzita v Plzni, 2006. 20, 44, 62 [39] P. Smid. CNC Programming Handbook. Industrial Press, Inc., New York, 2008. 20, 29, 52 [40] P. Smid. CNC Programming Techniques. Industrial Press, Inc., New York, 2005. 20, 29, 52 [41] J. N. Pires. Industrial Robots Programming. Springer, 2007. 20, 24, 52, 55 [42] B. Siciliano and K. P. Valavanis. Control Problems in Robotics and Automation. Springer, 1998. 20, 24, 52, 55 [43] G. Williams. CNC Robotics. McGraw-Hill, 2003. 20, 29, 52 [44] P. Keller. Programov´ an´ı a ˇr´ızen´ı CNC stroj˚ u. Technick´a univerzita v Liberci, Fakulta strojn´ı, 2005. 20, 29, 30, 52 [45] S.J. Kwon and W.K. Chung. Perturbation Compensator based Robust Tracking Control and State Estimation of Mechanical Systems. Springer, 2004. 22, 27, 30 [46] K. Ohnishi, M. Shibata, and T. Murakami. Motion control for advanced mechatronics. IEEE Transaction on mechatronics, 1996. 22, 23, 27, 30, 58 [47] K.K. Bong and Ch.K. Wan. Motion control of precision positioning systems using adaptive compensation. Proceedings of the American Control Conference, Anchorage, 2002. 22, 27
226
LITERATURA
[48] Y. Altintas, K. Erkorkmaz, and W.-H. Zhu. Sliding mode controller design for high-speed drives. Annals of the CIRP. 22 [49] S.-J. Huang and Ch.-F. Hu. Predictive fuzzy controller for robotic motion control. Industry applications conference IAS’95, 1995. 22 [50] J.B. Mbede, X. Huang, and M. Wang. Robust neuro-fuzzy sensor-based motion control among dynamic obstacles for robot manipulators. IEEE Transactions on Fuzzy Systems, 11, 2003. 22 [51] B. Armstrong-Helouvry, P. Dupont, and C.C. De Wit. A Survey of Models, Analysis Tools and Compensation Methods for the Control of Machines with Friction. Automatica, 30, 1994. 22 [52] M. Goubej and R. Skarda. Input Shaping Filters for the Control of Electrical Drive with Flexible Load. AT & P Journal PLUS, 2:116–121. 23 [53] M. Goubej and M. Schlegel. Feature-based parametrization of input shaping filters with time delays. IFAC Workshop on time delay systems, Praha, 2010. 23 [54] K. Tammi. Active control of radial rotor vibrations: Identification, feedback, feedforward and repetitive control methods. VTT Technical Research Centre of Finland, 2004. 23 [55] K. Sugiura and Y. Hori. Vibration Suppresion in 2- and 3-Mass system based on the feedback of imperfect derivative of the estimated torsional torque. IEEE Transactions on Industrial Electronics, 43, 1996. 23 [56] S.N. Vukosavic. Digital Control of Electrical Drives. Springer, 2007. 23 [57] L. Sciavicco and B. Siciliano. Modelling and Control of Robot Manipulators. Springer, 1999. 24, 28, 54, 55 [58] L. Piegl and W. Tiller. The NURBS Book. Springer, 1997. 30, 62, 65, 67, 69 [59] L.S. Pontryagin, V. G. Boltyanskii, R. V. Gamkrelidze, and E. F. Mischenko. The mathematical theory of optimal processes. Interscience Publishers, New York, 1962. 35
227
LITERATURA
´ ha. Optimal control of chain of [60] M. Schlegel, J. Moˇ sna, and L. Bla integrators with constraints. Proceedings of 17th International Conference on Process Control 2009, 2009. 36, 37 ´ ger. N´ [61] A. Ja avrh gener´ atoru ˇcasovˇe optim´ aln´ı trajektorie pro ˇretˇezec tˇr´ı integr´ ator˚ u s intervalov´ym omezen´ım na ˇr´ızen´ı a souˇradnice stavu. Z´apadoˇcesk´a univerzita v Plzni, 2009. 36 [62] B. Buchberger. Gr¨ obner Bases - An algorithmic method in polynomial ideal theory. Recent trends in multidimensional systems theory, Reidel Publishing Company, 1986. 37 ˇ e ˇtina, P. Balda, R. Piˇ [63] M. St sl, M. Goubej, and M. Schlegel. Motion control algorithms in the new version of the REX control system. Proceedings of 9th International Scientific Conference Process Controll 2010, 2010. 37 [64] M. Goubej. Vyuˇ zit´ı polynomi´ aln´ıch interpolac´ı pro generov´ an´ı trajektorie v syst´ emech ˇ r´ızen´ı pohybu. V´yzkumn´ a zpr´ ava k projektu VaV MPO, Z´ apadoˇcesk´ a univerzita v Plzni, 2010. 40, 50, 51 [65] M. Goubej. Polynomial Interpolation Methods for Synthesis of Electronic Cam Profiles. Sborn´ık konference SVK 2010, Z´ apadoˇcesk´ a univerzita v Plzni. 40 [66] A. Albu-Sch¨ affer and G. Hirzinger. Cartesian Impedance Control Techniques for Torque Controlled Light-Weight Robots. IEEE International Conference on Robotics and Automation, 2002. 58 [67] N. Hogan. Impedance Control: An approach to manipulation. ASME Journal on Dynamic Systems, Measurement and Control, 107, 1985. 58 [68] R.T. Farouki and Y.F. Tsai. Exact Taylor series coefficients for variable CNC curve interpolators. Computer Aided Design, 33, 2001. 72 [69] M.Z. Lin, M.-S. Tsai, and H.-T. Yau. Development of a dynamics-based NURBS interpolator with real-time look-ahead algorithm. International Journal of Machine Tools nad Manufacture, 47, 2007. 72
228
LITERATURA
[70] L. Zhang, K. Wang, Y. Bian, and H. Chen. A Real-Time NURBS Interpolator with Feed Rate Adjustment. International Conference on Intelligent Computing, 2008. 72 [71] W.T. Lei, M.P. Sung, L.Y. Lin, and J.J. Huang. Fast real-time NURBS path interpolation for CNC machine tools. International Journal of Machine Tools and Manufacture, 2007. 72, 73, 74 [72] D. Constantinescu. Smooth time optimal trajectory planning for industrial manipulators. University of British Columbia, 1998. 78, 89 [73] M. Weck, A. Meylahn, and C. Hardebusch. Innovative Algorithms for Spline-Based CNC Controller. Annals of the German Academic Society for Production Engineering, 1999. 80 [74] J. N. Nocedal and S. J. Wright. Numerical Optimization. Prentice Hall, 1999. 85 [75] Boston Dynamic. http://www.bostondynamics.com/. 92 [76] Ilian Bonev. The True Origins of Parallel Robots. www.parallemic.org, 2003. 97 [77] Atega s.r.o. http://www.atega.cz/. 103 [78] Eurotec JKR s.r.o. http://www.eurotec-jkr.cz. 105 [79] J-P. Merlet. Kinematics’ not dead! Proceedings of the 2000 IEEE, International Conference on Robotics and Automation, 2000. 106 [80] B. Siciliano L. Sciavicco. Modelling and Control of Robot Manipulators. Springer, 2 edition, 2000. 111, 128, 187, 198 [81] E Dombre W. Khalil.
Modeling, Identification and Control of Robots.
Butterworth-Heinemann, 2004. 111, 128, 139, 140, 152, 198 [82] Centre of Computer Graphics and UWB Pilsen Data Visualisation.
Analytick´ a
geometrie
pro
poˇ c´ıtaˇ covou
grafiku
http://herakles.zcu.cz/education/ZPG/cviceni.php?no=5. 112
229
II,
LITERATURA
[83] J. Denavit and R. S. Hartenberg. A Kinematic Notation for LowerPair Mechanisms Based on Matrices.
J. Appl. Mechanics, June 1955,
22:215–221, 1955. 116 [84] W. Khalil and J. Kleinfinger. A new geometric notation for open and closed-loop robots. 3, pages 1174 – 1179, apr. 1986. 116 ˇ [85] Martin Svejda. Kinematick´ a anal´ yza antropomorfn´ıho manipul´ atoru se ˇ Plzeˇ sf´ erick´ ym z´ apˇ est´ım. Technical report, katedra kybernetiky, FAV, ZCU n, 2010. 116 [86] J.F. Kleinfinger W. Khalil. A New Geometric Notation for Open and Closed-Loop Robots. Robotics and Automation. Proceedings. IEEE International Conference, 1986. 120 [87] Paul R.C.P. Robot manipulators: mathematics, programming and control. MIT Press, Cambridge, USA, 1981. 138 [88] W. Khalil and F. Bennis. Automatic generation of the inverse geometric model of robots. Robotics and Autonomous Systems, 7(1):47 – 56, 1991. 140 [89] M. Raghavan and B. Roth. Inverse Kinematics of the General 6R Manipulator and Related Linkages. Journal of Mechanical Design, 115(3):502– 508, 1993. 144 [90] S. Rabinowitz. A Useful Trigonometric Substitution. Digital Equipment Corporation, Nashua, NH, 1986. 144 [91] D. Murareci W. Khalil. On the General Solution of the Inverse Kinematics of Six-Degrees-of-Freedom Manipulators. Advances in Robot Kinematics and Computational Geometry, 115:309–318, 1994. 146 [92] D. Manocha and J.F. Canny. Efficient inverse kinematics for general 6R manipulators. Robotics and Automation, IEEE Transactions on, 10(5):648 –657, October 1994. 146, 182
230
LITERATURA
´ ha. Groebnerova b´ [93] L. Bla aze a teorie ˇ r´ızen´ı. Pr´ace ke st´atn´ı doktorsk´e ˇ v Plzni, 2011. 147, 185 zkouace, Katedra kybernetiky, FAV, ZCU ,
[94] Yan Wang, Lu-bin Hang, and Ting-li Yang. Inverse Kinematics Analysis of General 6R Serial Robot Mechanism Based on Groebner Base. Frontiers of Mechanical Engineering in China, 1:115–124, 2006. 10.1007/s11465005-0022-7. 147 c [95] M. vejda. Overview of parallel architectures for gearing robot. Techˇ Plzeˇ nical report, katedra kybernetiky, FAV, ZCU n, 2009. 147, 158 [96] F. Bennis. Contribution ´ a la mod´elisation g´eom´etrique et dynamique des robots ´ a chaine simple et complexe. PhD thesis, E.N.S.M., Nantes, France, 1991. 152 [97] F. Bennis. Modele g´ eom´ etrique inverse des robots a chaine d´ ecouplable : application aux ´ equations de contraintes des boucles ferm´ ees. Trans. of the Canadian Society for Mechanical Engineering, 17:473–492, 1993. 152 [98] J. P. Merlet. Parallel robots. Springer, 2006. 155, 164, 185 [99] http://www.mathpages.com/home/kmath544/kmath544.htm. The Resultant and Bezout’s Theorem. 160 [100] W. Fulton. Algebraic Curves: An Introduction To Algebraic Geometry. New York: Benjamin, 1969. 160 [101] J. Angeles. Kinematics Synthesis. lecture notes, Department of Mechanical Engineering, McGill University, Montreal (Quebec), Canada, 2009. 164 [102] J.-P. Merlet. Direct kinematics of planar parallel manipulators. In Robotics and Automation, 1996. Proceedings., 1996 IEEE International Conference on, 4, pages 3744 –3749 vol.4, April 1996. 171 [103] K. H. Hunt. Structural kinematics of in parallel actuated robot arms. J. of Mechanisms, Transmissions and Automation in Design, pages 705–712, 1983. 174
231
LITERATURA
[104] D. O’Shea D. Cox, J. Little. Ideals, Varieties, and Algorithms: An Introduction to Computational Algebraic Geometry and Commutative Algebra. Springer, 2nd edition, 2006. 185 [105] J-P. Merlet. Interval Analysis and Reliability in Robotics. International Journal of Reliability and Safety, 3(1-3):104–130, 2009. 185 [106] P. Nanua and K.J. Waldron. Direct kinematic solution of a Stewart platform. In Robotics and Automation, 1989. Proceedings., 1989 IEEE International Conference on, pages 431 –437 vol.1, May 1989. 185 [107] J.-P. Merlet. Direct kinematics and assembly modes of parallel manipulators. Int. J. Rob. Res., 11:150–162, April 1992. 185 [108] Jean-Pierre Merlet. Manipulateurs paralleles, 4eme partie : mode d’assemblage et cinematique directe sous forme polynomiale. Research Report RR-1135, INRIA, 1990. 185 [109] Anthony A. Maciejewski and Charles A. Klein. Numerical filtering for the operation of robotic manipulators through kinematically singular configurations. Journal of Robotic Systems, 5(6):527–552, 1988. 215 ´ˇ [110] M. Vala sek J. B¨ ohm, K. Belda. The Direct Kinematics for Path Control of Redundant Parallel Robots. Advances in Systems Science: Measurement, Circuits and Control, pages 253–258, 2001. 216 [111] Denny Oetomo and Marcelo H. Ang Jr. Singularity robust algorithm in serial manipulators. Robot. Comput.-Integr. Manuf., 25:122–134, February 2009. 216 [112] Bruno Siciliano.
Kinematic control of redundant robot manipula-
tors: A tutorial. Journal of Intelligent and Robotic Systems, 3:201–212, 1990. 10.1007/BF00126069. 216 [113] Luis Gracia and Josep Tornero. Tracking trajectories with a robotic manipulator with singularities. In Proceedings of the 2nd international conference on Advances in brain, vision and artificial intelligence, BVAI’07, pages 595–605, Berlin, Heidelberg, 2007. Springer-Verlag. 216
232
LITERATURA
[114] Tenreiro Machado J.A. Fonseca Ferreira N.M. RobLib: an educational program for robotics. IFAC symposium on robot control, 1(163-168), 2000. 216 [115] P. I. Corke. A robotic toolbox for MATLAB. IEEE Robot. Automat. Mag., (24 - 33), 1996. 216 [116] Wisama Khalil and Denis Creusot. SYMORO+: A system for the symbolic modelling of robots. Robotica, 15:153–161, March 1997. 216
233