Sterowanie Mechanizmów Wieloczłonowych
Raport
Tomasz Piesio, gr. ROB 8 listopada 2018
Temat nr 11: Opis modelu robota Fanuc M1iA - parametry modelu, model kinematyki i dynamiki
Spis Treści
1 Parametry modelu robota Fanuc M-1iA . . . 2
2 Opis kinematyki . . . 4
2.1 Kinematyka pozycji . . . 5
2.2 Kinematyka prędkości . . . 6
2.3 Kinematyka przyspieszeń . . . 7
3 Opis dynamiki . . . 9
Raport sporządzony na podstawie artykułu ’A Closed Form Inverse Dynamics Model of the Delta Parallel Robot’ autorstwa Philippe Guglielmetti’ego and Roland’a Longchamp’a pracujących w Institut d’ Automatique na uniwersytecie Ecole Polytechnique Federale de Lausanne (EPFL)
1 Parametry modelu robota Fanuc M-1iA
Opis robota M-1iA wg. strony producenta:
Ten 3-osiowy robot delta, stanowiący idealną alternatywę dla skomplikowanych i dro- gich urządzeń automatycznych, ma szeroki zakres pracy, dzięki czemu idealnie nadaje się do operacji przenoszenia na linii produkcyjnej niewielkich ciężarów. Skonstruowany tak, aby zajmował jak najmniej miejsca, działa według ścisłych parametrów, dzięki czemu ob- sługa linii może bezpiecznie pracować obok niego.
Rys. 1. Lekki robot delta Fanuc M-1iA1
Specyfikacja robota:
• liczba indywidualnie sterowanych osi: 3,
• powtarzalność: ± 0.02 mm,
• maksymalne obciążenie: 1 kg,
• masa (z kołnierzem mocującym): 18 kg,
• przestrzeń robocza: φ 280 x 100 mm,
• montaż: podłogowy lub sufitowy,
• teach pendant: iPendant Touch,
1 https://www.fanuc.eu/pl/pl/roboty/robot-strona-filtrowania/m1-series/m-1ia-1h
• zasilanie: 220 ∼ 230V AC,
• średni pobór mocy: 0.2 kW,
• wyprowadzenie sygnałów dla narzędzia: 6 I/O, złącze pneumatyczne,
• poziom hałasu: 61.2 dB,
• zakres temperatur: 0 ∼ 45◦C,
• stopień szczelności: IP20.
Rys. 2. Przestrzeń robocza robota Fanuc M-1iA2
2 https://www.fanuc.eu/media/files/pdf/products/robots/robots-datasheets-en/m- 1ia/datasheet%20m-1ia-1h.pdf?la=pl
2 Opis kinematyki
Roboty równoległe oparte są na konstrukcjach mechanicznych, gdzie więcej niż jeden łańcuch kinematyczny łączy podstawę robota do jej efektora końcowego. Te pod-łańcuchy kinematyczne tworzą zamknięty łańcuch kinematyczny. Zaletą zastosowania równoległych struktur kinematycznych względem łańcuchów szeregowych jest uzyskanie wyższej sztyw- ności oraz niższej bezwładności, co skutkuje możliwością osiągnięcia wyższej dynamiki przy zachowaniu wysokiej dokładności osiągania zadanych pozycji. Główną wadą robo- tów równoległych jest natomiast ograniczona przestrzeń robocza oraz niskie maksymalne obciążenie efektora.
Rys. 3. Struktura manipulatora typu Delta
Manipulator typu Delta 3 dof (ang. degrees of freedom - stopnie swobody) został zaprojektowany przez Reymond’a Claveld’a, obecnego dyrektora Laboratoire de Syst`emes Robotiques 2 na uniwersytecie ´Ecole Polytechnique F´ed´erale de Lausanne (EPFL), do wykonywania szybkich aplikacji typu pick & place. Jego struktura należy do drugiej klasy Merleta, ponieważ każdy pod-łańcuch składa się z dwóch członów. Pierwszy połączony jest z bazą przez ruchomy przegub obrotowy. Drugi człon tworzą równoległe pręty, które gwarantują, że platforma i podstawa pozostają koplanarne. Skutkiem takiej konstrukcji jest to, że platforma porusza się w przestrzeni kartezjańskiej, bez dodanych obrotów.
Typowa platforma Delta może osiągnąć przyspieszenia 300sm2 i prędkość maksymalną 10 ms. Obecnie roboty przemysłowe wykorzystujące tę konstrukcję są najszybszymi robo- tami stosowanymi w przemyśle. Biorąc pod uwagę dynamikę, są to także pierwsze roboty, w których efekty Coriolisa i przyspieszenia dośrodkowego mogą stać się dominujące, co oznacza, że większość momentu obrotowego generowanego przez silniki jest wykorzysty- wana do kompensowania tych efektów.
2.1 Kinematyka pozycji
Zdefiniujmy q jako wektor pozycji w układzie współrzędnych członu oraz wektor p jako wektor pozycji w kartezjańskiej przestrzeni roboczej. Definicje parametrów geome- trycznych konstrukcji zostały zaznaczone na rys. 4.
Rys. 4. Opis geometrii manipulatora typu Delta
Do opisu każdego z pod-łańcuchów kinematycznych wykorzystane są kartezjańskie układy współrzędnych {O, xi, yi, z}. Układy te są otrzymane przez rotacje kąta ϕi wokół osi z, gdzie ϕ = [0◦, 120◦, 240◦]. Zatem
pi =
xi yi zi
= Φi· p, gdzie Φi =
cos ϕi sin ϕi 0
− sin ϕi cos ϕi 0
0 0 1
= Φi· p (1)
opisuje kartezjańską pozycję platformy w odpowiadającym mu układzie współrzędnych, a
ei =
R 0 0
+ Laαi (2)
opisuje pozycję i-tego członu w układzie współrzędnych {O, xi, z} dla R = ra− rb oraz
αi =
cos ϕi 0 sin ϕi
(3)
opisującego jednostkowy wektor kierunku i-tego ramienia. Jednostkowy wektor kierunku opisujący odpowiadający drugi człon łańcucha to
βi = 1 Lb
(pi− ei) (4)
Stad wynika, że
pi = ei+ Lbβi (5)
2.2 Kinematyka prędkości
Różniczkując (5) uzyskamy ˙pi = Laα¯iq˙i+ Lbβ˙i , gdzie
¯
αi = ∂αi
∂qi = αi =
− sin qi 0 cos qi
(6)
Po wykonaniu przekształceń, równanie otrzymane dla i = 1, 2, 3 może zostać zapisane w formie
M · ˙p = LaV · ˙q (7)
gdzie
M =
β1T · φ1 β2T · φ2 β3T · φ3
=hφ−11 · β1 φ−21 · β2 φ−31 · β3iT (8)
a V jest macierzą diagonalną
V =
β1T · ¯α1 0 0 0 β2T · ¯α2 0 0 0 β3T · ¯α3
(9) .
Wówczas odwrotny Jakobian przyjmie postać J−1 = L1
aV−1 · M . Łatwiej jest uzy- skać odwrotny Jakobian J−1(p) = ∂p∂ F−1(p) dla robota równoległego, niż prosty Jakobian J , który zwykle może zostać uzyskany wyłącznie przez odwrócenie J−1. Łatwo zauwa- żyć, ze analityczna metoda otrzymania Jakobianu J = LaM−1· V będzie zbyt złożona obliczeniowo dla praktycznego jej wykorzystania.
2.3 Kinematyka przyspieszeń
Różniczkowanie (7) daje relację pomiędzy przyspieszeniami w układzie członów a układem roboczym
M · ¨p = LaV · ¨q − ˙M · ˙p + LaV · ˙˙ q (10)
M ˙˙p =
β1T · φ1 β2T · φ2 β3T · φ3
˙
p (11)
Ponieważ
β˙i = 1
Lb( ˙pi− ˙ei) = 1
Lb( ˙pi− Laα¯iq˙i), (12)
M ˙˙p = 1
Lb( ˙pT · ˙p)
1 1 1
− La
Lbq ˜˙P ˙p (13)
gdzie
P =˜
¯ αT1 · φ1
¯ αT2 · φ2
¯ αT3 · φ3
(14)
Dla spojności zdefiniujmy także
P =
αT1 · φ1 αT2 · φ2 αT3 · φ3
(15)
które zostanie wykorzystane w liczeniu modelu dynamiki. Ostatnie wyrażenie (10) obej-
muje
V ˙˙q = ˜V · ˙q − ˙q[V · ˙¯˜ q] (16) gdzie
V =˜
β˙1T · ¯α1 0 0 0 β˙2T · ¯α2 0 0 0 β˙3T · ¯α3
(17)
i
¯˜ V = −
β˙1T · α1 0 0 0 β˙2T · α2 0 0 0 β˙3T · α3
(18)
Wstawiając (13) i (16) do (10) otrzymamy odwrotny model kinematyki przyspieszeń
¨
q = V−1[ 1
LaM · ¨p + 1
LaLbp˙2− ¯V · ˙q − 1
Lbq[ ˜˙P · ˙p] + ˙q[V · ˙¯˜ q]] (19)
3 Opis dynamiki
Dynamika jest kluczem do kontroli robotów typu Delta ze względu na szybkość ruchu i osiągane przyspieszenia, czego konsekwencją jest rozwinięcie wielu podejść do opisu tego zagadnienia. Dayer zastosował metodę Newtona-Eulera i uzyskał układ równań wymiaru 21, który musiał być rozwiązany w każdym okresie próbkowania. Model ten wykorzysty- wał współrzędne uogólnione robota, a więc bazował na prostym rozwiązaniu kinematyki układu. Codourey zaproponował pierwszy model, który można było wykorzystać do kon- troli manipulatora w czasie rzeczywistym. Równoległe pręty łańcucha kinematycznego zostały zamodelowane przez dwie punktowe masy na ramieniu i na platformie, dlatego ich moment bezwładności został zaniedbany. Miller usunął to założenie i rozwiązał pro- blem za pomocą mnożników Lagrange’a. Te dwa modele wymagały licznych różniczkowań numerycznych do wyliczenia relacji kinematyki prędkości i przyspieszeń. W pracy Gugliel- metti’ego użyto tych relacji kinematycznych, aby napisać uproszczony model Codourey’a i zaproponować schemat kontroli w przestrzeni roboczej. W prezentowanym poniżej mo- delu usunięto założenie o bezwładności przedramion i opisano kompletny model, który można wykorzystać do kompleksowej analizy dynamiki robota.
Podstawowym założeniem metody jest oddzielenie przedramienia od reszty konstruk- cji. Ponieważ przedramię składa się z dwóch identycznych prętów, które zawsze pozostają równolegle, modelujemy każde przedramię jako pojedynczy pręt o masie mb i momencie bezwładności jb względem osi obrotu przechodzącej przez punkt łączenia członów. Po- zycja przedramienia w układzie współrzędnych {O, Xi, Yi, z} jest określona przez punkt końcowy członu pi i punkt przegubu ei. Przyspieszenie uzyskano przez zróżniczkowanie równ. (2) dwukrotnie: ¨ei = La( ˜αiq¨i− αiq˙i2). Ponieważ przyjmujemy, że pręt jest doskonałe sztywny, przyspieszenie punktu pi może być zapisane w formie ¨pi = ¨ei+ ¨si+ ¨ri, gdzie ¨si to przyspieszenie dośrodkowe obrotu pręta wokół przegubu ei, a ri to przyspieszenie styczne związane z przyspieszeniem kątowym pręta w okolicy przegubu. ¨ei można postrzegać jako przyspieszenie liniowe pręta. Siła, która powinna zostać przyłożona do platformy w celu uzyskania wymaganego przyspieszenia pręta jest opisana przez
di = mb
2 [¨ei+ Φig] +¨ jb
Lbr¨i+ mb· ¨si (20) Mamy
¨
si = βi· βiT[¨pi− ¨ei] (21) oraz
¨
ri = ¨pi− ¨ei− ¨si = [I − βi· βiT] · [¨pi− ¨ei], (22) stąd
di = mb
2 Φig + [¨ jb
LbI − (jb
Lb − mb)βi· βiT] · ¨pi+ [(jb
Lb − mb)βi· βiT + (mb 2 − jb
Lb)I] · ¨ei (23) Co więcej aby przyspieszyć ruch platformy, siła przyłożona do niej powinna wynosić mn· (¨p + ¨g). Dlatego też całkowita siła, która musi zostać przyłożona do platformy wynosi
f = ma[¨p + ¨g] +
3
X
i=1
Φ−1i · di (24)
Siła f jest przekazana z silników na platformę poprzez 3 siły o module hi przenoszone przez każde przedramię. Wektor h otrzymuje się przez rozwiązanie liniowego układu rów- nań MTh = f . Ponieważ MT jest regularną macierzą z wyjątkiem pojedynczych pozycji robota, które są wykluczone z obszaru roboczego, możemy zapisać h = M−T · f .
Dla każdego pod-łańcucha ja jest bezwładnością ramienia, a ma masą poddawaną działaniu grawitacji. Biorąc pod uwagę środek ciężkości ramienia znajdujący się dokładnie w środku jego długości, momenty obrotowe są podane przez
χi
τi ψi
=
0 ja
0
· ¨qi+ [mb+ ma
2 Φi¨g + hiβi+mb
2 e¨i] × Laαi (25)
gdzie τijest momentem obrotowym silnika, a χi i ψisą momentami obciążającymi łożyska.
Możemy teraz napisać odwrotny model dynamiki w postaci macierzowej
τ = (ja+ mb
2 L2a)I ¨q + JTf + La(mb+ ma)
2 α¯Ti · Φi· ¨g (26) Zauważmy, że składnik JTf odpowiada wcześniejszej obserwacji, że siły przyłożone do efektora końcowego są przekształcane na momenty silnika z wykorzystaniem transpo- nowanego Jakobianu. Dotyczy to zarówno równoległych, jak i szeregowych robotów.
Rozpisanie wszystkich składników z równ. (26) i pogrupowanie ich jako zależności prędkości i przyspieszenia prowadzi do
τ = Aqq + J¨ TApp + (C¨ q+ JTCp) ˙q2+ (H + mnJT)¨g (27) gdzie
Aq= (ja+mb
2 L2a)I + L2a(jb
Lb − mb)[VT · V ] + La(mb 2 − jb
Lb) ˜P (28)
Ap = (ma+ 3jb
Lb)I + 3(jb
Lb − mb)[MT · M ] (29)
Cq= La(jb
Lb − mb)I, Cp = La(jb Lb −mb
2 )P (30)
H = La(mb + ma) 2
P +˜ 3mb
2 I (31)
Równanie (27) nazywamy ”modelem odwrotnej dynamiki w dwóch przestrzeniach”, ponieważ korzysta ze stanu robota zarówno w układzie współrzędnych członów jak i ukła- dzie kartezjańskim przestrzeni roboczej. Macierz kwadratowa Aq jest nazywana ”macie- rzą bezwładności przestrzeni członów”, a Ap ”macierzą bezwładności przestrzeni robo- czej”, według analogii do powszechnej formy opisu dynamiki odwrotnej modelu robota szeregowego. Można zauważyć, że sformułowanie zwykłego opisu w przestrzeni członów τ = A(q)¨q + H(q, ˙q) byłoby znacznie bardziej skomplikowane w przypadku manipulatora typu Delta, a dla ogólnego robota równoległego nie istnieje, ponieważ rozwiązanie prostej kinematyki takich struktur nie uwzględnia. Jednak podstawienie odwrotnej kinematyki dla określenia relacji pozycji, prędkości (7) i przyspieszenia (10) do (26) prowadziłoby do sformułowania odwrotnej dynamiki w czystej przestrzeni roboczej τ = A(p)¨p + H(p, ˙p), która byłaby bardzo interesująca do celów analizy. Z bardziej praktycznego punktu widze- nia, równ. (27) jest przydatne do symulacji, ponieważ każda pojedyncza siła, działająca na strukturę robota, może zostać wyizolowana. Konieczności odwrócenia odwrotnego Ja- kobianu, aby uzyskać macierz JT, oraz mnożenia macierzy z Jakobianem można jednak uniknąć w implementacji w czasie rzeczywistym. Równanie (27) zostanie zapisane jako
τ = Aqq + H ¨¨ g + Cqq˙2+ σ (32)
J−Tσ = App + C¨ pq˙2+ mag¨ (33) σ jest uzyskiwana przez rozwiązywanie układu równań liniowych (33), których roz- wiązanie jest znacznie bardziej wydajne obliczeniowo.
