Algorytm generacji trajektorii zadanej ze zdefiniowaną kinetyką dla manipulatora IRb-6

ZESZYTY NAUKOWE POLITECHNIKI SLASKIEJ

Seria: AU TOM ATY KA 2 . n o

_________1132

Nr kol. 1176 Tadeusz Szkodny

Andrzej Fortuna Instytut Automatyki Poli te chn ik a Śląska

ALGORYTM GENERACJI TRAJEKTORII ZADANEJ ZE ZDEFINIOWANĄ KINEMATYKĄ DLA MAN IP ULATORA IRb-6

ALGORITHM OF GEN ER AT ION REQUIRED TRA JECTORIES WITH DEFINED KINEMATICS FOR IRb-6 M AN IPU LA TO R

AJirOPHTM TEHEP HP OBA HH H TP EEY EM OH TPAEKT OPM H C OilPEAEJIEHHblMM KHHEMATHHECHHMM CBOHCTBAMH RJIH MA HM nY H H TO P A IRb-6.

Streszczenie: W pracy pr zedstawiono opis oryginalnego algorytmu ko mp ute ro weg o P L A N 2 , gen er ującego trajektorie zadane ruchu członu roboczego. Kinema tyk ę tych trajektorii de finiujemy tylko w wybranych punktach, przez które ma prz echodzić chwytak, przy czym punkty te mogą być dowolnie oddalone od siebie. Algorytm ten bazuje na formułach stano wi ący ch an alityczne rozwiązanie zadania odwrotnego kinematyki m a nip ul at ora IRb-6.

S u m m a r y : Or y g in a ll y PLAN2 al go rithm for generation required tra je ctories with defined kinematics in this work is presented.

Ki ne mat ic s of this tra je ctories we must define only in points, over its a task required move. Distance between the points may be any value. This a l go ri th m applied formulas, which are analitical solution of inverse kinematics prob le m of IRb-6 manipulator.

Pe sw H e : B p o 6 o T e n p e n c r a B n e H O o r m c a H s e o p n r n H a m > H o r o a n r o p H T w a PLAN2, K OTo p t i H r e n e p s p y e T T p e 6 y e M b i e T p a e K T o p n n M a H n n y n s T o p a . K n H e n a T n q e c H n e C B O t t c T B a 3 t « x T p a e K T o p n n o n p e n e n e H b i T o n b H O b H 3 6 p a H H « x n y H K T a x , w e p e 3 H O T Opbie BO/PKHO n p O X O B M T b 3 a X Ba T bIBa lomye y c T p O H C T B O . nyHHTbl T p a e H T O p M H M o r y T 6biTb n p o v t 3 B o n b H o o r n a n e H b i n p y r o t n p y r a . A n r o p H T M o c h o b s h Ha

♦ o p M y n a x HsnfliomHXBH p e m e H H e H o C p a T H o R K M H e M a T H H e c K o t l 3 a n a q n a n n Ma HH nynHTopa IRb-6.

1 .Wstęp

Dla upr osz cz e n ia opisu bę dziemy stosować dalej skrót MRP-ma ni pul at or robota przemysłowego. Przestrzeń zewnętrzną MRP opisują jego współrzędne zewnętrzne po ł o że n ia x,y,z i orientacji (kąty Eulera). Ws półrzędne te opisują chwytak w z g l ę d e m wyb ranego układu odniesienia, niezależnie od st ru ktury ki nematycznej MRP. Przestrzeń wewnętrzną MRP opisują jego współr zę dne wewnętrzne. Współrzęd ny mi wewnętrznymi MR P są współrzędne nat u­

ralne członó w i siłownik ów [15]. Wartości zadane współrzędnych naturalnych siłown ik ów są w y ni k i e m uczen ia robota [11] bądź wy n ik iem działania warstwy wy zn acz an ia tra jektorii ruchu [11], ws pó łpracującej z kamerą lub innym se nsorem sceny.

Je dn ym z po dst aw ow y c h pr o b lem ów sterowania w robotach pr zemysłowych jest pro jek to w a ni e alg or yt m ów gen er ujących trajektorie zadane.

W p ra c a ch [4,5,6,10] pro po nu je się algorytmy generacji trajektorii zadanej, bazujące na arbitralnej dyskretyzacji wspó łr zęd ny ch wewnętrznych MRP. Z dy s k ret yz ow any opis prz estrzeni zewnętrznej M R P otrzymuje się ze z d ys kr et yzo wa ne go opisu pr zestrzeni wewnętrznej MRP. Tr ajektorię zadaną w przestr ze ni zewnętrznej aproksymuje się wy korzystując tak zdyskretyzowany opis p rze str ze ni zewnętrznej MRP. Każdemu punktowi dyskretyzacji w p r z e ­ strzeni zewnętrznej, aproks ym ują ce mu trajektorię zadaną, odpowiadają znane

302 Tadeusz Szkodny. Andrzej Fortuna

zdyskretyzowane ws półrzędne wewnętr zn e M R P . Wadami tych al go ry tmó w jest potrzeba duZej p a m i ę c i , konieczność prz es z uk i wa n i a dużych zbiorów stanowiących zd ys kretyzowany opis prz estrzeni wewnętrznej oraz brak możliwości zmniejszania błędu aproksymacji wy ni ka j ą ce g o z arbitralności dyskretyzacji przestrzeni wew nętrznej MRP.

W pracach [3,7,17] pr o p onu je się me to dy iteracy jn e wyznaczania ws półrzędnych w e wn ęt rz nyc h o dp o w ia da ją cyc h ws pół r z ęd n y m zewnęt rz nym punktu trajektorii zadanej. W tym sposobie wyz na cz a n ia w s pó łr zę dny ch wewnętrznych zbliżanie się do punktu tr ajektorii zadanej odbywa się w kol ejnych krokach obliczeń iteracyjnych. Krok dy skretyzacji wsp ół r zę d ny c h we wnę t r zn y ch w ko­

lejnych krokach ob liczeń ite ra cyjnych zalety od błędu współrzędnych zewnętrznych w po p r ze d ni m kroku iteracji. Metody iteracyjne nie wymagają dużych pamięci, gdyż dokonuje się tu obliczeń tylko dla punktów apr ok symujących trajekt or ię zadaną. W tych m e to da ch błąd apr oksymacji tra­

jektorii zadanej może być zmniejszany, ale zwiększa to liczbę kroków obliczeń iteracyjnych. W pracy [3] p r ze ds taw io no zmodyfik owa ną me to dę ite- racyjną generacji pr o s to li ni owe go odcin ka trajektorii zadanej. Modyfikacja ta po lega na a rb itr al ny m p rz y jęc iu rozkładu błędów w przestrzeni zewnętrznej, co redukuje liczbę kroków obliczeń iteracyjnych. Je dn ak tak przyjęty rozkład błędów jest słuszny tylko na krótkich odcinkach. Nie można wy zn aczyć długości grani cz nyc h tych odcinków, gw a r an tu ją cyc h ograniczenie tych błędów dla dowolnej konfiguracji MRP. Wadą metod ite ra cyjnych jest konieczność w ie l okr otn yc h obliczeń iteracyjnych.

Zaletą metod bazujących na ar bitralnej dyskretyz ac ji przestrzeni wew nę trznych i metod ite ra cyjnych jest pr o st o t a obliczeń, po l eg a j ąc a na stosowaniu tylko równań kinematyki prostej MRP. Jednak zaleta ta może b'-ć pułapką dla tych programistów. k.t6.rzy n i€ P r z e w id z ieli węześpjęj.

osobliwości kin ematycznych MRP [151.

Formuły anali ty czn e stanow ią ce rozwią za ni e zadania odwrotnego kinematyki MRP umożliw ia ją pro je k to w an i e alg or ytm ów generacji trajektorii obli cz ają cy ch w spó łrz ęd ne wew nę t rz n e pu nk tów do k ł adn ie leżących na trajektorii zadanej, w jednym kroku z d o kła dn oś cią wy n ik a ją c ą z długości rejestrów komputera. Formuły te wy mus za ją na p r og r am i s ta c h konieczność przew id zen ia a lt e rn aty wn yc h rozwiązań dla stanów oso bl iw y c h MRP. Formuły analityczne stanowiące ro związanie zadania odw ro tn ego kinematyki M RP o 6 stopniach swobody p rz ed st awi on o w pracach [8,12,14]. Formu ły te dla MR P o 5 stopniach swobody opracowano w pracy [9]. Jednak nie p r zyt oc zo no w niej równania więzów członu roboczego [151. Sugeruje to. Ze człon te n roote r e a l i z o w a ć trajektorie zadane s 6 stopniami swobody, co jest, niemożliwe..

Z przeglądu tego wynika, że pr ez ent ow an e w p r ac ac h [3,4,5,6,7,9,10,17]

modele kinematyki nie po zwalają pr oj ek t o wa ć do kła d n yc h i jednocześnie szybt kich algorytmów generacji trajektorii zadanej ze zde fi niowaną kinematyką .dla MRP z liczbą stopni swobody mniej sz ą ni z 6. W sp ó ła u t o r tej p r a c y opra­

cował m ode le knematyki [15] MRP z liczbą stopni swobody mnie js zą niż 6 . V p r a c y [15] ws pół a u to r prz edstawił model e kinematyki prostej i odwrotnej z

Algorytm generacji trajektorii ,,, 303

równaniami więzów dla MRP PR-02, IRb-6 i IRb-60. Modele te przedstawiono w postaci ciągłej i różniczkowej z uwz glę dn ie nie « osobliwości kinematycznych.

W punkcie 2 pr ac y opisano oryginalny algorytm generacji trajektorii zadanych dla manip ul ato ra IRb-6. Algo ry tm ten wyznacza współrzędne wewnętrzne pu nk tów dokładnie le żących na trajektorii zadanej w jednym kro­

ku. Umo żl iwi a także de fi nio wa ni e kinematyki zadanej w postaci współrzędnych zewnętrznych punktó w dowolnie oddalonych od siebie. W punkcie 3 opisano przykład li cz bow y,b ęd ący w y ni ki em dz iałania algorytmu generacji opisanego w 2 punkcie. W punkcie 4 sformułowano wnioski końcowe. Algo ryt m ten został napisany w języku FORTRAN 77 i urucho mi on y na mc.pVAX 3800 w ośrodku ETO przy Polit ech ni ce Śląskiej w Gliwicach.

2 ■G e n e ra c ja _ tra jek to r i i_ zadanych

Formuły stanowiące ro związanie zadania odwrotnego kinematyki MRP są podstawą do pr oje k t ow a ni a al go ryt mó w ge ne rujących przebiegi współrzędnych naturalnych siłowników. Wsp ół rz ę d ne te od powiadają trajektorii zadanej obiektu manipulacji, opisanej w przestrzeni zewnętrznej robota. Algorytmy generujące wsp ół rz ę d ne nat uralne siłowni kó w tworzą warstwę wy zn acz ani a tra­

jektorii ruchu, będącą e le m en tem str uktury funkcjonalnej układu sterowania robotami inteligentnymi [2]. A l go ry tmy te są niezbędnymi środkami pr o g r a ­ mowymi sprzęgającymi pracę warst wy rozpoznawania z warstw ą sterowania napędów 111],

W pracy 116] prz eds ta w i on o komput er owy algorytm PLAN2 generujący tr aje ­ ktorie chwyt ak a m an i pu l a to r a IRb-6. Zadane współr zę dne zewnętrzne punktów, przez które ma przech odz ić ge ne row an a tra jektoria ,będziemy nazywać głównymi punktami podporowymi. Do generacji po tr ze bn y jest opis wstępny trajektorii w postaci warto ści wsp ół r zę d ny c h ze wnę trznych co najmniej dwóch głównych punktów podporowych, dowolnie od da lon yc h od siebie. Algo ryt m PLAN2 generuje dodatkowe punkty pod po row e dla zdefiniowanej lub niezdefiniowanej ki n e ma ­ tyki między kolejnymi głównymi punk ta mi podporowymi.

R y s . 1.Kąty Eulera w układzie wsp ół rz ę dn yc h : a) kartezjańskim, bl cylindrycznym, cl sferycznym

F i g . 1.Euler angles in follow coordinate systems : ai kartesian.

b) cylindrical, c) spherical

304 JŁM e V .s^ .S ^ jŁQdj3X Ł. Ąp d r ? ,e j .. Fo r t un a

W przypadku pojawienia się osobliwości kinematy cz ny ch al gorytm PLAN2 podaje komunikat o tym stanie, podaje wartości ws pół r z ęd n y ch naturalnych członów możliwe do prz yjęcia i pyta użytkownika^ które z podanych wartości przyjąć. Al gorytm ten dla zdefiniowanej kinematyki opisano w pracy [16], a dla niezdefiniowanej kinematyki w pracy [1].

W niniejszej pra cy opisane zostanie działanie algorytmu PLAN2 dla zdefiniowanej kinematyki trajektorii zadanej.

Algorytm ten zawiera cztery podstawo we segmenty, z których obliczenia przekazywane są do 21 se gmentów pomocniczych. Segm ent y podsta wo we to:

a) segment główny, b) segment R0Z1 , c ' segment R 0 Z 2 , d) segment R0Z3.

Dla upr os zc z en ia opisu będziemy dalej stosować nast ęp u ją ce skróty:

GPP-giówny punkt p od po ro w y i D P P - d o da tk ow y punkt podporowy.

Po uruchomieniu al gorytm PLAN2 pyta o par ame tr y 1& i opisujące chwytak (patrz r y s .1 ) . Na stępnie pyta : o liczbę GPP^50, o współrzędne zewnętrzne i czas t kolejnego GPP, czy o ri ent ac ja kolejnego GP P jest zdefi­

niowana. Jeśli orient acj a jest zdefiniowana^ to a l go ryt m pyta, czy ma ją obliczyć. Jeśli tak, to pojawia się p yt a n i e ^ w jakim uk ładzie współrzędnych ma byc obliczona : k a r t e z j a ń s k i m , cy li nd r yc z n ym czy sferycznym. Po zadekla­

rowaniu od powiedniego układu w sp ó łrz ęd ny ch wyz na cz ane są kąty Eulera, opi­

sujące orientację danego GPP. Kąty te ilustruje rys.l. Jeśli orientacja zdefiniowana nie ma być obliczana, pojaw ia się pytanie o kąty Eulera GPP.

Dla nie zdefiniowanej orientacji GPP ust ala ny jest arbit ral nie kąt Kąty 4 i 6 są obliczane ze wsp ół r zę d ny c h zewn ętr zn yc h x,y,z (opisujących

położenie aktualnego GPP).

Dla tak określ on yc h w s p ół rz ęd n yc h zewn ęt rz n yc h x , y , z opisujących kolejny GPP, alg o ry tm wy zn ac za macierz *^S2ad [15], sprawdza czy spełnione jest równanie wi ęzów [15] i o b l ic za w s p ół rz ę dn e na tu ra ln e z formuł stanowiących an a lityczne rozwiązanie zada n ia o d wr ot ne g o kinematyki MRP TRb-6. Na s tępnie al gorytm pyta o układ w s p ó łr zę d ny ch op is uj ąc y kształt o dcinka trajektorii m i ędzy kolejnymi GPP. Dla odci n ka w kształcie linii prostej należy wybrać układ w s p ó ł rz ę dn yc h k a r t e z j a ń s k i , dla odcinka k rzywo l in io we g o - układ cylin d ry cz ny lub sferyczny. Dalej alg or yt m pyta o czas d yskretyzacji AT na ak tu al n ym odcinku trajektorii zadanej. Po zdefiniowaniu kształtów i czasó w dys kr e ty za cj i na w sz y st k i c h odcinkach miedzy kolejnymi GPP ał gorytm pyta o d o p us zc za l ny bład po ło żenia DP i orientacji DF chwytaka. Jeśli wc z eśniej zad ek la r ow an o dowolną orientacje chwytaka, wtedy al gorytm arbit r al ni e przyjmuje DF=360 . Dalej a lgorytm pyt«1 o rodzaj generacji trajektorii, która m czna zad ek l ar ow ać jako: swobodną, zgrubna 1 dokładną. Po za d eklarowaniu zgrubnej lub dokładnej generacji poja w ia się p y ta ni e o dopu s zc za ln y bład orientacji DFK-.

Dla swobodnej generacji alg or y tm w y z n ac z a ws pó łr z ę d n e zewnę tr zn e DPP >

w yn ik aj ą ce z z ad e kl ar ow a ny ch wcześniej p ar am e t r ó w DP, DF i AT,

Algorytm generacji trajektorii .. 305

zapewniających d ek la ro wan y kształt odcink a trajektorii w przestrzeni zewnętrznej. Tak wyznac zo ne DPP ilustruje ry s .2. Jeśli przez 1 oznaczymy długość odcinka trajektorii m ięd zy i-l-szym i i-tym GPP, to długość Al^

odcinka tr ajektorii między kolejnymi DPP można wyr az ić następująco :

2•DF 2•DF 2-DF 2-AT

Alj= min

|4> - * 1 i i-i1

1

e - e

' i I -11

t - 'f

i i - i 1 T -T

1*1 i -tI

Al(d = 0 . 5 -DP)

: ł , $

V r kąty Eulera i-tego

(1)

i i-l-szego gdzie : 4’( , * )_1 , 0 ; , 6 ^ , T

GPP ; Al ,d , Tj_ i T - pa ram e tr y zilustrowane na rys .2. Al(d) jest długością odcink a trajektorii między kolejnymi DPP, zalezną od odległości d zi lustrowanej na r y s .2. d jest odleg łoś ci ą środka odcinka trajektorii Al od prostej łączącej j-l-s zy i j-ty DPP. Z r y s. 2 wynika, Ze im mniejsze d, tym mniejsze Al. Prz y takiej długości odc in ków trajektorii można prz ypu sz­

czać, że ws zystkie pu n k t y trajektorii opisane przez ws półrzędne z zakresów od po wiadających są siednim DPP nie wy jd ą poza " tr ą b ę” o średnicy DP, ut wo ­ rzoną przez kolejne cy l ind ry zilust ro wa ne na r y s .2. Do wyznac zen ia or i en ­ tacji DPP alg ory tm prz yjmuje liniową zmianę z dł ugością 1 wz dłuż traje­

ktorii dwóch kątów Eulera, a trzeci obli cz a z równ an ia wi ęzów [15].

R y s .2.Ode inek tr ajektorii m ię dzy i-l-szym i i-tym GPP. Al-długość o d cin ka mi ę d zy j- l-szym i j-tym DPP. d- odległość środka odcinka o długości Al od prostej łączącej j-l-szy i j-ty DPP.T, ,i T -czasy odp ow iad aj ąc e zi l ust ro wa nym GPP

F i g .2.Fragment of tr aje c t or y betwe en i-l-th and i-th GPP. A1 -length of fra gments tr aj ec tor y betwee n j-l-th and j-th DPP. d- distance middle of fragment trajec tor y (which have length A1 ) to st rai ghtline binding j-l-th and j-th DPP

assign shown GPP

T , and T ( -times

Do obliczania czasu DPP algorytm także prz yjmuje liniową zależność czasu od długości 1. W pr zypadku gdy kolejne GPP różnią się tylko o r i e n ­ tacją, rolę długości 1 prze jm uj e max[ |4“. _j-Gj | , 1 _ j-Sj | , _ j-'*1, I 1 • Wtedy d jest odległ oś cią środka odcink a trajektorii między kolejnymi DPP od

306 Tadeuss Szkodny, Andrzej .Fortuna

środka kuli o średnicy DP, pokrywaj ące go się z GPP. Formuła (1) jest nadal słuszna. Po obliczeniu współrzę dny ch ze wn ętrznych DPP, algo ryt m sprawdza czy ten DPP jest z przestrzeni roboczej. Jeśli sprawdzany DPP wychodzi poza przestrzeń roboczą, nas tępuje kore kc ja jego położenia, orientacji i czasu.

Następnie obliczane są ws półrzędne naturalne człon ów oraz współrzędne naturalne siłowników odpowiad ają ce temu DPP.

Dla niezbyt dużych wymag ań dotycz ąc ych generacji trajektorii parametry DP DF, AT mają stosunkowo duże wartości. Ta ki m wa rto śc io m tych parametrów odpowiadają stosunkowo długie odcinki trajektorii mi ędzy sąsiednimi DPP.

Wtedy, prz y zadeklarowanej swobodnej generacji trajektorii, przypuszczenia o lokalizacji jej wsz ys t ki c h pu nk tów we wsp omnianej "trąbie" mogą być nie­

słuszne. Wynik a to z tego, że t r aje kt or ia reali zow an a jest we współrzędnych wewnętrznych. Dla zapo bie że ni a w y jś ci u tra jektorii po za "trąbę", przy sto­

sunkowo dużych warto śc iac h DP, DF i AT-^ uż yt kow ni k może zadeklarować zgrubną generację trajektorii oraz pa r am e t r DFW. Wt e dy algorytm wyznacza dwie grupy punktów podpo row yc h : dodatkowe punkty pod po ro we zewnętrzne- i dodatkowe punkty podporowe wewnętrzne. Do datkowe pu nkty podporowa zewnętrzne są wy zn acz an e identycznie jak przy swobodnej generacji traje­

ktorii i będziemy je oznaczać jak dotychczas skrót em DPP. Dodatkowe punkty podporowe wewnętrzne wyni kaj ą z podz iał u ws pół r z ęd n yc h naturalnych siłowników (z pr zed z i ał ó w od pow ia da jąc yc h k o le jn ym DPP) i będ zi emy je ozna­

czać skrótem DPPW. Sp osób ob lic z an i a DPPW międz y j-l-s zy m i j-tym DPP ilustruje rys .3. Każdy z za kresów ws p ół r zę d n yc h natura ln yc h siłowników odp ow iadających kolejnym DPP po dz ie l o ny jest na N+l części, gdzie N opisuje formuła (2). W formule tej zakładamy, że min ima ln e prz eł o że n ie kątowe dla błędó w orientacji jest równe |k4(l-ks )| (k4=-128 ,kg=19/32 [13,15] ) :

N = E(2 -X X/| k<(l-k5 )-DFW|) ,

XX = max( le,ik_0«ipl ’ • • ’ le.5k~e .5pl )> <2) gdzie : E - część całk ow ita argumentu, k^ i k 5-p rz eło ze ni a przekładni w czwar ty m i pi ąt ym stopniu swobody man ip u la t o ra IRb-6. Po wyznaczeniu współrzęd ny ch natur al nyc h sił ow n ik ó w p ie rws ze go DPPW na odcin ku trajekto­

rii mi ędzy j-l-szym i j-tym DPP al g or ytm wy z na c za jego współrzędne zewnętrzne i sprawdza czy jest wewn ąt rz cyl ind ra o śr ednicy DP (zilustro­

wanego na r y s . 3). Jeśli ten DPP W jest poza cylindrem, to liczba N zostaje powięks zo na o 2 i powtórnie są wy zn acz an e w sp ółr zę dn e zewnętrzne pierwszego DPPW. Jeśli nowo obl ic zon y p i er w s z y DPP W jest dalej p o za w s po m ni a n ym cylin­

drem, to al gorytm zwiększa liczbę N o 2 i czyni to aż do chwilij kiedy ten DPPW znajdzie się we wnątrz walca. Nas tę pn ie dla tak zmodyfiko wa nej liczby N wyzn ac za ws półrzędne zewnętrzne ko lejnego DPPW. Jeśli sąsiednie DPP różnią się tylko orientacją, wte dy rolę walce (zilustrowanego na r y s . 3) przejmuje kula o średnicy DP i środku po k ry w aj ą c ym się z DPP. A lg o ry t m sprawdza zanu­

rzenie każdego DPPW w przestrzeni roboczej. Jeśli sp ra wd zan y DPPW wychodzi poza przestrzeń roboczą, to na stępuje kore kc ja jego położenia, orientacji i wsp ół rzę dn yc h nat ur al n y ch siłowników. Do obl ic z an i a czasu DPPW algorytm pr zy jmuje liniową zależność od w s pó łr zęd ny ch na t ur a ln y c h siłowników.

Algorytm generacji trajektorii ... 307

j - i - a y DPP

R y s . 3 .Odcinek tr ajektorii m ięd zy j-l -s zym i j-tym DPP, d-odległość DPP W od prostej łączącej sąsiednie DPP, 6 . +6 _ -współrzędne

J lip l&p na turalne si ło wn i k ów odpow ia daj ąc e j-l-szemu DPP,

ws półrzędne natu ra lne siłowników odpowiada ją ce j-temu D P P ; t Tj-czasy j- l- szego i j-tego DPP

F i g .3.Fragment of tr aje ct or y betwe en j-l-th and j-th DPP. d-distance DPP W to strai ght li ne binding j-l-th and j-th DPP.

6 >1k+6tSk-natural coordi na tes of actuators adequate j-th DPP. Tj T j ^ - t i m e s as sign to j-th and j-l-th DPP

R y s .4.Ko n f i g u r a c ja ma ni pu l a to r a IRb-6, przy której efe ktywny promień obrotu c h wy ta ka R,=0.0, jest największy

Z 1 o

Fi g. 4 .Configuration of ma ni pu l a to r IRb-6 for maximal effective radius rotation R 2=^1^6 t as ^

308 T a d eusz Szkodny. Andrze.j Fortuna

Generowanie trajektorii z niezdefiniowaną kinematyką

Wczytanie kątów <p,6,y

E U L E R : Segment obliczający kąty <t>,9,ip Wybór układu współrzędnych, w którym

mają być obliczone kąty ig jp

Wczytanie współrzędnych x,y,z

i T przyporządkowanych GPP

Algorytm generac.ii tra.iekt.Qrii..-,..,.» 309

a

ROZ-2 Segment wyznacza­

nia współrzędnych zewnętrznych i we­

wnętrznych DPP z dokładnością DP i DF, ewentualna ko­

rekta położenia DPP

R 0Z -3 Segment wyznacza­

nia współrzędnych wewnętrznych i zew­

nętrznych DPPW z dokładnością DP i DFW, ewentualna korekta położenia

DPPW

Wydruk obliczonych współrzędnych zew­

nętrznych i wewnę­

trznych wygenerowanej traje­

ktorii

Wczytanie maksy­

malnego dopuszczal­

nego błędu położenia DP

Wczytanie maksy­

malnego dopuszczal­

nego błędu orientacji D F

Wybór kształtu kolej­

nych odcinków traje­

ktorii

Wczytanie czasów dyskretyzacji na ko­

lejnych odcinkach trajektorii

Wybór dokładności generacji

KO N IEC O BLICZEŃ

ROZ-1

Segment obliczający kąty

<p,0,tp dla zadeklarowanej dowolnej orientacji.

Sprawdzenie równania więzów i zanurzenia GPP

w przestrzeni roboczej. ?

TAK NIE

Czy zadeklarowano swo­

bodną dokładność gene­

racji ?

TAK NIE

Wczytanie DFW

R y s . 5. Schemat bloko wy algorytmu P L A N 2 F i g . 5. Błock diagr am of PLAN2 algorithm

Tade us z Sekodny, ńndrze.i Fortuna

Przy zgrubnej generacji trajektorii algor yt m uw z gl ę d ni a w obliczani!

ws półrzędnych naturalnych siłown ik ów tylko para me tr DFW (patrz formułt.

(2)1. Po zad eklarowaniu dokładnej gen eracji tra jektorii każdy z zakresów ws półrzędnych natur al nyc h siłowników (zi lu strowanych na rys. 3) podzielony jest, podobnie jak prz y zgrubnej generacji. Jedyną różnicą jest sposób obli­

czania początkowej liczb y podziału N. Przy dokładnej generacji trajektorii liczba ta w yni ka z pa ra met ró w DP i DFW w na st ępu ją cy sposób :

gdzie : E -część ca łk owi ta argumentu, R 2 jest na jw ię ksz ym ef ek ty wny m pro­

mieni em obrot u chwy tak a p r ze m ie s zc zaj ąc eg o się o DP/2, w przestrzeni zewnętrznej. Jest to długość łuku jaki zakreśla chwyt ak dla d0J=d02=d0J*

d 0 ’=O i dfMm DP / (2 R 2 ), p r zy konfiguracji man ip ul a t or a jak na r y s . 4. Dalsza korekta liczby N oraz gen er acj a DPP W pr z e bi e ga identycznie jak dla opisanej już zgrubnej generacji trajektorii.

E f ek t e m końcowym dzi ał ani a alg or ytm u jest utw or zen ie zbiorów opisują cy ch wsz ystkie w spó łr zę dne we w nę t r zn e i zewnętrz ne manipulatora I R b - 6 .

R y s . 6 ilustruje schemat bl ok owy algo ry tmu PLAN2 dla zdefiniowanej kine­

matyki trajektorii zadanej chwytaka.

3 .Przykład

R y s . 6 ilustruje pr z yk ł ad o w ą pro st o li n io w ą tr aj ek t o ri ę zadaną chwytaka, ogran ic zon ą p u nk te m po czą t k ow y m P i końcowym K.

K

F i g. 6. Req uir ed tra je ct ory X zad

Współr zę dne tych punkt ów są następ uj ąc e : x p=-0.60 m, y p=0.60 i, zp=1.0

def iniujące kinematykę tr ajektorii z rys. 6 : p a r a m et r y chwy ta ka * V ° . l 6 “ • lic z ba GPP=2 (punkty P i K); wsp ół r zę d ne zewnętrz ne GPP równe (3)

R y s . 6. Tr aj ek t or i a z ada na X

A l g o r y t m generacji trajektorii ... 311

od po wi e d ni m ws p ół r z ęd n ym pu nk tu P i K; czas pierwszego GPP T t=0, czas drugiego GPP T 2=1.0 sek,; kształt trajektorii - linia prosta; rodzaj generacji - zgrubny; czas dyskretyzacji AT=0.004 sek.; DP=0.0002 m; DF=60°, DFW=2 . W yn ik ie m generacji jest 1283 DPP, które ilustrują ry s. 7 i rys .8.

Rys.7a. Zadane ws pó łr z ę dn e p oł o że nia chwytaka x j (t) , y z (t) , z^ft)- Fig.7a. Requi re d c oor di na tes of posit io n task x j;(t) , y i (t) , z j (t)

R y s .7b.Zadane ws pó łrz ęd ne orientacji ch wytaka ^ ( t ) , S^tt) , ^ ( t ) F i g . 7b. Requi re d coord ina te s ori entation of task ^ ( t ) , ® 2 (t) , ? i (t)

1 1 2 Tade us z Szkodny. Andrzej Fortuna

Współrzędne y oraz

z^,

równy jest 360 , gdyż o tyle różnią się m i ni ma ln a i ma ksy ma ln a wartość graniczna 0’ .

-1

-11

R y s .8.Zadane współ rzę dn e nat ur al ne siłowników 6< (1 1+6^ z (t ) F i g .8.Required natural co or din at es of actuators 8(t )+8 <6 (t )

Przykład ten pokazuje, że nie mo żl i w a jest liniowa zmiana kąta ł między punktami P i K trajektorii zadanej jak na ry s .6. Spowodo wa ne jest to wł aś ciwościami str uk tur y kin em atycznej M RP IR b -6.

4 .Wnioski

Przed st awi on y tu algo ry tm jest n ar z ęd z i em k o mpu te ro wym um o żl i w ia j ąc y m : a) pla no wa n i e trajektorii zadanej c hw yt aka MRP IRb- 6 z w yb r an y m kształtem

toru ruchu i zdefi ni owa ną lub dowol ną orientacją,

b) analizę możliw ośc i kine ma tyc zn yc h realizacji tr ajektorii zadanych przez MRP IRb-6, w yn i k aj ą cy c h z pr o je kt ow ane go zr o bo t yz owa ne go sta­

nowiska technologicznego,

c) gener owa ni e p rz ebi eg ów ws p ó łr z ęd n y ch nat ura ln y c h siłowników, odpowiad aj ący ch traje kto rio m zadany m chwytaka, będą cy ch jed no cześnie wa rtościami zadanymi se r wo m ech ani zm ów steruj ąc ych M R P I Rb -6,

d) opr acowanie warstw y wy zn a c za n ia tr ajektorii robota IR b- 6 i I Rp -6, które wyp os aż one są w identyczne manipulatory.

Pr ze dst aw ion y algo ry tm jest p od at ny na modyfikacje, np. przez m od y fi ­ kację segmentu pomocniczego, defi niu jąc eg o prz es tr z eń roboczą, możem y uwzględnić przeszkody, Tctóre należy ominąć.

A l g o r y tm g e n e r a c j i t r a .ie k t e r i i ... 313

LITERATURA

[1] Baraniec T.: Planowa ni e trajektorii mi ni mal noczasowych z dowolną kine­

matyką dla robota IRb-6. Praca dyplomowa, Instytut Automatyki, Po lit ech ni ka Śląska, Gliwice 1991 .

[2] C ama ri nk a-M at os L.M.: Plan G ene ra ti on in Robotics, IEEE J.Robotics and Automation, No-3, 1987

[3] Dulęba I Ły sa kow sk a B . :Zmo dyf ik ow ana metoda R.M.Taylor a pa r a met ry ­ zacji ścieżek pr o st o lin io wy ch ruchu robota IRb-6. III K.K.Robotyki t.2, Wrocł aw 1990.

[4] Gerke W .:C o l l i s i o n - f ree and Shortest Path for Industrial Robots Found by Dynamic Programing. Ro botosysteme 1, 1985.

[5] Gous en es L .:S t r a t e g ies for Solving Collision Free Trajectories.

Probl em for Mo bile and Manipu lat or Robots, I n t e r n .J .Robotics Res.

3(4), 1984.

[6] Jac ak W . :St ra teg ie s of Searching for Collision-Free Manipulators Motio ns : Au tom a ta Theory Approach, Robotica 7(2), 1989.

[7] Jacak W . ,Ł ysa ko ws ka B . ¡Dyskretne metody m odelowania kinematyki robota.

III K .K .R o b o t y k i . t.2, Wrocław 1990.

[8] Kn a pcz yk J.,Kisiel I.¡ We ktorowa metod a wyznaczania ruchu członów m a n i ­ p u la to ra z sz eś cioma parami obrotowymi (zadanie odwrotne) Z.N.Pol.Śl., s . Mec ha ni ka 2.8 6, Gliw ice 1987.

[9] Knapc zy k J .,Stępniewski A. ¡ A na l iz a kinematyczna i dynamiczna ma nip u­

l at ora z pi ę c io m a parami obrotowymi metodą macierzową dla zadanej tra­

jektorii ruchu. Z.N.Pol.Śl. s.M echanika z.85. Gliwice 1985.

10) L oz an o- Per ez T . ¡Spatial Planing : Configura ti on Space Approach. IEEE Trans, on Co mputers 32(2), 1983.

[11) Ni e der li ńs ki A. ¡ Roboty prz emysłowe , Wyd. Szkolne i Pedagogiczne, W a r s z a w a , 1981.

[12J Paul R. P. ¡ Robot M a n i p u l a t o r s ¡ Mathem ati c , Programing and Control.

MIT Press, Cambridge, Mas sac hu se tts and London, 1983.

[13) Szkod ny T . ¡ Dyn am ika manipulatorów, praca niepublikowana.

[14) Szko dn y T. ¡ K i ne ma ty czn a dekom poz yc ja ruchu członu roboczego robota IRb-60. K . K . R o b o t y k i , Wrocław, 1990.

[15) S zk od ny T . ¡ M an ip ul ato ry robotów przemysło wy ch - Modele matematyczne.

Sk rypt P o l .Ś 1 .n r .1507, Gliwice 1989.

[16) Sz y naw a T.; Kompute ro we pl an ow ani e trajektorii minimalnoczasowych z za daną k ine ma ty ką dla ma ni pul at or a IRb-6. Pra ca dyplomowa. Instytut Au to mat yk i Pol.Śl., Gliwice 1991.

[17) Ta yl or R.M.: Plan nin g and ex ecution of straight-line manipulator trajectories, Robot mot io n ¡ planning and control.eds. M.Brady i in.

MIT Press Cam bri dg e 1983.

R e c e n z e n t ¡ Prof.dr h.inZ. Antoni Wożniak Wpły nę ło do Redakcji do 30.04.1992 r.

Abstract;

In this wor k origin all y PLAN2 algorithm of generation required traj ec tor ie s with defined kinematics is presented. Kinematics of this traj ec tor ie s we must d e fin ed onl y in points, over its a task required move.

Distance betwe en the points may be any value. This algorithm applied formulas, which are analitycal solution of inverse kinematics problem of IRb- 6 manipulator.

This a l go ri thm we ma y applied to ;

a) plan ni ng tr aj ectories wi th required shape path (s t r a i g ht l in e , cylindrical, spherical) of move task;

b) analysis of kinematic possibi li ty realization of required t r a j e c t o r i e s ;

c) ge ne rat io n required value of actuators coordinates (adequates to required traj ect or ie s of task) for servo regulators;

d) proje ct of layer, wh i ch is functional element of structure control of IRb- 6 and IRp- 6 robots, w hi c h have identic manipulators .