Planowanie trajektorii ruchu chwytaka z punktem pośrednim

Transkrypt

1 Dr nŝ. Andrzj Graboś Dr nŝ. ark Boryga Katdra InŜynr chancznj Automatyk, Wydzał InŜynr Produkcj, Unwrsytt Przyrodnczy w ubln, ul. Dośwadczalna 50A, 0-80 ubln, Polska -mal: andrzj.grabos@up.lubln.pl -mal: mark.boryga@up.lubln.pl Planowan trajktor ruchu chwytaka z punktm pośrdnm Słowa kluczow: planowan trajktor, chwytak, wlomanowy profl przyspszna, udar Strszczn: W pracy zaprzntowano mtodę PC (Polynomal Cross thod do planowana trajktor ruchu chwytaka z punktm pośrdnm. PC ma zastosowan do planowana ruchu chwytaka, którgo tor składa sę z dwóch odcnków prostolnowych. Profl przyspszna na obu odcnkach opsany został wlomanm sódmgo stopna. W pracy przdstawono algorytm mtody oraz wynk w postac przbgów prędkośc, przyspszna udaru lnowgo.. Wprowadzn Planowan trajktor ruchu jst jdnym z prwszych kluczowych tapów przy ksploatacj zrobotyzowanych stanowsk pracy. Zagadnn to jst aktywnym polm badań naukowych wl pozycj ltraturowych pośwęconych jst tmu problmow. Autorzy stosowal róŝn tchnk planowana trajktor ruchu. Nktór z mtod uwzględnały ogranczn nkorzystngo zjawska udaru, co w praktyc korzystn wpływa na zmnjszn błędów odwzorowana trajktor. Vsol [0] oraz Dyllong Vsol [] do planowana trajktor stosowal splajny szścnn trygonomtryczn trzcgo rzędu, w przypadku których występuj nkorzystn zjawsko udaru w początkowym końcowym punkc toru. Zjawsko to, w nktórych przypadkach, zostało wylmnowan przz zastosowan splajnów trygonomtrycznych czwartgo rzędu. Jdnym z krytrów optymalzacj planowanj trajktor ruchu w pracy Cho nn [] było utrzyman udaru w okrślonych grancach. Otrzyman przbg udaru w parach knmatycznych są ncągł mają charaktr skokowy. Jdnoczśn, zarówno w początkowym jak końcowym punkc toru, udar jst róŝny od zra. Rd [7] do planowana trajktor wykorzystał splajny stosował stał (jdnak róŝn od zra wartośc udaru przy przjścu mędzy stałym fazam przyspszna opóźnna. Analzując przdstawon w pracy Rubo nn [8] przbg przyspszna ognw manpulatora Puma 560, moŝna stwrdzć, Ŝ w początkowym końcowym punkc toru występuj nkorzystn zjawsko udaru w parach knmatycznych. Podobna uwaga dotyczy przbgów udaru w parach knmatycznych w pracy Saramago Cccarll [9]. Przbg udaru w parach knmatycznych, uzyskan w pracy Huang nn [5], w początkowj końcowj chwl ruchu są blsk zru. toda zastosowana w pracy Olab nn [6], gnruj gładk przbg udaru narzędza z uwzględnnm ogranczń w parach knmatycznych. Bardzo ntrsując rzultaty dotycząc przbgu udaru w parach knmatycznych uzyskano w pracy Gaspartto Zanotto [4]. Wykorzystując B-splajny pątgo rzędu uzyskano n tylko cągły przbg udaru, al takŝ jgo zrow wartośc w początkowym końcowym punkc toru. Wlomany wyŝszych stopn opsując profl przyspszna wykorzystal Boryga Graboś []. W pracy analzowal przbg prędkośc, przyspszń oraz udarów dla wlomanów pątgo, sódmgo oraz dzwątgo stopna. Na

2 podstaw przprowadzonych badań symulacyjnych uzyskal najmnjsz wartośc udarów lnowych kątowych stosując wloman sódmgo stopna. W nnjszj pracy autorzy przdstawl algorytm PC, który umoŝlwa planowan ruchu chwytaka, którgo tor składa sę z dwóch odcnków prostolnowych. Odcnk t są połoŝon w przstrzn roboczj manpulatora. Sformułowano następując załoŝna dotycząc ruchu chwytaka: profl przyspszna na obu odcnkach prostolnowych opsano wlomanm sódmgo stopna, w początkowym końcowym punkc toru profl przyspszna jst styczny do os czasu, co lmnuj nkorzystn zjawsko udaru, zmana fazy rozruchu na fazę hamowana następuj w punkc pośrdnm, wartość przyspszna lnowgo dowolnj współrzędnj n przkracza załoŝonj wartośc maksymalnj - a max, ruch chwytaka odbywa sę w tak sposób, by w punkc pośrdnm (połączn odcnków prostolnowych wartość prędkośc wypadkowj n ulgała zman. Konskwncją załoŝna stałj wartośc prędkośc wypadkowj w punkc pośrdnm jst zrowa wartość przyspszna wypadkowgo. NalŜy tu zaznaczyć, Ŝ w punkc pośrdnm zman ulga krunk wktora prędkośc wypadkowj, co wynka z przyjętgo toru ruchu chwytaka. Istotną zaltą przdstawongo algorytmu jst fakt, Ŝ współczynnk wlomanów opsujących profl przyspszna, na dowolnj współrzędnj, wyznacza sę jdyn na podstaw przyrostu współrzędnj załoŝongo przyspszna maksymalngo. Jdnoczśn wylmnowan udaru w początkowym końcowym punkc toru pozytywn wpłyn na dokładność odwzorowana trajktor ruchu, co jst szczgóln korzystn w przypadku procsów tchnologcznych takch jak np. prznoszn, malowan, montaŝ, spawan, zgrzwan lnow, uszczlnan (kljn, paltyzacja dpaltyzacja. Układ pracy jst następujący. W rozdzal przdstawono sposób planowana trajktor ruchu z wykorzystanm wlomanu sódmgo stopna wykorzystujący krotnośc mjsc zrowych. W rozdzal przdstawono algorytm mtody, który podzlono na oblczna wstępn, oblczna dłuŝszgo krótszgo odcnka prostolnowgo oraz oblczna końcow. W rozdzal 4 pracy przdstawono przykład wykorzystana zaproponowango algorytmu. Wynk symulacj przdstawono w rozdzal 5. Wnosk przdstawon zostały w ostatnm rozdzal pracy.. Planowan trajktor ruchu z wykorzystanm wlomanów Do planowana trajktor ruchu chwytaka wykorzystywać moŝna wlomany wyŝszych stopn, któr umoŝlwają kształtowan proflu przyspszna. W pracy Boryga Graboś [] wykazano, Ŝ spośród wlomanów pątgo, sódmgo dzwątgo stopna opsujących profl przyspszna, najmnjsz wartośc udarów lnowych kątowych występują w przypadku wlomanu sódmgo stopna. W zwązku z tym, w nnjszj pracy do opsu przbgu przyspszna na współrzędnj x wykorzystano wloman sódmgo stopna postac & x ( t = a ( t ( t 0.5t ( t t ( gdz: a współczynnk wlomanu na współrzędnj x, =,, numr współrzędnj, t czas końca ruchu. Profl przyspszna opsango wlomanm sódmgo stopna przdstawa rys..

3 Rys.. Profl przyspszna opsany wlomanm 7 stopna Profl przyspszna jst funkcją cągłą na kaŝdj współrzędnj kartzjańskgo układu współrzędnych x. Zmana fazy rozruchu na fazę hamowana następuj w czas t=0.5t. Jdnoczśn profl przyspszna w chwlach t=0, t=0.5t oraz t=t jst styczny do os czasu. W tn sposób, w punktach tych lmnowan jst zjawsko udaru. Wlomany opsując profl prędkośc przmszczna, wyznaczon w wynku analtyczngo całkowana zalŝnośc (, przdstawają sę następująco: x& ( t = a t tt + t t tt + t t t t ( x ( t = a t tt + t t tt + t t tt ( Wyznaczona wlkość x (t jst drogą przbytą przz chwytak na współrzędnj. Aby okrślć współrzędną chwytaka w dowolnj chwl czasu, nalŝy uwzględnć początkową współrzędną chwytaka na współrzędnj oznaczoną x b oraz krunk ruchu chwytaka zgodny bądź przcwny do zwrotu wrsora os. Współrzędną chwytaka na współrzędnj okrśla wówczas równan x (t = xb ± a t tt + t t tt + t t t t ( JŜl ruch chwytaka jst zgodny z wrsorm os to w równanu nalŝy uwzględnć znak plus, w przcwnym przypadku znak mnus.. Planowan trajktor ruchu z punktm pośrdnm.. Polynomal Cross thod (PC PC ma zastosowan do planowana ruchu chwytaka, którgo tor składa sę z dwóch połączonych odcnków prostolnowych B E (rys.. Wprowadzn wlomanowgo proflu przyspszna chwytaka, okrślongo zalŝnoścą (, na zaplanowanych odcnkach toru B E spowodowałoby, Ŝ w punkc pośrdnm prędkość chwytaka wynosłaby zro. Problm planowana trajktor ruchu uproścłby sę wówczas do ruchu z zatrzymanm w punkc pośrdnm.

4 Rys.. Planowany tor ruchu BE odcnk pomocncz E B Z tgo względu wprowadza sę pomocncz punkty E B. Punkt E powstaj przz symtrę osową punktu B względm punktu pośrdngo, natomast punkt B powstaj w wynku symtr osowj punktu E względm punktu pośrdngo. Do opsu przyspszna okrślongo zalŝnoścą ( wykorzystywan są odcnk BE oraz B E (nazywan w algorytm całkowtym. Na obydwu odcnkach całkowtych zmana fazy rozruchu na fazę hamowana następuj w punkc pośrdnm. Wartość maksymalngo przyspszna chwytaka ogranczono na współrzędnj o maksymalnym przyrośc drog do wartośc a max. ZałoŜono, Ŝ przy przjścu z odcnka B na odcnk E (w punkc pośrdnm wartość wypadkowj prędkośc n ulga zman, a wartość wypadkowgo przyspszna jst równa zro. Zmana krunku zwrotu wktora prędkośc w punkc, wynka z przyjętgo toru ruchu chwytaka. W punkc tym następuj obrót wktora prędkośc wypadkowj z krunku B na krunk E. Problm tn moŝ zostać wylmnowany poprzz wprowadzn łuku łączącgo odcnk prostolnow lub przz altrnatywn zatrzyman w punkc pośrdnm. Współczynnk wlomanów opsujących profl przyspszna na odcnkach całkowtych wyznacza sę oddzln na kaŝdj współrzędnj x. Czas trwana ruchu oblcza sę wykorzystując jdyn przyrosty drog oraz załoŝon przyspszn maksymaln a max. Wartość prędkośc w punkc pośrdnm wyznacza sę podstawając do zalŝnośc okrślającj profl prędkośc czas t = 0.5t. Wypadkowy wktor prędkośc w punkc pośrdnm prznos sę z jdngo odcnka na drug rzutuj na os nruchomgo układu współrzędnych. UmoŜlwa to, wyznaczn współczynnków wlomanu opsującgo profl przyspszna na drugm odcnku całkowtym. PonwaŜ czas ruchu na obu odcnkach całkowtych moŝ być róŝny, nalŝy takŝ dokonać odpowdngo przsunęca w czas proflów przyspszna, prędkośc, przmszczna udaru... Algorytm mtody PC... Oblczna wstępn Krok. ZałoŜn współrzędnych punktu początkowgo, pośrdngo oraz końcowgo b b b m m m oznaczn ch: B( x ; x ; x, ( x ; x ; x E( x ; x ; x. Punkty t pownny lŝć w przstrzn roboczj. b' b' b' Krok. Wyznaczn współrzędnych punktów pomocnczych B' ( x ;x ;x oraz ' ' ' E' ( x ; x ; x na podstaw zalŝnośc: b' m x = x x dla =,, (5 x ' m = x x dla =,, (6 b Punkty pomocncz B oraz E n muszą spłnać warunku przynalŝnośc do przstrzn roboczj.

5 Krok. Wyznaczn przyrostów drog na poszczgólnych współrzędnych odcnków całkowtych: x = x x dla =,, (7 BE' B' E ' b x = x x dla =,, (8 b' Krok 4. Uszrgowan oblczonych przyrostów począwszy od najwększgo oznaczn ndksm dolnym w nawas klamrowym w koljnośc uszrgowana BE' BE' x x (9 x BE' { } x{ } B' E B' E B' E x{ } x{ } (0 Krok 5. Wyznaczn maksymalngo przyrostu współrzędnj spośród odcnków BE B E oznaczn go jako tzn. x W oznacznu x = BE' B' E x BE' B' E x = max{ x, x } ( x ndks górny oznacza dłuŝszy odcnk całkowty. JŜl przyrosty BE' są równ to nalŝy przyjąć x =. x Krok 6. ZałoŜn maksymalngo przyspszna chwytaka a max na współrzędnj o maksymalnym przyrośc drog. Przyspszna na pozostałych współrzędnych n x przkroczą wówczas załoŝongo przyspszna a max, co wynka z mnjszych bądź równych przyrostów drog na tych współrzędnych.... Oblczna dłuŝszgo odcnka całkowtgo ( Krok. Wyznaczn współczynnka wlomanu współrzędnj o najwększym przyrośc drog - a oraz czasu trwana ruchu x { } wymaga rozwązana układu równań t na 9 a( t = x c - a( ct ( ct -0.5t ( ct -t =amax ( Rozwązan powyŝszgo układu równań jst następując: 5 c amax a = 9 ( c c a ( x x ( cc amax x t = (4 amax gdz: c = 0080, c = -, c = - c ( c - ( c Krok. Wyznaczn współczynnków wlomanu na pozostałych współrzędnych [] c a { } = x 9 { } dla =, (5 ( t Krok. Wyznaczn składowych oraz wypadkowj prędkośc chwytaka w punkc 8 ( t x &{ } = a{ } dla =,, (6 644 max

6 ( t x& (7 8 = ( a{ } 644 =... Oblczna krótszgo odcnka całkowtgo (S Krok. Wyznaczn kosnusów krunkowych mędzy wktorm prędkośc w punkc, a osam nruchomgo układu współrzędnych ' b x x B' E BE' cos(α = jŝl x > (8 = = ( x ' x b' x x cos(α = jŝl ( x b x b' x x (9 B' E BE' x gdz: α kąt mędzy wktorm prędkośc w punkc, a osą x nruchomgo układu współrzędnych. Zwrot wktora prędkośc wynka z krunku ruchu na odcnku. Krok. Wyznaczn składowych prędkośc w punkc S x& cos(α dla =,, (0 Zgodn z załoŝnm wartość wypadkowj prędkośc w punkc na dłuŝszym krótszym S odcnku n ulga zman x &. Krok. Wyznaczn czasu trwana ruchu na krótszym odcnku całkowtym S S 05 x t = dla = ( S 64 x& Wzór ( wynka z rozwązana układu równań zbudowango z zalŝnośc (5 (6. Tn sam czas ruchu otrzymuj sę podstawając jdnoczśn odpowdn przyrosty współrzędnych odpowdn składow prędkośc w punkc. Krok 4. Wyznaczn współczynnków wlomanów na poszczgólnych współrzędnych S 644 S a S 8 dla =,, ( ( t..4. Oblczna końcow Krok. Wyznaczn czasu rozpoczęca ruchu na odcnku B E S t t tb = ± ( Czas tn wyznacza sę po to, by w punkc wystąpła ta sama prędkość dokładn w tj B'E BE' samj chwl czasu. JŜl x > to w zalŝnośc ( nalŝy przyjąć znak mnus w { } x{ } przcwnym wypadku znak plus. Krok. Przsunęc w czas wlomanu okrślającgo profl przyspszna na współrzędnych odcnka B E o wartość t b B'E BE' & x ( t = a ( t t ( t 0.5t t ( t t t jŝl x > (4 b b b & x jŝl S S S ( t = a ( t tb ( t 0.5t tb ( t t tb { } x{ } x (5 B' E BE' { } x{ } Analogczngo przsunęca w czas nalŝy dokonać w przypadku wlomanów okrślających profl prędkośc, przmszczna, a takŝ udaru. Krok. Wyznaczn czasu ruchu na odcnkach wzdłuŝ toru BE

7 t S t + t = (6 4. Przykład numryczny Współrzędn punktów zaplanowango toru ruchu chwytaka B,, E oraz pomocnczych punktów E B przdstawono w tab.. Przyrosty drog na poszczgólnych współrzędnych wynoszą: x BE' BE' BE' B' E B' E = 0, x = x = 0.5 m, x = x = 0.5 m, x B' E = 0. BE' B' E BE' PonwaŜ x = stąd x =. x x{ } Tabla. Współrzędn punktów zaplanowango toru ruchu chwytaka punktów pomocnczych Oznaczn Współrzędn punktu [m] punktu x x x B E B E Przyjęto przyspszn maksymaln a max = m/s na współrzędnj x. Współczynnk wlomanu okrślającgo profl przyspszna na poszczgólnych współrzędnych oraz czas 9 trwana ruchu wynoszą odpowdno: a = a{ } = m / s, a { } = 0, t =.4 s. PonwaŜ na odcnku BE najdłuŝsz przyrosty drog występują na współrzędnj x oraz x, stąd wyznaczon współczynnk a a dotyczą odpowdno tych właśn { } współrzędnych. Wypadkowa prędkość w punkc wynos x& = m / s. Kosnusy krunkow wktora prędkośc w punkc odcnka B E wynoszą cosα = /, cosα = 0, cosα = /. Składow prędkośc w punkc na odcnku B E wynoszą S S x& = 0.6m / s, x& S = 0, przy czym ndksy, odnoszą sę do os nruchomgo układu współrzędnych. Współczynnk wlomanów okrślających profl przyspszna na poszczgólnych współrzędnych odcnka B E wynoszą S S 9 a = a = m /, = 0. Czas trwana ruchu na odcnku B E wynos t S =.4 s. s a S Czas ruchu wzdłuŝ toru BE wynos 5. Wynk symulacj t =.4 s, zaś t b = 0. W wynku przprowadzonych symulacj uzyskano przbg knmatycznych charaktrystyk ruchu chwytaka, któr przdstawono na rys. -5. Na kaŝdym z przdstawonych rysunków lną cągłą oznaczono przbg knmatycznych charaktrystyk ruchu planowango toru BE, natomast lną przrywaną przbg na odcnkach pomocnczych (E B. Planowany tor ruchu odcnk pomocncz przdstawono w nruchomym układz współrzędnych x x x, natomast knmatyczn charaktrystyk ruchu przdstawono w dwóch płaszczyznach prostopadłych do płaszczyzny wyznaczonj przz punkty planowango toru BE. Na rys. przdstawono przbg prędkośc chwytaka na odcnkach BE B E. S aksymalna prędkość x& m / s osągana jst w punkc. Wartość prędkośc =

8 przy przjścu z odcnka B na E n ulga zman, natomast wktor prędkośc wypadkowj zmna krunk z B na E. Rys.. Przbg prędkośc wypadkowj na zaplanowanym torz BE odcnkach pomocnczych Rys. 4. Przbg przyspszna wypadkowgo na zaplanowanym torz BE na odcnkach pomocnczych Rys. 5. Przbg udaru wypadkowgo na zaplanowanym torz BE na odcnkach pomocnczych Przbg wypadkowgo przyspszna lnowgo chwytaka na odcnkach BE B E przdstawon są na rys. 4. W punktach B, E przyspszn jst równ zru. Osągana bzwzględna wartość maksymalna przyspszna na poszczgólnych współrzędnych n przkracza wartośc załoŝonj a max =m/s, a maksymalna wypadkowa wartość wynos.8 m/s. Wartość udaru w punktach B E jst równa zro (rys. 5. aksymalna wartość udaru wynos 9.8 m/s.

9 4. Wnosk Na podstaw przprowadzonj symulacj ruchu chwytaka wg mtody PC sformułowano następując wnosk: a Przbg wypadkowj prędkośc, przyspszna udaru otrzyman przy wykorzystanu PC są funkcjam cągłym na odcnkach B E. W punkc pośrdnm wartość wypadkowj prędkośc n ulga zman, zgodn z przyjętym załoŝnm. Krunk wktora prędkośc zmna sę z krunkm ruchu (z B na E. b Planowan trajktor wdług PC moŝna wykorzystać w nktórych procsach tchnologcznych (prznoszn, malowan, montaŝ, spawan, zgrzwan lnow, uszczlnan, kljn, paltyzacja dpaltyzacja, gdz szczgóln stotn jst wylmnowan udaru w początkowym końcowym punkc toru ruchu. c W przypadku uwzględnna odkształcalnośc łańcucha knmatyczngo, wylmnowan udaru pownno wpłynąć na ogranczn drgań zwększn dokładnośc pozycjonowana. Przdmotm dalszych badań będz wpływ trajktor ruchu chwytaka (planowanj na podstaw mtody PC na knmatykę dynamkę manpulatora. Bblografa. Boryga., Graboś A. Plannng of manpulator moton trajctory wth hghr-dgr polynomals us. chansm and achn Thory 009; 44: Cho Y.K., Park J.H., Km H.S., Km J.H. Optmal trajctory plannng and sldng mod control for robots usng voluton stratgy. Robotca 000; 8: Dyllong E., Vsol A. Plannng and ral-tm modfcatons of a trajctory usng spln tchnqus. Robotca 00; : Gaspartto A., Zanotto V. Optmal trajctory plannng for ndustral robots. Advancs n Engnrng Softwar 00; 4: Huang P., Xu Y., ang B. Global mnmum-jrk trajctory plannng of spac manpulator. Intrnatonal Journal of Control, Automaton, and Systms 006; 4 no. 4: Olab A., Béaré R., Gbaru O., Damak. Fdrat plannng for machnng wth ndustral sx-axs robots. Control Engnrng Practc 00; 8(5: Rd E. A dynamc optmal trajctory gnrator for Cartsan Path followng. Robotca 000; 8: Rubo F.J., Valro F.J., Suñr J.., ata V. Smultanous algorthm to solv th trajctory plannng problm. chansm and achn Thory 009; 44: Saramago S.F.P., Cccarll. An optmum robot path plannng wth payload constrants. Robotca 00; 0: Vsol A. Trajctory plannng of robot manpulators by usng algbrac and trgonomtrc splns. Robotca 000; 8: 6-6.