KATEDRA ROBOTYKI I MECHATRONIKI

Rozprawa doktorska na temat:

Generacja trajektorii dla mikromanipulacji

za pomocą robotów

Autor: mgr inż. Janusz Ochoński

Promotor: dr hab. inż. Maciej Petko

Kraków 2015

2

Podziękowania

W tym miejscu chcę szczególnie podziękować mojemu promotorowi

dr hab. inż. Maciejowi Petko za wszelką pomoc, za wyjaśnienia i za cierpliwość, którą

okazał podczas moich studiów doktoranckich. Podziękowania należą się również

pozostałym osobom z naszego zespołu badawczego: Grzegorzowi Karpielowi,

Konradowi Gacowi, Konradowi Kobusowi i Grzegorzowi Górze. Dziękuję jeszcze osobom

niewymienionym z imienia i nazwiska, które pomagały mi w trakcie studiów

doktoranckich.

3

Dedykacja

Niniejszą pracę dedykuję moim rodzicom, za miłość, wsparcie i inspirację.

4

Spis treści

Spis oznaczeń ............................................................................................................... 6

1. Wstęp ............................................................................................................... 12

2. Cel, teza i zakres pracy ..................................................................................... 18

3. Metody generacji trajektorii dla frezarek sterowanych numerycznie ............. 20

3.1. Interpolacja czasowa ................................................................................ 20

3.2. Interpolacja przestrzenna ......................................................................... 26

3.3. Generacja trajektorii ................................................................................. 32

4. Opracowany algorytm generacji trajektorii ..................................................... 44

4.1. Frezarka hybrydowa o pięciu stopniach swobody .................................... 44

4.2. Część off-line Generatora Trajektorii ........................................................ 47

4.2.1. Interpretacja G-kodu ............................................................................. 49

4.2.2. Kompensacja położenia ........................................................................ 51

4.2.3. Wykrywanie styczności segmentów ..................................................... 55

4.2.4. Wyznaczanie profilów przyspieszenia ................................................... 59

4.2.5. Korekta prędkości liniowej .................................................................... 63

4.3. Część on-line Generatora Trajektorii ........................................................ 67

4.3.1. Interpolacja czasowa ............................................................................. 68

4.3.2. Interpolacja przestrzenna ..................................................................... 74

5. Implementacja opracowanego Generatora Trajektorii ................................... 87

5.1. Implementacja w środowisku symulacyjnym ........................................... 87

5.2. Implementacja na docelowej platformie sprzętowej ............................... 91

6. Testowanie Generatora Trajektorii .................................................................. 99

6.1. Weryfikacja otrzymanej trajektorii w środowisku symulacyjnym ............ 99

6.2. Weryfikacja otrzymanej trajektorii na docelowej platformie sprzętowej

105

5

7. Podsumowanie i wnioski ................................................................................ 111

8. Bibliografia ..................................................................................................... 114

6

Spis oznaczeń

– przyrost przemieszczenia wzdłuż krzywej,

– wektor mnożników Lagrange’a,

– kąt kierunkowy będący rzutem wektora jednostkowego, stycznego do toru ruchu

w danym segmencie, na płaszczyznę XY – mierzony od dodatniego kierunku osi OX

w układzie współrzędnych XYZ,

– współczynniki wielomianu funkcji interpolującej g(u),

– kąt kierunkowy będący kątem pomiędzy wektorem jednostkowym, stycznym do

toru ruchu w danym segmencie, a płaszczyzną XY mierzony od poziomu

płaszczyzny w układzie współrzędnych XYZ,

– kąt kierunkowy będący kątem pomiędzy wektorem jednostkowym, stycznym do

przemieszczenia kątowego w danym segmencie, a dodatnim kierunkiem osi A w

układzie współrzędnych AC,

– chwilowa wartość kąta obrotu promienia wodzącego,

– wartość kąta promienia wodzącego na początku segmentu,

– tolerancja aproksymacji długości krzywej,

– promień wykonywanego łuku,

– znormalizowany parametr funkcji korekcji,

– kąt, którego tangens określa stosunek przemieszczenia liniowego do

przemieszczenia kątowego,

– współrzędna A orientacji w punkcie początkowym danego segmentu w globalnym

układzie współrzędnych,

– współczynniki wielomianu funkcji korekcji,

– wartość kompensacyjna rotacji wokół osi OX globalnego układu współrzędnych

zmierzona przez operatora podczas montażu obrabianego przedmiotu,

– wartość kąta obrotu napędu obrotowego A odczytana z instrukcji G-kodu,

– współczynniki wielomianu opisujące krzywą -tego segmentu dla osi

X globalnego układu współrzędnych,

– współczynniki wielomianu opisujące krzywą -tego segmentu dla osi

Y globalnego układu współrzędnych,

7

– zastępcze przyspieszenie obliczone jako długość wektora ,

– chwilowa wartość przyspieszenia wyznaczana dla aktualnej chwili czasu ,

– wartość początkowa przyspieszenia, od którego rozpoczyna się aktualna faza

danego segmentu toru ruchu,

– znormalizowane współczynniki funkcji korekcji,

– wartość chwilowego przyspieszenia liniowego dla ruchu rotacyjnego,

– wartość chwilowego przyspieszenia liniowego dla ruchu translacyjnego,

– przyspieszenie kątowe w ruchu obrotowym wokół osi OX w globalnym układzie

współrzędnych,

– przyspieszenie kątowe w ruchu obrotowym wokół osi OZ w globalnym układzie

współrzędnych,

– maksymalne chwilowe przyspieszenie liniowe,

– przyspieszenie liniowe wzdłuż osi X w globalnym układzie współrzędnych,

– przyspieszenie liniowe wzdłuż osi Y w globalnym układzie współrzędnych,

– przyspieszenie liniowe wzdłuż osi Z w globalnym układzie współrzędnych,

– wartość kompensacyjna rotacji wokół osi OY globalnego układu współrzędnych

zmierzona przez operatora podczas montażu obrabianego przedmiotu,

– ciągłość funkcji klasy ,

– współrzędna C orientacji w punkcie początkowym danego segmentu w globalnym

układzie współrzędnych,

– zmienna pomocnicza,

– wartość kompensacyjna rotacji wokół osi OZ globalnego układu współrzędnych

zmierzona przez operatora podczas montażu obrabianego przedmiotu,

– wartość kąta obrotu napędu obrotowego C odczytana z instrukcji G-kodu,

– równanie funkcji krzywej toru ruchu,

– sumaryczna odległość krzywej łamanej łączącej punkty pk,

– maksymalna wartość dżerku dla konkretnego segmentu,

– chwilowa wartość dżerku wyznaczana dla aktualnej chwili czasu t,

– maksymalna wartość dżerku,

– energia sygnału dyskretnego,

– błąd estymacji równania macierzowego,

– liniowa prędkość posuwu,

8

– fazy składowe profilu przyspieszenia dla różnych interpolacji czasowych,

– ciąg harmonicznych ( ),

– badany sygnał dyskretny,

– jednostka urojona,

– numer harmonicznej,

– uproszczona funkcja celu w optymalizacji funkcji korekcji,

– długość krzywej wielomianowej,

– maksymalna wartość udaru czyli pochodnej dżerku po czasie,

– długość odcinka liniowego,

– długość odcinka łączącego punkt początkowy i końcowy i-tego segmentu,

– wyznaczana długość wektora ,

– liczba punktów kontrolnych,

– liczba segmentów cząstkowych w krzywej wielomianowej,

– bazowa funkcja B-sklejana stopnia zdefiniowana na nieokresowym

(i niejednorodnym) wektorze węzłów ,

– numer próbki sygnału,

– współrzędne i-tego punktu kontrolnego,

– stopień wielomianu funkcji bazowej,

– wektor zawierający współrzędne położenia i orientacji danego punktu toru

ruchu w zależności od parametru geometrycznego wyrażone w globalnym układzie

współrzędnych,

– funkcja trajektorii narzędzia,

– wartość chwilowa pojedynczej współrzędnej wyznaczona dla danego parametru

geometrycznego ,

– współczynniki funkcji wielomianowej,

– macierz rotacji, wyznaczona jako ,

– obrót o kąt o wartości wokół osi 0X,

– obrót o kąt o wartości wokół osi 0Y,

– obrót o kąt o wartości wokół osi 0Z,

– orientacja narzędzia wokół osi OX w globalnym układzie współrzędnych,

– orientacja narzędzia wokół osi OZ w globalnym układzie współrzędnych,

– całkowite przemieszczenie wzdłuż krzywej B-sklejanej,

9

– zmienna pomocnicza,

– obliczone przemieszczenie zastępcze jakie ma do pokonania narzędzie robota

hybrydowego do frezowania wyznaczone jako długość wektora ,

– chwilowa wartość przemieszczenia wyznaczana dla aktualnej chwili czasu ,

– wartość początkowa przemieszczenia, od której rozpoczyna się aktualna faza

danego segmentu toru ruchu,

– długość -tego segmentu cząstkowego,

– wartość przemieszczenia wzdłuż toru ruchu od początku segmentu dla ruchu

rotacyjnego,

– przemieszczenie kątowe w danym segmencie,

– chwilowa wartość przemieszczenia wzdłuż toru od początku segmentu dla ruchu

translacyjnego,

– przemieszczenie liniowe w danym segmencie,

– okres próbkowania Układu Sterowania,

– przesunięcie o wartość wzdłuż osi X,

– przesunięcie o wartość wzdłuż osi Y,

– przesunięcie o wartość wzdłuż osi Z,

– czasy w których następuje zmiana fazy w trapezoidalnym profilu

przyspieszenia,

– pozycja narzędzia wzdłuż osi X w globalnym układzie współrzędnych,

– pozycja narzędzia wzdłuż osi Y w globalnym układzie współrzędnych,

– pozycja narzędzia wzdłuż osi Z w globalnym układzie współrzędnych,

– wektor węzłów krzywej B-sklejanej,

– parametr geometryczny funkcji,

– estymowany wektor parametru geometrycznego,

– pierwsza pochodna funkcji korekcji po przemieszczeniu,

– druga pochodna funkcji korekcji po przemieszczeniu,

– trzecia pochodna funkcji korekcji po przemieszczeniu,

– aktualny znormalizowany wektor parametru geometrycznego,

– funkcja przemieszczenia w czasie,

– pierwsza pochodna krzywej B-sklejanej po przemieszczeniu,

– druga pochodna krzywej B-sklejanej po przemieszczeniu,

10

– trzecia pochodna krzywej B-sklejanej po przemieszczeniu,

– minimalna wartość argumentu funkcji,

– maksymalna wartość argumentu funkcji,

– wartość parametru geometrycznego dla -tego punktu należącego do krzywej,

– prędkość zastępcza posuwu obliczona jako długość wektora ,

– chwilowa wartość prędkości wyznaczana dla aktualnej chwili czasu ,

– wartość początkowa prędkości, od której rozpoczyna się aktualna faza danego

segmentu toru ruchu,

– chwilowa wartość prędkości liniowej dla ruchu rotacyjnego,

– maksymalna prędkość kątowa, jaką osiąga stół uchylno – obrotowy,

– wartość chwilowej prędkości liniowej dla ruchu translacyjnego,

– prędkość posuwu definiowana za pomocą poleceń G-kodu,

– prędkość kątowa dla obrotu wokół osi OX w globalnym układzie współrzędnych,

– prędkość kątowa dla obrotu wokół osi OZ w globalnym układzie współrzędnych,

– wartość prędkości posuwu na końcu segmentu,

– maksymalna chwilowa prędkość liniowa,

– wartość prędkości posuwu na początku segmentu,

– prędkość liniowa wzdłuż osi X w globalnym układzie współrzędnych,

– prędkość liniowa wzdłuż osi Y w globalnym układzie współrzędnych,

– prędkość liniowa wzdłuż osi Z w globalnym układzie współrzędnych,

– wektor jednostkowy styczny do toru ruchu w punkcie początkowym -tego

segmentu,

– wektor jednostkowy styczny do toru ruchu w punkcie końcowym -ego

segmentu,

– współrzędna X punktu początkowego danego segmentu w globalnym układzie

współrzędnych,

– wartość kompensacyjna położenia dla osi X globalnego układu współrzędnych

zmierzona przez operatora podczas montażu obrabianego przedmiotu,

– współrzędna X położenia narzędzia odczytana z instrukcji G-kodu,

– współrzędna X położenia narzędzia w globalnym układzie współrzędnych z

uwzględnieniem kompensacji pozycji montażu przedmiotu obrabianego,

– współrzędna X toru ruchu w układzie współrzędnych stołu uchylno – obrotowego,

11

– współrzędna X środka obrotu na płaszczyźnie XY względem położenia punktu

początkowego segmentu,

– współrzędna X -tego punktu -tego segmentu krzywej wielomianowej,

– współrzędna Y punktu początkowego danego segmentu w globalnym układzie

współrzędnych,

– wartość kompensacyjna położenia dla osi Y globalnego układu współrzędnych

zmierzona przez operatora podczas montażu obrabianego przedmiotu,

– współrzędna Y położenia narzędzia odczytana z instrukcji G-kodu,

– współrzędna Y położenia narzędzia w globalnym układzie współrzędnych z

uwzględnieniem kompensacji pozycji montażu przedmiotu obrabianego,

– współrzędna Y toru ruchu w układzie współrzędnych stołu uchylno – obrotowego,

– współrzędna Y środka obrotu na płaszczyźnie XY względem położenia punktu

początkowego segmentu,

– współrzędna Y -tego punktu -tego segmentu krzywej wielomianowej,

– współrzędna Z punktu początkowego danego segmentu w globalnym układzie

współrzędnych,

– wartość kompensacyjna położenia dla osi Z globalnego układu współrzędnych

zmierzona przez operatora podczas montażu obrabianego przedmiotu,

– współrzędna Z położenia narzędzia odczytana z instrukcji G-kodu,

– współrzędna Z położenia narzędzia w globalnym układzie współrzędnych z

uwzględnieniem kompensacji pozycji montażu przedmiotu obrabianego,

– współrzędna Z toru ruchu w układzie współrzędnych stołu uchylno – obrotowego,

– współrzędna Z środka obrotu na płaszczyźnie ZX względem położenia punktu

początkowego segmentu.

12

1. Wstęp

Mikromanipulacja jest zadaniem polegającym na manipulowaniu obiektami o

rozmiarach w skali mikro (mikroobiekty). Tę definicję w swojej pracy zaproponował

Clévy [1]. Następnie Fukuda [2] rozbudował powyższą definicję do postaci:

Mikromanipulacja to manipulowanie mikroobiektami lub też manipulowanie obiektem

z rozdzielczością ruchu rzędu mikrometrów. Manipulatory robotów dzięki

odpowiedniemu układowi sterowania są w stanie realizować zadania manipulowania

przedmiotami czy też narzędziami z tego typu rozdzielczością ruchu. Jedną z gałęzi

przemysłu, która powoduje dążenie do osiągania wysokich dokładności ruchu przez

manipulatory, jest przemysł wytwórczy. Wykonywanie różnych podzespołów

najczęściej wiąże się z procesami obróbki skrawaniem, takimi jak: toczenie lub

frezowanie. Frezowanie jest z punktu widzenia wymaganych dokładności realizacji

ruchów jednym z najtrudniejszych rodzajów mikromanipulacji. Tor ruchu narzędzia jest

ściśle zdefiniowany i musi zostać odtworzony z bardzo wysoką dokładnością względną

(odniesioną do zakresu ruchu) w każdym punkcie. Dodatkowo ważnym jest aby nie

dostarczać do układu energii w paśmie wysokich (z punktu widzenia konstrukcji

mechanicznej) częstotliwości żeby nie wzbudzać drgań względnych narzędzie –

przedmiot obrabiany. By sprostać oczekiwaniom rynku producenci części zwiększają

produktywność ich zakładów poprzez kładzenie coraz mocniejszego nacisku na

następujące wymagania: wzrost jakości wytwarzanych podzespołów oraz skrócenie

czasu ich wykonywania. Do sprostania tym wymaganiom opracowano

zautomatyzowane obrabiarki CNC (Computer Numerical Control) pracujące z wysokimi

prędkościami posuwu podczas skrawania - HSM (High Speed Machining). Prędkość

posuwu jest parametrem reprezentującym ruch narzędzia skrawającego w stosunku to

obrabianego przedmiotu i wyrażany w mm/min. Obróbka HSM wyróżnia się

stosowaniem narzędzi skrawających o małych średnicach, w połączeniu z wysokimi

prędkościami posuwu, dzięki czemu zachowana zostaje wydajność obróbki

skrawaniem. Dodatkowo mniejsze narzędzia wprowadzają mniejsze naprężenia na

skrawany przedmiot. Obrabiarki typu HSM zapewniają wysoką jakość powierzchni

obrabianego produktu i szybki czas realizacji procesu obrabiania, jak również – dzięki

konstrukcji wieloosiowej – umożliwiają wykonywanie elementów o skomplikowanych

13

kształtach. Jednak wraz ze wzrostem prędkości posuwu obrabiarki HSM muszą osiągać

większą dynamikę ruchu. Rozwój urządzeń mechanicznych pociąga za sobą rozwój

układów sterowania, które muszą zapewnić precyzyjną realizację ruchów z wysoką

prędkością, wymaganą przez obrabiarki HSM.

Tradycyjne centra obróbcze konstruowane są na zasadzie otwartego łańcucha

kinematycznego, czyli konstrukcji szeregowych. W tych obrabiarkach każda z osi

obróbczych ma dedykowany napęd zapewniający ruch wzdłuż lub wokół tej osi. Takie

obrabiarki są urządzeniami ciężkimi, wielkogabarytowymi i wymagają napędów o dużej

mocy. Na przestrzeni ostatnich lat zaczęto opracowywać urządzenia do frezowania w

oparciu o manipulatory o zamkniętym łańcuchu kinematycznym – manipulatory

równoległe [3, 4, 5, 6, 7, 8, 9]. W tego typu konstrukcjach ruch narzędzia/frezu wzdłuż

głównych osi obróbczych nie został przypisany poszczególnym napędom, lecz jest

wykonywany przez złożenie równoczesnych przemieszczeń wszystkich napędów

wchodzących w skład zamkniętego łańcucha kinematycznego. Pozwala to na

zwiększenie sztywności manipulatora bez potrzeby zwiększania masy całego

urządzenia [10, 11, 12].

W Katedrze Robotyki i Mechatroniki w AGH w Krakowie opracowano hybrydową

frezarkę, składającą się z trzyramiennego robota równoległego oraz ze stołu uchylno –

obrotowego [13, 14, 15]. Pośrednie rozwiązanie, jakim są konstrukcje hybrydowe, łączy

zalety i wady obu typów mechanizmów. Zastosowanie zamkniętego łańcucha

kinematycznego sprawiło, iż frezarka ta ma większą sztywność niż obrabiarka

szeregowa o zbliżonej masie konstrukcji. Pozwoliło to uzyskać lepsze dokładności.

Robot równoległy odpowiedzialny jest za ruchy translacyjne wzdłuż trzech osi

globalnego układu współrzędnych. Stół uchylno – obrotowy zapewnia możliwość

rotacji obrabianym elementem wokół dwóch osi globalnego układu współrzędnych, co

w połączeniu z robotem równoległym pozwala na pięcioosiową obróbkę elementów o

skomplikowanych kształtach. Jako napędy w robocie frezującym użyte zostały silniki

bezpośrednie, cechujące się wysoką dynamiką pracy (pozwalają one realizować

frezarce hybrydowej obróbkę typu HSM), dużymi wartościami generowanych sił i

momentów oraz brakiem konieczności używania przekładni, co ogranicza możliwość

powstawania luzów w konstrukcji [16], jednak silniki te wymagają bardziej

zaawansowanego sterowania w porównaniu do klasycznych serwonapędów.

14

W klasycznych frezarkach stosowane są serwonapędy, które mogą pracować w

trybie pozycyjnym lub prędkościowym. W trybie pozycyjnym układ sterowania określa

docelową pozycję liniową lub kątową, którą serwonapęd ma osiągnąć. Stosowanie

trybu pozycyjnego powoduje uproszczenie układu sterowania, oraz ułatwienie

użytkownikowi współpracy z generatorem trajektorii. Jednakże sterowanie pozycyjne

ogranicza dokładność odwzorowania zadanego toru narzędzia. W obrabiarkach

wykorzystywany jest również tryb prędkościowy serwonapędów. W tym trybie układ

sterowania narzuca prędkość obrotową, z jaką dany serwonapęd ma pracować.

Wykorzystywano następnie przekładnie śrubowe, by zamienić ruch obrotowy

serwonapędu na ruchy liniowe narzędzia w obrabiarkach CNC. Napędy bezpośrednie

wymagają zastosowania dodatkowego trybu, w którym układ sterowania określa

wartości siły lub momentu, jaki mają wygenerować poszczególne napędy. W tym trybie

układ sterowania musi obliczyć wartości sił i momentów, jakie każdy z napędów ma

wygenerować. Aby obliczyć te wartości konieczne jest wyznaczenie pełnej zadanej

trajektorii ruchu narzędzia/frezu, czyli położenia i orientacji narzędzia względem

obrabianego przedmiotu, jego prędkości liniowej i kątowej oraz wartości przyspieszeń

ruchu w każdym okresie próbkowania. Część układu sterowania odpowiedzialna za te

obliczenia nazywa się Generatorem Trajektorii. Do zadań tego Generatora należą:

interpretacja poleceń programowanych przez operatora, wybór sposobu realizacji

pożądanego toru narzędzia opisanego w kodzie wejściowym w celu zmniejszenia czasu

frezowania elementu (z uwzględnieniem ograniczeń dynamicznych, jakie ma każdy z

zastosowanych w obrabiarce napędów), obliczenie trajektorii ruchu narzędzia będącej

wzorcem odniesienia dla Układu Śledzenia Trajektorii.

Powszechnie stosowanym sposobem komunikacji operatora z obrabiarką CNC jest

język G-kodów. Jest to znormalizowany [17] zapis poleceń jednoznacznie opisujący

kształt toru narzędzia. Z jego pomocą opisywane są również wytyczne dotyczące

parametrów procesu skrawania takie jak: prędkość posuwu (wyrażony w mm/min ruch

narzędzia skrawającego względem obrabianego przedmiotu), prędkość obrotowa

wrzeciona czy też stosowanie czynnika chłodzącego miejsce obróbki. Do zadań

Generatora Trajektorii należy poprawna interpretacja poleceń Operatora zapisanych za

pomocą G-kodów – wartości zawarte w G-kodzie mogą być podawane w różnych

jednostkach.

15

Następnym zadaniem Generatora Trajektorii jest wyznaczenie jak najlepszego pod

względem czasu wykonania i sposobu realizacji toru narzędzia, opartego na

odczytanych z G-kodu informacjach oraz przyjętych wartościach granicznych

parametrów dynamicznych napędów zastosowanych w obrabiarce CNC. Operator

tworząc program do obróbki w pojedynczej linii narzuca dystans, o jaki ma się

przemieścić narzędzie/frez, kształt toru ruchu oraz maksymalną chwilową prędkość

liniową, z jaką ma się owo przemieszczenie wykonać, jednak wartość przyspieszenia

osiągana podczas ruchu narzędzia jest uzależniona od możliwości technicznych

aktywnych napędów, od czasu reakcji tych napędów, od ich pasma przenoszenia oraz

bezwładności napędzanych mechanizmów. Robot hybrydowy do frezowania jest

zaprojektowany do obróbki metali miękkich takich jak np. miedź czy aluminium.

Maksymalna prędkość posuwu stosowana przy frezowaniu tego typu materiałów

wynosi 2

. Chcąc osiągnąć rozdzielczość ruchu poniżej 10µm konieczne jest

stosowanie częstotliwości próbkowania co najmniej 10kHz. Dalsze zwiększanie

częstotliwości w celu poprawy rozdzielczości ruchu jest ograniczone przez

częstotliwość pracy komutatorów wynoszącą 15kHz, dlatego też jako wartość

częstotliwości pracy Układu Sterowania przyjęto 10kHz i z taką częstotliwością również

powinien pracować Generator Trajektorii.

Opracowane dotychczas generatory trajektorii dla standardowych obrabiarek CNC,

przez wzgląd na dużą złożoność przeprowadzanych obliczeń dla obróbki wieloosiowej,

nie wyznaczają wartości wyjściowych z takimi częstotliwościami, w związku z czym

częstotliwość pracy generatorów trajektorii była kilkukrotnie niższa od częstotliwości

działania Sterownika Pozycji w Układzie Sterowania [18], co niekorzystnie obijało się na

dokładności obróbki. Koniecznym stało się obliczanie trajektorii z wyższymi

częstotliwościami [16]. Najlepszy wariant występuje w przypadku gdy częstotliwości

pracy Układu Sterowania oraz Generatora Trajektorii są sobie równe. W takim

rozwiązaniu można uzyskać większą dokładność odwzorowania zaprogramowanego

toru narzędzia poruszającego się z dużymi prędkościami. Obliczanie trajektorii w

trakcie obróbki było zbyt skomplikowane numerycznie, więc wyznaczano ją przed

rozpoczęciem procesu frezowania, jednak w przypadku robota hybrydowego do

frezowania ilość koniecznych danych, które by należało zapisać i później przesłać do

16

Sterownika Pozycji, jest jeszcze większa niż przy standardowych obrabiarkach (przez

wzgląd na konieczność obliczania nie tylko chwilowej pozycji, lecz również prędkości i

przyspieszenia). Przy zapisie z wykorzystaniem liczb zmiennoprzecinkowych podwójnej

precyzji (8 Bajtów na liczbę), liczby osi, dla których należy wyznaczyć pełną trajektorię

(3 parametry: położenie, prędkość i przyspieszenie dla pięciu osi obrabiarki) oraz

wymaganego próbkowania z częstotliwością pracy Układu Sterowania (10kHz), dla 1s

pracy obrabiarki należałoby przygotować 1.2MB danych. Obróbka wieloosiowa

zazwyczaj wykorzystywana jest do wytwarzania skomplikowanych kształtów, dla

których cały proces trwa wiele godzin, co wymaga bardzo dużej przestrzeni dyskowej w

pamięci wewnętrznej Układu Sterowania. Koniecznym stało się opracowanie podejścia,

w którym trajektoria będzie wyznaczana na bieżąco podczas wykonywania operacji

frezowania, co pozwoli uniknąć gromadzenia tak dużych ilości danych. Lavernhe [19]

zaproponował podzielenie zadań Generatora Trajektorii, tak by możliwe było

wyznaczanie skomplikowanych kształtów z wykorzystaniem obrabiarek HSM. Część

obliczeń jest wykonywana przed rozpoczęciem operacji frezowania w tzw. części off-

line. Następnie przygotowane dane służą już do szybszego obliczania trajektorii z

częstotliwością pracy układu sterowania – tę część nazwano on-line. To podejście

zastosowano w opracowanym Generatorze Trajektorii dla robota hybrydowego do

frezowania.

W części off-line wykonywane jest zadanie interpretacji G-kodu oraz etap

wytyczania i analizowania trajektorii w oparciu o zadany profil przyspieszenia. Kształt

profilu przyspieszenia w czasie ma wpływ nie tylko na efektywność realizowanej

trajektorii, ale dodatkowo wpływa na drgania, jakie mogą się pojawiać podczas

procesu obróbki [20]. Optymalne czasowo przyspieszenie zmienia się w sposób

skokowy, jednakże takie nagłe szarpnięcia powodują wzrost amplitudy drgań w

układzie narzędzie – przedmiot obrabiany, dlatego najnowsze generatory trajektorii

obliczają bardziej skomplikowany kształt profilu przyspieszenia i dążą do ograniczania

poziomu dżerku (pierwszej pochodnej przyspieszenia względem czasu). Powoduje to

zredukowanie drgań podczas obróbki, a także zwiększenie dokładności wykonania

elementu kosztem nieznacznego wydłużenia czasu realizacji operacji.

Gdy Generator Trajektorii wyznaczy parametry realizacji zadanego toru narzędzia,

jego następnym zadaniem jest obliczanie wartości potrzebnych dla Układu Śledzenia

17

Trajektorii. Ta część pracy Generatora Trajektorii odbywa się w trybie on-line, czyli z

częstotliwością pracy Układu Sterowania. Jeżeli narzędzie będzie się przemieszczać z

prędkością posuwu 2

, to w pojedynczym okresie próbkowania sterownika

przemieszczenie narzędzia wyniesie poniżej 3µm. Taki rząd precyzji ruchu robota

hybrydowego kwalifikowany jest jako mikromanipulacja. Sterowanie ruchami

narzędzia z wymaganą precyzją rzędu kilku mikrometrów wymaga generacji trajektorii

zadanej z równie wysoką dokładnością.

Opracowane dotychczas generatory trajektorii nie są w stanie sprostać

wymaganiom robota hybrydowego do frezowania. Nie ma aktualnie generatora

trajektorii, który jednocześnie pracowałby z częstotliwością próbkowania 10kHz,

generował trajektorię dla obrabiarki pięcioosiowej oraz by obliczana trajektoria

zawierała nie tylko wartości położenia, ale również by obliczane były wartości

chwilowe prędkości oraz przyspieszenia dla każdej z pięciu osi. Koniecznym stało się

opracowanie nowego Generatora Trajektorii, który jest w stanie współpracować z

robotem hybrydowym do frezowania, wypełnić założone wymagania wykonywanego

procesu technologicznego oraz pozwolić na wykorzystanie możliwości sterowania

napędami w trybie siłowym i momentowym.

18

2. Cel, teza i zakres pracy

Celem pracy jest opracowanie algorytmu generacji trajektorii, która pozwoli

wyznaczyć on-line pełną trajektorię dla robota hybrydowego do frezowania

szybkoobrotowego, z wysoką częstotliwością próbkowania i z wysoką dokładnością

odwzorowania zadanego toru narzędzia.

Głównym zadaniem Generatora Trajektorii jest obliczanie na podstawie zadanego

kodu trajektorii z trapezoidalnym profilem przyspieszenia w trybie on-line w

przestrzeni kartezjańskiej: wartości położenia i jego dwóch kolejnych pochodnych, z

częstotliwością pracy układu sterowania z odpowiednią dokładnością we wszystkich

osiach.

Dla tak określonego zadania można postawić następującą tezę pracy:

Możliwe jest opracowanie algorytmu generacji pełnej trajektorii (położenia i jego

dwóch pochodnych po czasie) w przestrzeni kartezjańskiej z trapezoidalnym profilem

przyspieszenia dla robota do frezowania o pięciu stopniach swobody, który może

zostać zaimplementowany on-line z wysoką częstotliwością próbkowania.

By rozwiązać postawiony problem, zrealizowano badania opisane w kolejnych

rozdziałach. Na początku, w rozdziale trzecim, dokonano analizy sposobów

generowania trajektorii, jakie używane są w obrabiarkach numerycznych oraz w

nowoczesnych centrach obróbczych. Dodatkowo zostały przedstawione różne sposoby

interpolacji kształtu toru narzędzia, jakie stosowane są w tego typu urządzeniach. W

kolejnym rozdziale zaprezentowano opracowany Generator Trajektorii. Dostosowano

go do współpracy z Układem Sterowania pięcioosiowego robota hybrydowego do

frezowania. Robot ten poza ruchami translacyjnymi ma możliwość wykonywania

ruchów rotacyjnych wokół osi OX i OZ globalnego układu współrzędnych. Podkreślono

wymagania, dotyczące generacji trajektorii, które wynikają z konstrukcji robota

hybrydowego. Rozdział piąty poświęcony jest implementacji opracowanych

algorytmów w środowisku symulacyjnym (służącym do testów poprawności działania

19

opracowanej metody generacji trajektorii) oraz w drugim środowisku, jakim jest

docelowa platforma sprzętowa Układu Sterowania robotem hybrydowym do

frezowania. Opisane zostały oba środowiska oraz narzędzia, dzięki którym możliwa

była implementacja opracowanych algorytmów. Następny rozdział zawiera wyniki

przeprowadzonych eksperymentów weryfikujących poprawność generowanych

trajektorii. Eksperymenty wykonano porównując trajektorie otrzymane z Generatora

Trajektorii robota hybrydowego z trajektoriami wyznaczonymi w środowisku

symulacyjnym. Rozdział siódmy zawiera podsumowanie przeprowadzonych badań oraz

wnioski, które wyciągnięto na podstawie otrzymanych wyników.

20

3. Metody generacji trajektorii dla frezarek sterowanych

numerycznie

Problem planowania trajektorii sprowadza się do odnalezienia relacji pomiędzy

dwoma elementami należącymi do różnych dziedzin: czasu i przestrzeni [21]. Do

głównych zadań Generatora Trajektorii wlicza się dwie interpolacje: czasową i

przestrzenną, które w efekcie pozwalają uzyskać pożądaną trajektorię narzędzia. Typ

interpolacji czasowej decyduje o dynamicznej pracy napędów frezarki, od niej zależy

również czas, w jakim wykonywany jest cały proces frezowania [20, 22, 23, 24],

natomiast interpolacja przestrzenna wpływa na dokładność odwzorowania zadanego

kształtu toru narzędzia oraz na uzyskaną jakość obrabianej powierzchni [12, 25].

3.1. Interpolacja czasowa

Pojęcie interpolacji czasowej odnosi się do przebiegów czasowych przemieszczenia i

jego kolejnych pochodnych. Najczęściej stosowanym typem interpolacji czasowej w

klasycznych obrabiarkach sterowanych numerycznie jest interpolacja z trapezoidalnym

przebiegiem prędkości. Przykład tego typu interpolacji czasowej przedstawiono na

Rysunku 1, w którym przyjęto następujące wartości parametrów:

s = 0.2m – całkowite przemieszczenie,

vmax = 0.1m/s – maksymalna chwilowa prędkość liniowa,

amax = ±0.15m/s2 – maksymalne chwilowe przyspieszenie liniowe,

Rysunek 1 Przykład interpolacji czasowej z ograniczonym przyspieszeniem i trapezoidalnym przebiegiem prędkości.

f1 f2 f3

21

W fazach rozpędzania i wytracania prędkości, oznaczonych jako f1 i f3, natychmiast

osiągana jest maksymalna dopuszczalna wartość przyspieszenia. W fazie przejściowej f2

ruch odbywa się z jednostajną prędkością. Ten typ interpolacji wyznacza trajektorię z

ciągłością C1, tzn. iż przebiegi przemieszczenia oraz jego pierwszej pochodnej są ciągłe.

Zapewnia to gładkość przebiegu przemieszczenia i brak skokowych zmian prędkości,

jednakże gwałtowne zmiany przyspieszenia w tego typu interpolacji są źródłem

zwiększonych drgań wysokoczęstotliwościowych, działających niekorzystnie na jakość

obrabianych powierzchni i grożących uszkodzeniem frezu lub wykonywanego

elementu. Zaletą tego typu interpolacji czasowej jest efektywność (czyli osiągnięcie jak

najkrótszego czasu realizacji ruchu). Ma to znaczenie, jeśli bardziej od jakości

wykonania elementu istotna jest zwiększona produktywność.

Kolejnym przykładem interpolacji czasowej jest wykorzystanie eksponencjalnego

profilu prędkości [26]. Przykład tego profilu przedstawiono na Rysunku 2. Za pomocą

tego typu interpolacji możliwe jest utworzenie gładkiego przebiegu przemieszczenia i

ciągłego przebiegu prędkości, analogicznie jak to miało miejsce we wcześniej opisanym

przykładzie. Dla lepszego zobrazowania różnic pomiędzy poszczególnymi typami

interpolacji czasowej w przykładowych przebiegach użyto takiego samego zestawu

parametrów.

Rysunek 2 Przykład interpolacji czasowej z eksponencjalnym przebiegiem prędkości.

W tym przypadku również otrzymuje się przebiegi o ciągłości klasy C1. Ten typ

interpolacji czasowej cechuje się dynamiczną fazą rozpędzania f1 i łagodnym

f1 f2

f3

22

wyhamowywaniem f3, co ma odzwierciedlenie w czasie realizacji ruchu. Ostatnia faza,

w której następuje dojazd do pozycji zadanej odbywa się z bardzo małą prędkością i

małym przyspieszeniem. Pozwala to na osiągnięcie zadanej pozycji końcowej z

odpowiednią dokładnością.

Następny przykład interpolacji czasowej trajektorii to trapezoidalny profil

przyspieszenia nazywany również profilem „podwójnego S” (przez wzgląd na kształt

profilu prędkości) lub też profilem przyspieszenia z ograniczonym dżerkiem. Na

Rysunku 3 zaprezentowano przykład z wykorzystaniem tego typu interpolacji. Aby go

wyznaczyć przyjęto dodatkowy parametr:

dmax = ±1.5m/s3 – maksymalna wartość dżerku.

W przeciwieństwie do poprzednio opisanych metod ta tworzy przebiegi czasowe

ciągłe klasy C2, co oznacza, iż otrzymywane funkcje są różniczkowalne do drugiej

pochodnej przemieszczenia. Dodatkowo otrzymywane przebiegi przemieszczenia i

prędkości są gładkie.

Rysunek 3 Przykład interpolacji czasowej z trapezoidalnym przebiegiem przyspieszenia.

23

Ten typ interpolacji jest rozbudowanym profilem z trapezoidalnym przebiegiem

prędkości, zawierającym dodatkowe fazy, w których przyspieszenie jest liniowo

narastające, dzięki czemu ograniczony jest poziom dżerku tak, by nie przekraczał

zadanej wartości maksymalnej i prędkość zmienia się w sposób płynny. Otrzymuje się

w ten sposób łagodne rozpędzanie od zerowej prędkości, dynamiczne osiąganie

prędkości maksymalnej i łagodne wyhamowanie do pozycji końcowej. Czas, w jakim

osiągany jest punkt docelowy, z trapezoidalnym przebiegiem przyspieszenia, jest

nieznacznie dłuższy od czasu najbardziej efektywnego osiągniętego w profilu z

trapezoidalnym przebiegiem prędkości. Jednak dzięki ograniczeniu dżerku zostają

zmniejszone drgania występujące w przypadku interpolacji z trapezoidalnym

przebiegiem prędkości.

Dojazdowy profil prędkości [27] (Rysunek 4) został opracowany jako uproszczenie

interpolacji z trapezoidalnym przebiegiem przyspieszenia. Zrezygnowano z liniowego

narastania przyspieszenia w momentach rozpoczęcia ruchu i osiągania pozycji zadanej.

Następstwem tego jest nieograniczony dżerk występujący w momencie skokowej

zmiany przyspieszenia. Pozostawiono za to płynną zmianę prędkości w trakcie trwania

ruchu napędu.

Rysunek 4 Przykład interpolacji czasowej typu dojazdowego.

Dojazdowy profil prędkości jest wyjściem pośrednim, gdy operatorowi zależy na

zwiększeniu produktywności wykonywanych operacji kosztem zmniejszenia jakości

24

gotowego elementu. Uzyskiwana jest ogólnie wysoka jakość frezowanych powierzchni

przy pogorszeniu dokładności wykonania miejsc narożnych, w których obrabiarka

zatrzymuje frez pomiędzy kolejnymi segmentami toru ruchu.

Następnym przykładem interpolacji czasowej jest trygonometryczny profil

przyspieszenia [28]. Przykład tej interpolacji czasowej przedstawiono na Rysunku 5.

Z jego pomocą można wyznaczyć trajektorię o ciągłości klasy C2. Mimo iż funkcje

trygonometryczne należą do grupy funkcji o różniczkowalności C∞, w wyniku sklejania z

fazami ruchu jednostajnego przyspieszenia i prędkości klasa tego profilu jest

ograniczona do ciągłości drugiej pochodnej przemieszczenia. Podobnie jak w

przypadku profilu z trapezoidalnym przebiegiem przyspieszenia ten typ interpolacji

również składa się z siedmiu faz. Różnica polega na tym, iż w fazach, w których

oryginalnie przyspieszenie zmieniało się liniowo w czasie, zastąpiono typ zmienności na

zgodny z przebiegiem sinusoidalnym.

Rysunek 5 Przykład interpolacji czasowej z sinusoidalnym profilem przyspieszenia.

25

Fazy stałego przyspieszenia i prędkości występują analogicznie w obu przykładach

interpolacji czasowej. Sinusoidalny kształt profilu przyspieszenia wpływa korzystnie na

ograniczanie drgań wysokoczęstotliwościowych, jednak jego mankamentem jest duża

złożoność obliczeniowa, a w szczególności konieczność wyznaczania wartości funkcji

trygonometrycznych, co ma wpływ na szybkość działania algorytmu.

W publikacji Guoyong Zhao [29] opisano jeszcze inny sposób interpolacji czasowej,

w którym tworzy się gładki przebieg profilu przyspieszenia. Uzyskano to przez

wykorzystanie trapezoidalnego profilu dżerku, dzięki czemu generowany przebieg

przemieszczenia ma ciągłość klasy C3. Przykład profilu przyspieszenia wyznaczanego dla

przyjętych wartości maksymalnych zaprezentowano na Rysunku 6.

Rysunek 6 Przykład interpolacji czasowej z trapezoidalnym profilem dżerku.

By wytyczyć ten profil przyspieszenia przyjęto kolejny parametr:

Lmax = ±60m/s4 – maksymalna wartość udaru, czyli pochodnej dżerku po

czasie.

26

Gładkie zmiany przyspieszenia przyczyniają się do najlepszego ograniczenia drgań

wysokoczęstotliwościowych, spośród wymienionych metod interpolacji czasowej. Czas

wykonywania segmentu toru jest nieznacznie dłuższy od przypadku wyznaczonego za

pomocą interpolacji z trapezoidalnym profilem prędkości. Do minusów metody z

trapezoidalnym przebiegiem dżerku należy jej złożoność – pojedynczy segment składa

się z piętnastu faz cząstkowych. Wyznaczenie takiego segmentu wymaga również

bardziej skomplikowanych obliczeń, co ma wpływ na czas, w jakim mogą być

zrealizowane, a więc i na możliwą do osiągnięcia maksymalną częstotliwość generacji

trajektorii.

3.2. Interpolacja przestrzenna

Interpolacja przestrzenna odpowiada za odwzorowanie wymaganego kształtu toru

narzędzia w przestrzeni kartezjańskiej. Dzięki tej interpolacji wartości przemieszczenia,

prędkości i przyspieszenia wyznaczone z profilu czasowego są rzutowane na wszystkie

osie obrabiarki w taki sposób, by po zakończeniu wykonywania danego segmentu

narzędzie znalazło się w punkcie zdefiniowanym poprzez instrukcję G-kodu.

Oznaczenia osi obrabiarek i kierunki ruchów są określone w normie: PN-72/M-

55251:1993 [30]. W przypadku robota hybrydowego do frezowania interpolacja

przestrzenna rzutuje tor narzędzia na pięć osi. Trzy osie są osiami translacyjnymi

odpowiadającymi za położenie narzędzia w globalnym układzie współrzędnych,

pozostałe dwie osie odpowiadają za orientację narzędzia względem osi OX i OZ

globalnego układu współrzędnych. Interpolacja przestrzenna jest obliczana w trybie

on-line, więc wybierając możliwe kształty segmentów realizowane przez robota

hybrydowego należy wziąć pod uwagę konieczność szybkiego działania algorytmu

generacji trajektorii z daną interpolacją przestrzenną.

Pierwszym, podstawowym typem interpolacji przestrzennej jest interpolacja

liniowa. W języku programowania pracy obrabiarek za pomocą G-kodów ruch liniowy

oznacza się poleceniem G01. Ten typ interpolacji był wykorzystywany już przez

najstarsze obrabiarki numeryczne. Nawet najbardziej złożone kształty toru narzędzia

aproksymowano za pomocą odcinków liniowych [31, 32]. Ogromną zaletą tego

algorytmu jest jego prostota. Do jednoznacznego wytyczenia odcinka liniowego w

przestrzeni, mając dane współrzędne punktu, w którym aktualnie znajduje się

27

narzędzie, wystarczą współrzędne kartezjańskie punktu końcowego danego segmentu

wykonywanego z przestrzenną interpolacją liniową. Przykład przejazdu narzędzia z tym

typem interpolacji przestrzennej zaprezentowano na Rysunku 7. Wadą tego

rozwiązania jest wzrost koniecznej liczby poleceń G-kodu potrzebnych do realizacji

odwzorowania trajektorii krzywoliniowej za pomocą aproksymacji elementami

liniowymi – przy wykonywaniu dużych krzywizn konieczne jest wyznaczanie wielu

krótkich odcinków, by zachować dobrą dokładność wytwarzanego elementu.

Rysunek 7 Przejazd narzędzia z wykorzystaniem liniowej interpolacji przestrzennej.

Kolejnym typem interpolacji przestrzennej jest interpolacja kołowa [27]. Zamiast

przybliżać fragmenty krzywizn za pomocą odcinków liniowych, wytycza się wprost

trajektorię o kształcie łuku. Ten typ interpolacji jest określony w języku G-kodów

poleceniem G02 (dla obrotu zgodnie z ruchem wskazówek zegara) i poleceniem G03

(dla obrotu w przeciwnym kierunku). Do poprawnego wyznaczenia danego łuku w

przestrzeni konieczne jest podanie współrzędnych kartezjańskich punktu docelowego,

punktu środka łuku i płaszczyzny, na której łuk ma leżeć. Przykład przejazdu z tym

typem interpolacji przestrzennej zaprezentowano na Rysunku 8.

Do określenia aktywnej płaszczyzny używane są odpowiednie polecenia G-kodu:

G17 (płaszczyzną na której leży interpolowany łuk jest płaszczyzna równoległa do

płaszczyzny wyznaczonej przez osie X i Y globalnego układu współrzędnych), G18

(płaszczyzna równoległa do płaszczyzny wyznaczonej przez osie Z i X globalnego układu

współrzędnych) oraz G19 (płaszczyzna równoległa do płaszczyzny wyznaczonej przez

osie Y i Z globalnego układu współrzędnych).

28

Rysunek 8 Przejazd narzędzia z wykorzystaniem kołowej interpolacji przestrzennej w kierunku zgodnym z ruchem wskazówek zegara rzutowany na płaszczyznę XY.

Stosowanie tego typu interpolacji przestrzennej znacząco zmniejsza liczbę

koniecznych instrukcji G-kodu potrzebnych do odwzorowania skomplikowanych

kształtów. Za wadę można uznać ograniczoną do trzech liczbę płaszczyzn, w których

mieszczą się wykonywane okręgi, jednak w przypadku obrabiarki wieloosiowej ten

problem jest rozwiązywany przez zmianę orientacji obrabianego przedmiotu. W

robocie hybrydowym do frezowania takie zmiany orientacji również mogą być

wykonywane, a więc interpolacja kołowa jest realizowana w dowolnej płaszczyźnie.

Mankamentem interpolacji kołowej jest konieczność obliczania wartości funkcji

trygonometrycznych w każdym punkcie trajektorii interpolowanego segmentu. Funkcje

te pod kątem obliczeniowym należą do grupy operacji czasochłonnych, co też należy

wziąć pod uwagę przy dążeniu do minimalizacji czasu obliczeń w pojedynczym cyklu

układu sterowania.

Kolejnym z typów interpolacji przestrzennej jest wykorzystanie funkcji

wielomianowych wyższego rzędu w celu wytyczenia ciągłego toru narzędzia pomiędzy

zadanymi punktami określającymi trajektorię [22, 25, 33, 34]. Pomiędzy każde dwa

kolejne punkty trajektorii wpasowywana jest odpowiednia funkcja wielomianowa.

Parametry tej funkcji określa się na podstawie:

współrzędnych punktów w zadanej krzywej,

wektorów styczności w każdym z danych punktów w krzywej,

ciągłości funkcji w punktach zadanej krzywej.

29

Rysunek 9 przedstawia przykład krzywej wielomianowej dla dziewięciu punktów

rozmieszczonych w przestrzeni. W poniższym przykładzie wykorzystano wielomiany

piątego stopnia.

Rysunek 9 Przykład interpolacji przestrzennej z wykorzystaniem funkcji wielomianowych.

Zapewniają one ciągłość nie tylko przebiegu położenia, ale również wektory

chwilowych prędkości i przyspieszeń ruchu są takie same (w każdym z punktów dla obu

funkcji do których dany punkt należy). Równanie (1) przedstawia postać parametryczną

obliczanych wielomianów piątego rzędu:

(1)

gdzie:

– wartość chwilowa pojedynczej współrzędnej wyznaczona dla danego parametru

geometrycznego ,

– parametr geometryczny funkcji,

– współczynniki funkcji wielomianowej.

Dla każdej uwzględnianej zmiennej przestrzennej ruchu konieczne jest obliczenie

współczynników dodatkowej funkcji wielomianowej. Dla obrabiarek wieloosiowych

wiąże się to z przeprowadzaniem dużej ilości obliczeń numerycznych – w przypadku

robota hybrydowego do frezowania jest to pięć funkcji wielomianowych opisujących

pojedynczy segment wykonany z tą interpolacją przestrzenną. Do niewątpliwych zalet

30

stosowania tego typu interpolacji należy zaliczyć brak potrzeby wyhamowywania

pomiędzy poszczególnymi segmentami. Wytyczona trajektoria jest ciągła w każdym

punkcie pośrednim, nie licząc punktów skrajnych. Zwiększa to efektywność pracy

obrabiarki i poprawia jakość wyfrezowanego elementu.

Nowsze centra obróbcze mogą interpolować ścieżkę narzędzia wykorzystując tzw.

krzywe B-sklejane [19, 35, 36]. Ten typ interpolacji przestrzennej znalazł szczególne

zastosowanie w różnego typu programach graficznych przy projektowaniu złożonych

kształtów. Wraz z rozwojem programów CAM (Computer Aided Manufacturing) [37],

pozwalających automatycznie wytyczać ścieżki dla obrabiarek CNC, opracowano

algorytmy pozwalające obrabiarkom odwzorowywać krzywe B-sklejane wprost, bez

potrzeby aproksymacji skomplikowanych krzywych za pomocą odcinków liniowych.

Krzywe te są wykreślane na podstawie wielomianowych równań parametrycznych [38]

obliczanych na podstawie punktów kontrolnych. Formuła (2) przedstawia równanie

ogólne krzywej B-sklejanej:

(2)

gdzie:

– zdefiniowane punkty kontrolne,

– liczba punktów kontrolnych,

– stopień wielomianu funkcji bazowej,

– bazowa funkcja B-sklejana stopnia zdefiniowana na nieokresowym

(i niejednorodnym) wektorze węzłów :

(3)

Zmienna jest znormalizowana zazwyczaj do przedziału w którym i .

Ilość powtórzeń elementów i w wektorze węzłów jest zależna od stopnia

wielomianu, jaki przyjęto dla funkcji bazowej, i wynosi ona . Na Rysunku 10

zaprezentowano przykład toru narzędzia wyznaczonego z wykorzystaniem tego typu

interpolacji.

31

Rysunek 10 Przykład interpolacji przestrzennej z wykorzystaniem krzywych B-sklejanych, wykres czerwony - krzywa łącząca punkty kontrolne, wykres niebieski - krzywa B-sklejana.

Jeżeli rozkład wartości zmiennej jest różny od rozkładu jednorodnego, to w takim

przypadku krzywa B-sklejana nosi nazwę krzywej NURBS (ang. Non-Uniform Rational B-

Spline) [39]. Zaletą korzystania z tego typu interpolacji przestrzennej jest to, iż

ogranicza się liczbę poleceń G-kodu koniecznych do opisu linii o dużej krzywiźnie w

porównaniu do aproksymacji krzywą łamaną z wykorzystaniem odcinków liniowych.

Również dodatkową zaletą jest ciągłość funkcji o stopniu zależnym od stopnia

wielomianów funkcji bazowych. Do wad tej metody należy duża złożoność

obliczeniowa algorytmu. Wyznaczanie kolejnych punktów trajektorii wykonywane jest

metodą iteracyjną, co mimo wykonywania prostych operacji arytmetycznych sprawia,

że częstotliwość próbkowania, z jaką może być interpolowana trajektoria z

wykorzystaniem krzywych B-sklejanych, jest niższa niż w przypadku podstawowych

typów interpolacji przestrzennej. Dodatkowy problem pojawia się w przypadku

obróbki pięcioosiowej. W pracach [40, 41] Wang i Yuen opisują wyznaczanie trajektorii

dla obrabiarki CNC z wykorzystaniem dwóch osobnych krzywych B-sklejanych. Pierwsza

z nich opisuje zmianę pozycji, a druga krzywa zmianę orientacji narzędzia skrawającego

w przestrzeni roboczej. Wytyczenie kolejnej krzywej B-sklejanej podwaja konieczną

liczbę wykonywanych operacji matematycznych, co ma następstwo w dalszym

redukowaniu możliwej do osiągnięcia częstotliwości pracy Generatora Trajektorii.

32

3.3. Generacja trajektorii

Trajektoria opisuje położenie i orientację narzędzia w zależności od czasu. Jest ona

powiązaniem ze sobą interpolacji czasowej wraz z interpolacją przestrzenną.

Interpolację czasową opisują równania ruchu w postaci:

(4)

gdzie:

– funkcja przemieszczenia w czasie.

Z kolei interpolacja przestrzenna opisuje geometrycznie tor narzędzia uwzględniając

jego położenie i orientację w przestrzeni roboczej. Interpolację przestrzenną można

zdefiniować równaniem:

(5)

gdzie:

– wektor zawierający współrzędne położenia i orientacji danego punktu toru

ruchu w zależności od parametru geometrycznego wyrażone w globalnym układzie

współrzędnych,

– minimalna wartość argumentu funkcji,

– maksymalna wartość argumentu funkcji.

Tor ruchu, którym podąża narzędzie, jest wyznaczany w oparciu o więzy narzucone

przez typ interpolacji przestrzennej, zależy od punktów początkowych i pośrednich, na

bazie których oblicza się interpolację przestrzenną. Równania ruchu są określone przez

inne, często nakładające się więzy, takie jak wartości maksymalne prędkości,

przyspieszeń, sił lub momentów jakie napędy są w stanie wytworzyć. Przez połączenie

geometrycznego toru ruchu narzędzia (5) wraz z równaniami ruchu (4) otrzymuje się

równanie trajektorii w postaci:

(6)

gdzie:

– funkcja trajektorii narzędzia.

33

W zależności od rodzaju interpolacji przestrzennej argument funkcji jest inaczej

interpretowany. W przypadku interpolacji liniowej argument jest równy

przemieszczeniu liniowemu w danym segmencie, liczonym od punktu początkowego

tego przemieszczenia. Wartości graniczne wynoszą odpowiednio:

(7)

gdzie:

– długość odcinka liniowego.

Dla interpolacji kołowej argument jest przemieszczeniem po krzywiźnie

interpolowanego łuku. W tym przypadku oznaczać będzie długość łuku

interpolowanego w danym segmencie.

Sposób obliczania trajektorii z wykorzystaniem interpolacji krzywymi

wielomianowymi typu splajn, Erkorkmaz opisał w pracy [22]. Dla lepszej przejrzystości

metody przedstawiono obliczenia dla przemieszczenia tylko w dwóch osiach X i Y

globalnego układu współrzędnych (Rysunek 11).

Rysunek 11 Przykładowy tor ruchu narzędzia opisany za pomocą interpolacji krzywą typu splajn [22].

Interpolacji krzywej typu splajn nie można wprost połączyć z interpolacją czasową

tak, jak to miało miejsce w przypadku dwóch poprzednich przykładów interpolacji

34

przestrzennej. Dla interpolacji liniowej oraz kołowej możliwie prosto oblicza się stałą

proporcję pomiędzy przyrostem czasu a przyrostem przemieszczenia liniowego lub

kątowego. W przypadku interpolacji z wykorzystaniem krzywych wielomianowych

proporcja pomiędzy wspomnianymi przyrostami się zmienia, co zaprezentowano na

Rysunku 12.

Rysunek 12 Przebieg przemieszczenia wzdłuż krzywej wielomianowej w funkcji czasu [22].

Pierwszym krokiem do wyznaczenia trajektorii dla krzywej splajn jest obliczenie

przybliżonej długości tej krzywej:

(8)

gdzie:

– liczba segmentów cząstkowych w krzywej wielomianowej,

– długość i-tego segmentu cząstkowego.

By obliczyć przybliżoną długość krzywizny dla zadanego okresu próbkowania

szacowana jest liczba cykli, jaką zajmie realizacja -tego segmentu trajektorii z

interpolacją wielomianową (9):

(9)

gdzie:

– długość odcinka łączącego punkt początkowy i końcowy i-tego segmentu,

– zakładana prędkość liniowa,

– okres próbkowania Układu Sterowania.

35

Następnie wartość całki w równaniu (8) zostaje przybliżona sumą odcinków

wyznaczanych dla każdej kolejnej pary punktów pośrednich:

(10)

gdzie:

– współrzędne -tego punktu -tego segmentu krzywej wielomianowej.

Krzywa wielomianowa jest sparametryzowana względem długości odcinka

łączącego dwa kolejne punkty referencyjne, dlatego w idealnym przypadku należy

sparametryzować krzywą wielomianową względem długości krzywizny, co pozwoli na

połączenie tego typu interpolacji przestrzennej z interpolacją czasową w stałych

okresach próbkowania (Rysunek 13).

Rysunek 13 Porównanie dwóch sposobów próbkowania krzywej wielomianowej [22].

Przyrost długości krzywej wielomianowej obliczany jest za pomocą równań:

(11)

(12)

gdzie:

– długość krzywej wielomianowej,

36

– liniowa prędkość posuwu,

– okres próbkowania Układu Sterowania.

Korzystając z definicji równania krzywej wielomianowej (1) wyprowadzone zostały

wzory na przyrosty współrzędnych i :

(13)

gdzie:

– współczynniki wielomianu opisujące krzywą -tego segmentu dla

osi X globalnego układu współrzędnych,

– współczynniki wielomianu opisujące krzywą -tego segmentu dla

osi Y globalnego układu współrzędnych,

– parametr funkcji.

Z zależności oraz z równania (13) można obliczyć wartość

parametru , dla którego dla znanych współczynników przyrost długości

krzywej wyniesie . Poszukiwana wartość parametru jest pierwiastkiem wielomianu

dziesiątego stopnia:

(14)

Współczynniki od do wyznaczane są z układu równań:

(15)

37

gdzie:

,

,

Do obliczenia wartości miejsca zerowego wielomianu (14), można wykorzystać

metodę iteracyjną Newtona – Raphsona. Dodatkowo współczynniki od do nie

zmieniają się dla danego -tego segmentu, natomiast pozostałe współczynniki muszą

być obliczane dla każdego okresu próbkowania. Ta metoda pozwala na połączenie

interpolacji krzywych wielomianowych z wybraną interpolacją czasową.

Dla toru ruchu narzędzia z interpolacją krzywymi B-sklejanymi powiązanie z

interpolacją czasową jest również przypadkiem skomplikowanym. Piegl i Tiller w swojej

pracy [38] zaproponowali metodę wyznaczenia funkcji , która (dla danego

przemieszczenia ) pozwoli wyznaczyć wartość parametru geometrycznego funkcji

krzywej B-sklejanej (2). Tę metodę opisał również Yuen w publikacji [41]. Najpierw

należy wyznaczyć wszystkie brakujące elementy równania (2). Funkcje bazowe

wyznaczane są za pomocą równań:

(16)

(17)

gdzie:

– parametr funkcji,

– stopień wielomianu funkcji bazowej.

Dla wygenerowania trajektorii z wykorzystaniem krzywych B-sklejanych, w której

uzyskana będzie ciągła trzecia pochodna położenia, konieczne jest obliczenie funkcji

bazowych piątego stopnia ( ). Mając dane punktów

( ), przez które dana krzywa B-sklejana ma przechodzić,

koniecznym jest wyznaczenie wektora węzłów oraz współrzędne punktów

kontrolnych z równania (2). Do wyznaczenia wektora węzłów wstępnie wyznaczane

są wartości parametrów z równań:

38

(18)

gdzie:

– sumaryczna odległość krzywej łamanej łączącej punkty ,

– ilość zadanych punktów trajektorii.

Na podstawie obliczonych parametrów wyznaczany jest wektor węzłów z

równań:

(19)

gdzie:

– stopień wielomianu funkcji bazowej.

Mając obliczony wektor węzłów możliwym jest wyznaczenie odpowiednich wartości

funkcji bazowych z równania (16). Dodatkowo każdy zadany punkt trajektorii ma

przypisany parametr z równania (18), dzięki czemu możliwe jest wyznaczenie

współrzędnych punktów kontrolnych wykorzystując równanie macierzowe:

(20)

Następnie równanie (20) przekształcane jest do postaci:

(21)

39

Tym sposobem obliczane są współrzędne punktów kontrolnych , a z ich pomocą

możliwe jest obliczenie interpolacji przestrzennej krzywej B-sklejanej. Ponieważ

geometryczny parametr krzywej B-sklejanej nie zależy liniowo od przemieszczenia ,

konieczne jest wyznaczenie innej funkcji , która interpoluje parametr pozycji na

krzywej jako funkcję przemieszczenia . Przemieszczenie pomiędzy argumentami

i obliczane jest z równania:

(22)

Równanie (22) jest następnie rozwiązywane numerycznie. By estymować wartość

przemieszczenia użyto metody Simpsona z adaptacyjną metodą podziału:

(23)

gdzie:

.

Przedział jest dzielony na dwa równe segmenty i , po czym

przemieszczenia i są aproksymowane z użyciem równania (23). Dla

danej tolerancji sprawdzana jest nierówność dotycząca aproksymacji

przemieszczenia pomiędzy argumentami i :

(24)

gdzie:

– tolerancja aproksymacji długości krzywej.

Gdy nierówność (24) nie jest spełniona, przedział jest dzielony na dwa

segmenty i procedura aproksymacji jest powtarzana dla każdego z segmentów do

czasu, aż dla wszystkich segmentów będzie spełniona nierówność (24). W wyniku

opisanej procedury otrzymuje się szereg przedziałów z odpowiadającymi im

przemieszczeniami . Przemieszczenia pomiędzy przedziałami odpowiednio

40

sumuje się, otrzymując zestaw przemieszczeń oraz odpowiadające im parametry

geometryczne . Funkcję korekcji parametru geometrycznego w

postaci wielomianu dziewiątego rzędu, którą dopasowuje się do zestawów

przemieszczeń prezentuje równanie:

(25)

gdzie:

– przemieszczenie jako parametr funkcji,

– współczynniki wielomianu funkcji korekcji.

By uniknąć błędów w kondycjonowaniu, równanie (25) normalizowane jest z

użyciem całkowitego przemieszczenia do postaci:

(26)

gdzie:

– znormalizowany parametr funkcji korekcji,

– całkowite przemieszczenie wzdłuż krzywej B-sklejanej,

– znormalizowane współczynniki funkcji korekcji.

Równanie (26) zapisano w postaci macierzowej (27), a następnie współczynniki

wielomianu estymowano za pomocą metody najmniejszych kwadratów.

(27)

Błąd estymacji zdefiniowano jako:

41

(28)

gdzie:

– aktualny znormalizowany wektor parametru geometrycznego,

– estymowany wektor parametru geometrycznego.

Funkcją celu jest zminimalizowanie wartości :

(29)

Następnie wyznaczono pierwszą , drugą i trzecią pochodną parametrycznej

postaci krzywej B-sklejanej:

(30)

Dla funkcji korekcji (25) obliczono również te same pochodne:

(31)

Wykorzystując równania (30) i (31) oraz uwzględniając warunki brzegowe dla

oraz wyprowadzono następujące równanie macierzowe:

42

(32)

Jako wynik otrzymano następujący problem optymalizacyjny, który musi być

rozwiązany, by otrzymać współczynniki wielomianu korekcyjnego

z uwzględnieniem: (33)

który sprowadza się do liniowego problemu minimalizacji. Do funkcji minimalizacji

dodano wektor mnożników Lagrange’a oraz uwzględniono warunki brzegowe, co dało

nową funkcję celu:

(34)

gdzie:

– wektor mnożników Lagrange’a,

– uproszczona funkcja celu.

By obliczyć optimum funkcji celu (34) jej pochodne cząstkowe muszą być równe

zeru:

(35)

W wyniku otrzymano liniowy układ równań:

(36)

43

Ponieważ wszystkie segmenty są niezerowe, macierz jest pełnego rzędu, a więc

rozwiązanie równania (36) prowadzi do wyznaczenia znormalizowanych

współczynników . Współczynniki te po denormalizacji służą do obliczenia wartości

współczynników ( ) z równania (25). Tym sposobem wyznaczona zostaje

funkcja obliczająca przybliżoną wartość parametru geometrycznego w zależności od

przemieszczenia wzdłuż krzywej B-sklejanej. Na Rysunku 14 zaprezentowano wykresy

funkcji korekcyjnej dla dwóch iteracji przybliżających ostateczny wynik.

Rysunek 14 Wykresy funkcji korekcyjnej, wyznaczającej wartość parametru w funkcji przemieszczenia [41].

Wyznaczenie funkcji łączącej interpolację geometryczną wielomianową i B-sklejaną

z interpolacją czasową wymaga dużej ilości obliczeń oraz iteracyjnego rozwiązywania

równań metodami numerycznymi. Należy również wziąć pod uwagę iż przedstawione

przykłady tych dwóch metod należy rozszerzyć o uwzględnienie generowania

trajektorii w pięciu osiach. W związku z czym interpolacja wielomianowa i interpolacja

krzywymi B-sklejanymi nie nadają się do implementacji on-line z dużą częstotliwością

próbkowania.

44

4. Opracowany algorytm generacji trajektorii

W tym rozdziale zostanie dokładnie omówiony opracowany Generator Trajektorii.

Na początku opisano konstrukcję hybrydowego robota frezującego wraz z opisem

Układu Sterowania pod kątem wymagań, jakie stawiają przed Generatorem Trajektorii.

W następnym podrozdziale opisano opracowane podprogramy i ich algorytmy, które

pracują w części off-line. Mają one za zadanie przygotować parametry potrzebne do

generowania trajektorii w części on-line, którą opisano w ostatnim podrozdziale.

4.1. Frezarka hybrydowa o pięciu stopniach swobody

Opracowany w Katedrze Robotyki i Mechatroniki w AGH w Krakowie hybrydowy

robot do frezowania powstał z połączenia dwóch głównych elementów: równoległego

robota trójramiennego oraz z szeregowego stołu uchylno – obrotowego. Wygląd całej

konstrukcji zaprezentowano na Rysunku 15. Robot równoległy realizuje ruchy

translacyjne ruchomej platformy, w której zamontowane jest elektrowrzeciono.

Platforma ta jest połączona z konstrukcją za pomocą trzech ramion, zmiana ich

długości jest realizowana poprzez trzy liniowe napędy bezpośrednie. Położenie

biegnika napędu liniowego, a co za tym idzie i długości całego ramienia, jest mierzone

za pomocą optycznych przetworników impulsowych (enkoderów przyrostowych).

Zadaniem stołu uchylno – obrotowego jest ustawianie w pożądanej orientacji

zamocowanego na nim obrabianego przedmiotu. Stół zapewnia odchylanie się

przedmiotu wokół osi OX oraz jego obrót wokół osi OZ globalnego kartezjańskiego

układu współrzędnych. Obie osie obrotowe wyposażone są w optyczne przetworniki

obrotowo-kątowe (enkodery absolutne) pozwalające określić aktualne położenie bez

konieczności wcześniejszej kalibracji. Z punktu widzenia Układu Sterowania robot

równoległy oraz stół uchylno – obrotowy pracują niezależnie. Generator Trajektorii

zapewnia synchronizację pracy tych manipulatorów, co pozwala na realizację założonej

trajektorii narzędzia względem przedmiotu obrabianego. Generator musi uwzględniać

w obliczeniach lokalizację względną robota równoległego i stołu uchylno – obrotowego

oraz dodatkowo brać pod uwagę ewentualne korekty tej lokalizacji wynikające z

tolerancji wykonania i montażu frezarki. Instrukcje G-kodu zawierają informacje o

45

torze, jakim ma podążać narzędzie, zdefiniowanym w układzie współrzędnych

obrabianego przedmiotu.

Rysunek 15 Pięcioosiowy hybrydowy robot frezujący.

Generator Trajektorii ma za zadanie wyznaczyć trajektorię narzędzia w układzie

współrzędnych globalnych (kartezjański układ współrzędnych) związanym z podstawą

robota w taki sposób, by została poprawnie zinterpretowana przez Układ Sterowania.

Rysunek 16 przedstawia schemat funkcjonalny Układu Sterowania robota

hybrydowego do frezowania. Pierwszy z najważniejszych elementów tego systemu to

Panel Operatorski. Z jego pomocą użytkownik definiuje polecenia dla robota

hybrydowego oraz wprowadza program NC (kontrolny, ang. – Numerical Control)

zawierający opis toru narzędzia z użyciem instrukcji G-kodu. Służy on również do

wyświetlania stanu pracy robota frezującego. W Panelu Operatorskim wykonywana

jest również część off-line Generatora Trajektorii.

46

Rysunek 16 Schemat funkcjonalny Układu Sterowania.

Kolejną częścią Układu Sterowania jest jego Jądro (z ang. NCK - Numerical Control

Kernel). Ten ważny blok składa się z czterech mniejszych fragmentów: z Menadżera

Zadań, z Generatora Trajektorii, z Rozwiązania Zadania Kinematyki oraz ze Sterownika

Śledzenia Trajektorii. Menadżer Zadań odpowiada za przeprowadzenie procedury

kalibracji robota oraz czuwa nad tym, by robot nie wykonał przejazdu poza dozwolony

obszar przestrzeni roboczej. Generator Trajektorii na bieżąco (część on-line) oblicza

położenie, prędkość i przyspieszenie zadane dla robota równoległego i dla stołu

uchylno – obrotowego we współrzędnych globalnych. Wartości te są następnie

transformowane do współrzędnych złączowych za pomocą rozwiązania zadania

odwrotnego kinematyki. Sterownik Śledzenia Trajektorii odpowiada za obliczenie

wartości zadanych momentów i sił, które stanowią wejście Komutatorów Napędów

odpowiedzialnych za generację zadanych sił i momentów przez napędy robota

hybrydowego. Dodatkowo ze Sterownika Śledzenia Trajektorii otrzymywana jest

informacja o wskazaniach enkoderów, co pozwala na wyznaczenie aktualnego

•Generacja programu NC

•Wgrywanie programu dla obrabianego elementu

•Wykonywanie zadań części off - line Generatora trajektorii

Panel Operatorski

•Menadżer Zadań - Kalibracja, - Detekcja i unikanie kolizji,

•Generator Trajektorii - Interpolacja czasowa - Interpolacja przestrzenna

•Rozwiązanie Zadania Prostego i Odwrotnego Kinematyki Manipulatora

•Sterownik Śledzenia Trajektorii - Model dynamiczny, - Regulator

Jądro Układu

Sterowania

•Regulacja Momentów i Sił

•Komutacja uzwojeń stojanów i pędników napędów

Komutatory Napędów

47

położenia zarówno robota równoległego, jak i stołu uchylno – obrotowego. Wartości

aktualnego położenia są konieczne dla prawidłowej pracy Generatora Trajektorii.

W celu zwiększenia wydajności obliczeniowej Generator Trajektorii został

podzielony na dwie części: off-line i on-line. W pierwszej z nich wykonywane są

operacje arytmetyczne czasochłonne obliczeniowo i wymagające do działania

algorytmu danych wyznaczonych dla poprzednich segmentów toru, a nie tylko dla

aktualnego segmentu. Są one jednak wykonywane tylko raz dla danej linii G-kodu, więc

operacje te nie muszą być obliczane z częstotliwością próbkowania Sterownika

Śledzenia Trajektorii. Dlatego też ta część jest obliczana w Panelu Operatorskim, który

przesyła do Generatora Trajektorii dane konieczne do wygenerowania pożądanej

trajektorii. W części on-line kluczowym jest, by była ona obliczana na tyle szybko, aby

sprostać wymaganej częstotliwości próbkowania Sterownika Śledzenia Trajektorii oraz

by pełna trajektoria (zawierająca wartości położeń, prędkości i przyspieszeń dla każdej

osi robota hybrydowego do frezowania) była wyznaczana z uwzględnieniem wartości

granicznych narzuconych przez konstrukcję robota lub parametry procesu obróbczego.

W dalszej części rozdziału opisano szczegółowo algorytmy programów wchodzących w

skład obu części Generatora Trajektorii.

4.2. Część off-line Generatora Trajektorii

Program działający w trybie off-line jest częścią Generatora Trajektorii służącą

analizie pliku tekstowego zawierającego instrukcje G-kodu oraz przygotowaniu danych

potrzebnych do działania części on-line. Czas, w jakim część off-line zakończy działanie,

jest uzależniony od długości pliku tekstowego zawierającego instrukcje G-kodu. Na

samym początku części off-line otwierany jest plik, którego nazwę podaje operator,

następnie uruchamiana jest pętla (Rysunek 17) trwająca do czasu przeanalizowania

całego pliku tekstowego. Analizowanie instrukcji G-kodu następuje linia po linii.

Pierwsza uruchamiana zostaje funkcja Interpretatora, która jest odpowiedzialna za

przetłumaczenie poleceń tekstowych na wektor wartości liczbowych odpowiednich

parametrów.

48

Rysunek 17 Schemat blokowy operacji w trybie off-line.

Najczęściej występującymi poleceniami G-kodu są instrukcje zlecające określoną

interpolację przestrzenną segmentu toru ze współrzędnymi oznaczającymi pozycję i

orientację narzędzia, jakie ma osiągnąć na końcu tego segmentu toru. Dodatkowe

instrukcje pozwalają na modyfikację parametrów obróbczych, takich jak prędkość

posuwu czy prędkość obrotowa frezu, a także nadzór nad procesem frezowania (np.

włączanie chłodziwa w trakcie obróbki). Może zaistnieć sytuacja, w której ulegnie

zmianie zamocowanie przedmiotu lub G-kod będzie przygotowany dla innego układu

współrzędnych niż układ globalny frezarki. W tych przypadkach po etapie Interpretacji

następuje faza Kompensacji różnicy w położeniach pomiędzy układem współrzędnych

związanych z obrabianym przedmiotem oraz globalnym układem współrzędnych

robota hybrydowego do frezowania. Operator podaje odchyłkę dla współrzędnych, w

których występuje różnica położeń, a program kompensacyjny oblicza skorygowane

START Wczytanie linii

kodu Interpteracja instrukcji G

– kodu

Kompensacja położenia Styczność segmentów

Wyliczenie profilów przyspieszenia

Korekta prędkości

Zapis przetworzonych danych

Koniec pliku tekstowego?

STOP

Nie

Tak

49

współrzędne punktów w układzie współrzędnych globalnych robota. Następny etap

analizy danych to wyszukiwanie w analizowanym torze ruchu segmentów, które są

styczne z sąsiednimi segmentami i mogą być zrealizowane bez wyhamowania

pomiędzy nimi. Pozwala to skrócić czas realizacji całego procesu frezowania i

wykonywać przejazdy narzędzia ze stałą prędkością posuwu po torze krzywych

wielomianowych lub krzywych B-sklejanych, które operator aproksymował w G-kodzie

krzywą łamaną zawierającą odcinki liniowe i łuki. Gdy ta procedura dobiegnie końca, w

zależności od tego, czy segment jest styczny z sąsiednimi czy nie, następuje ustawienie

wartości początkowej i końcowej chwilowej prędkości liniowej, dla których algorytm

ma wyznaczyć profil przyspieszenia. Poza wymienionymi prędkościami granicznymi do

obliczenia profilu konieczne jest znalezienie dystansu, jaki pokonuje narzędzie w

danym segmencie toru. Dla interpolacji liniowej jest to długość odcinka pomiędzy

punktami w przestrzeni, dla interpolacji kołowej - długość łuku na płaszczyźnie. Gdy te

wartości zostaną obliczone, następuje wyznaczanie profilów przyspieszenia. Może się

zdarzyć, iż dla ostatniego segmentu trajektorii ciągłej (po obliczeniu profilów

przyspieszeń) początkowa prędkość posuwu, od jakiej narzędzie musi wyhamować z

ograniczonym dżerkiem, jest za duża. W takim przypadku program identyfikuje

segmenty, których profile przyspieszeń musi skorygować – obliczyć na nowo z

uwzględnieniem zredukowanych prędkości końcowych, dzięki czemu zostaje

zachowana ciągłość chwilowej prędkości liniowej. Gdy wszystkie konieczne wartości

zostaną obliczone, następuje ich zapis do pamięci. Cała procedura jest realizowana dla

każdej z instrukcji G-kodu. Gdy program wykryje, iż plik tekstowy nie zawiera już

więcej instrukcji, to do pamięci dodana zostaje linia oznaczająca zakończenie programu

obróbczego. Poniżej opisano szczegółowo kolejne etapy części off-line algorytmu.

4.2.1. Interpretacja G-kodu

W tym etapie zostaje poddana analizie każda z linii tekstu wczytana z pliku

zawierającego program obróbki zapisany za pomocą poleceń G-kodu. Jako efekt

działania funkcji uzupełniana zostaje tablica wartości, jakie są identyfikowane w pliku

tekstowym. Schemat blokowy działania funkcji Interpretacji przedstawiono na Rysunku

18. Na początku parametry z analizy poprzedniej linii zostają skopiowane do kolejnego

wiersza w tablicy, gdyż wartości, które nie są w danej linii modyfikowane, pozostają

50

bez zmian. Jeśli algorytm natrafi w tekście na którykolwiek ze znaków: "(", "/" ,"%" lub

"↲" to dalszy tekst interpretuje jako koniec linii lub komentarz i przerywa analizę

przechodząc do kolejnej linii pliku tekstowego.

Rysunek 18 Schemat blokowy operacji interpretowania polecenia G-kodu.

G-kod jest językiem, w którym dane są zapisywane w postaci słów. Każde słowo

składa się z oznaczenia literowego (tzw. adresu) i z wartości numerycznej. W programie

przewidziano rozpoznanie następujących oznaczeń literowych:

G – funkcje przygotowawcze,

M – funkcje maszynowe,

S – zmiana prędkości obrotowej wrzeciona,

F – zmiana prędkości liniowej posuwu,

T – wybór narzędzia,

P – czas postoju w danej pozycji,

Linia G-kodu

Czy już koniec linii?

Odnaleziono słowo?

Rozpoznanie znaku

Odczytanie wartości

Tak

Nie

Nie

Zapis do pamięci

Koniec pod - programu

51

X, Y, Z – położenie w osiach "X", "Y" i "Z" globalnego układu współrzędnych,

I, J, K – położenie środka obrotu względem aktualnego punktu odpowiednio w

kierunkach osi "X", "Y" i "Z",

A, C – rotacja wokół osi "X" i "Z" globalnego układu współrzędnych.

Po rozpoznaniu adresu algorytm odczytuje wartość numeryczną polecenia i

modyfikuje odpowiadającą adresowi wartość w tablicy zgodnie z odczytaną wartością

numeryczną. Najczęściej interpretowanymi parametrami są współrzędne końców

kolejnych segmentów tworzących tor ruchu narzędzia. Dodatkowo program skaluje

wartości współrzędnych przez stałą związaną z używanymi w pliku jednostkami. Można

je modyfikować specjalnymi instrukcjami G-kodu. W ten sposób wartości położeń

wykorzystywane przez algorytm w dalszych etapach zawsze są wyrażone w metrach.

Dodatkowym etapem pracy funkcji Interpretacji jest wyznaczenie aktywnej płaszczyzny

roboczej (rolę tych płaszczyzn opisano w rozdziale 3.2). Jest to konieczne dla

poprawnej analizy współrzędnych środka okręgu przy interpretowaniu ruchów

narzędzia z interpolacją kołową. Domyślną płaszczyzną, w której realizowany jest ten

typ interpolacji, jest płaszczyzna równoległa do płaszczyzny XY. Dopóki algorytm nie

rozpozna polecenia G-kodu zmieniającego aktywną płaszczyznę na inną, każdy

segment wykorzystujący interpolację kołową będzie wykonywany w płaszczyźnie

równoległej do płaszczyzny XY globalnego układu współrzędnych. Algorytm przypisuje

więc odpowiednią wartość w oznaczeniu typu interpolacji uwzględniającą płaszczyznę,

w której wykonywany jest dany segment. Ostatni etap analizy tekstu to sprawdzenie,

czy nie zostało wywołane polecenie G81 oznaczające tryb wiercenia otworów – jeśli

tak, dodany zostaje dodatkowy wiersz w przygotowywanej tablicy danych

zapewniający ruch powrotny narzędzia do pozycji, z której rozpoczynano wiercenie.

4.2.2. Kompensacja położenia

Na tym etapie algorytm dokonuje korekty współrzędnych pozycji i orientacji toru

narzędzia w zależności od różnicy w położeniu i orientacji układów współrzędnych:

globalnego robota hybrydowego i układu współrzędnych związanego z obrabianym

przedmiotem. Dodatkowo uwzględniono możliwość kompensacji ewentualnych

tolerancji montażowych w przypadku różnicy w orientacji stołu uchylno – obrotowego i

52

robota równoległego wokół osi OY. Różnica ta nie może być skompensowana za

pomocą żadnego z napędów robota. Pozostałe wartości, które należy skorygować,

wprowadza użytkownik za pomocą Panelu Operatorskiego. Zmianę orientacji wykonuje

się poprzez bezpośrednie dodanie do aktualnych wartości położeń kątowych

parametrów korygujących. Obliczenie położenia skorygowanego wymaga bardziej

złożonych operacji, ponieważ konieczne jest uwzględnienie zmiany korekty przy

ewentualnym obrocie stołu uchylno – obrotowego. Globalny układ współrzędnych

frezarki (XS, YS, ZS) umiejscowiony jest w centrum stołu uchylno – obrotowego (Rysunek

19). Instrukcje opisane za pomocą G-kodu podają współrzędne pozycji narzędzia

frezującego w przypadku idealnego zamocowania w układzie współrzędnych

związanym z obrabianym przedmiotem.

Rysunek 19 Schematyczne rozmieszczenie układów współrzędnych związanych z robotem równoległym (XR, YR, ZR), stołem uchylno – obrotowym (XS, YS, ZS) i obrabianym przedmiotem (XP, YP, ZP).

Zadaniem algorytmu kompensacyjnego jest przetransponowanie współrzędnych

położenia związanych z układem współrzędnych przedmiotu do nowego układu

współrzędnych związanego z robotem hybrydowym uwzględniając przy tym tolerancje

montażowe konstrukcji robota. Aby wyznaczyć współrzędne punktów opisanych za

pomocą G-kodu w globalnym układzie współrzędnych robota, posłużono się

macierzami przekształceń jednorodnych (37) dla przesunięć oraz dla obrotów układów

współrzędnych [42]:

ZS YS

S XS

ZR YR

XR ZP

YP

XP

AS

CS

CP

AP

53

, (37)

gdzie:

– przesunięcie o wartość wzdłuż osi X,

– przesunięcie o wartość wzdłuż osi Y,

– przesunięcie o wartość wzdłuż osi Z,

– obrót o kąt o wartości wokół osi 0X,

– obrót o kąt o wartości wokół osi 0Y,

– obrót o kąt o wartości wokół osi 0Z.

Macierze (37) pozwalają obliczyć macierz przekształcenia jednorodnego

(zawierającą wartości położenia wraz z orientacją) układu współrzędnych związanego z

obrabianym przedmiotem, obliczonego względem bazowego układu współrzędnych.

Macierz przekształcenia jednorodnego obliczono posługując się równaniem (38):

(38)

gdzie:

– współrzędne przemieszczenia wrzeciona odczytane z instrukcji G-kodu,

– wartości kątów obrotu napędów obrotowych A i C odczytane z instrukcji G-

kodu,

– wartości kompensacyjne rotacji w trzech osiach globalnego układu

współrzędnych zmierzone przez operatora podczas montażu obrabianego przedmiotu.

Kolejność przekształceń jednorodnych w równaniu (38) wynika z łańcucha

kinematycznego stołu uchylno – obrotowego. Wykorzystując macierz przekształcenia

jednorodnego oraz współrzędne punktów toru narzędzia otrzymanych z G-kodu,

54

obliczane są współrzędne tych punktów przetransponowanych do układu

współrzędnych związanego z stołem uchylno – obrotowym:

(39)

(40)

(41)

gdzie:

– wartości kompensacyjne przemieszczenia dla trzech osi globalnego

układu współrzędnych, wyznaczone przez operatora podczas montażu obrabianego

przedmiotu,

– współrzędne punktu toru ruchu w układzie współrzędnych stołu uchylno

– obrotowego .

Kompensacja uwzględniająca zmianę pozycji układu współrzędnych przedmiotu

odbywa się dwuetapowo. W pierwszym etapie wyznaczane są współrzędne punktów

toru ruchu z G-kodu względem stołu uchylno – obrotowego (Równania (39)(41)).

Następnie dokonywane jest przesunięcie obliczonych współrzędnych o zadane

parametry kompensacyjne ( ) pomiędzy środkami układów współrzędnych:

stołu uchylno – obrotowego i tego związanego z obrabianym przedmiotem. W drugim

etapie wykonywana jest odwrotna transformacja punktów o dane wartości rotacji

wokół osi obrotu stołu uchylno – obrotowego. Wykorzystano tu fakt, iż macierz

przekształcenia jednorodnego jest macierzą ortogonalną, a co za tym idzie można

wyznaczyć jej macierz odwrotną [42]. Macierz odwrotna pozwala

transponować punkty wyrażone w układzie współrzędnych stołu uchylno –

obrotowego do globalnego układu współrzędnych robota hybrydowego:

55

(42)

(43)

(44)

gdzie:

– współrzędne położenia narzędzia w globalnym układzie współrzędnych

z uwzględnieniem kompensacji pozycji montażu przedmiotu obrabianego.

Równania (39) (44) pozwalają obliczyć współrzędne punktów trajektorii z

uwzględnieniem nie tylko zmiany lokalizacji mocowania przedmiotu obrabianego, ale

również wprowadzają możliwość kompensacji ewentualnego błędu montażu

konstrukcji stołu uchylno – obrotowego i robota równoległego.

4.2.3. Wykrywanie styczności segmentów

Wysoką jakość powierzchni podczas frezowania można otrzymać, gdy narzędzie

wykonuje płynne ruchy. Najkorzystniej jest, gdy chwilowa prędkość liniowa posuwu

jest stała. Można to uzyskać, kiedy cały tor narzędzia jest gładki. W praktyce tor składa

się z wielu gładkich segmentów łączących się ze sobą bez zachowania gładkości toru w

punktach połączeń. Niemożliwa do wykonania jest natychmiastowa zmiana kierunku

ruchu podczas utrzymywania stałej prędkości posuwu narzędzia. By to wykonać,

należałoby przyłożyć nieskończoną siłę lub moment w napędach użytych do zmiany

kierunku, dlatego też Układ Sterowania musi wyhamować i zatrzymać narzędzie

dokładnie w punkcie załamania toru. Następnie narzędzie jest rozpędzane do

właściwej prędkości posuwu podczas wykonywania kolejnego segmentu toru. Przez to

trajektoria gładka jest wykonywana szybciej od łamanych segmentów. W praktyce,

jeżeli załamanie jest bardzo małe, segmenty traktuje się jako styczne (o gładkim

przejściu).

W tym podrozdziale zostanie przedstawiona metoda klasyfikowania przejść między

segmentami jako gładkich. Podstawowy wyznacznik to styczność w miejscu połączenia

56

dwóch segmentów ścieżki narzędzia. Przejście między dwoma segmentami

klasyfikowane jest jako gładkie, gdy są one styczne (z dokładnością do parametru

deklarowanego przez Operatora) i nie zmienia się zwrot ruchu narzędzia.

Rysunek 20 Fragment toru narzędzia zawierający gładkie połączenia segmentów.

Na Rysunku 20 zaprezentowano przykład dwóch gładkich fragmentów toru

narzędzia. Pierwszy rozpoczyna się w punkcie 2 i kończy w punkcie 5. Drugi fragment

leży pomiędzy punktami 6 i 8. W punkcie 6 dwa segmenty toru są do siebie styczne.

Jeżeli algorytm klasyfikowałby przejście między segmentami tylko na podstawie ich

styczności, wtedy w punkcie 6 tor narzędzia byłby oznaczony jako gładki. Taka sytuacja

może doprowadzić do uszkodzenia frezowanego elementu lub narzędzia, dlatego

zwrot frezowania musi być również brany pod uwagę przy identyfikowaniu gładkich

fragmentów trajektorii.

Do analizy styczności brane pod uwagą są każdorazowo dwa sąsiednie segmenty.

Sprawdzana jest możliwość kontynuowania przejazdu pomiędzy nimi bez redukowania

chwilowej prędkości liniowej narzędzia. Jeżeli dla danego przejścia wykryto zmianę

położenia kątowego któregokolwiek z napędów w stole uchylno – obrotowym, to to

przejście między segmentami automatycznie jest oznaczone jako „niestyczne”.

Przyczyną tego jest fakt, iż przy przejeździe torem stycznym, gdy np. robot równoległy

kontynuuje ruch a stół uchylno – obrotowy musiałby rozpocząć ruch, niemożliwym jest

natychmiastowe rozpędzenie napędów stołu tak, by nie utracić synchronizacji ruchu.

Pierwszym z etapów wyznaczenia styczności sąsiednich segmentów toru ruchu

narzędzia jest obliczenie współrzędnych wektorów jednostkowych stycznych do toru

ruchu w punktach początkowym i końcowym poszczególnych segmentów. Dla

segmentu liniowego wektory jednostkowe będą identyczne, zaś w przypadku

1 2

1

3

1

4

1

5 6

7 8

57

interpolacji kołowej taki wariant jest możliwy jedynie gdy segment ma kształt pełnego

okręgu. Przykładową reprezentację wektorów jednostkowych dla segmentu liniowego i

łukowego przedstawiono na Rysunku 21.

Rysunek 21 Przykład wyznaczania wektorów jednostkowych stycznych do toru dla segmentów z interpolacją liniową i kołową.

Kolejnym etapem jest obliczenie z równania (45) długości wektora różnicy

pomiędzy wektorem jednostkowym ruchu dla punktu końcowego jednego segmentu

a wektorem jednostkowym ruchu dla punktu początkowego segmentu następnego

.

(45)

gdzie:

– wyznaczana długość wektora ,

– wektor jednostkowy styczny do toru ruchu w punkcie początkowym -tego

segmentu,

– wektor jednostkowy styczny do toru ruchu w punkcie końcowym -1-ego

segmentu.

Jeśli długość jest mniejsza od parametru granicznego zadanego przez

użytkownika, to segmenty są uznawane za styczne. Na koniec jest jeszcze dokonywane

oznaczenie, czy dany segment rozpoczyna fragment trajektorii gładkiej, kończy etap

gładki lub też jest segmentem pośrednim, w którym prędkość liniowa posuwu nie

maleje do zera. W zależności od tego odpowiedni profil przyspieszenia zostanie

obliczony dla danego segmentu.

P0(x0,y0,z0)

P1(x1,y1,z1) Z

Y

X

Z

Y

X

w1

w0 w0=w1

Z

Y

X

Z

Y

X

w0 w1

P0(x0,y0,z0)

P1(x1,y1,z1) w0

w1

58

Rysunek 22 Schemat działania algorytmu klasyfikującego przejścia pomiędzy segmentami jako gładkie.

Współrzędne dwóch

segmentów

Typ interpolacji przestrzennej?

Tak

Tak Nie

Nie

Pierwszy segment rozpoczyna

trajektorię gładką

Koniec pod - programu

Wyznaczenie wektorów

jednostkowych

Obliczenie długości

wektora

Wyznaczenie wektorów

jednostkowych

Liniowa Łukowa

Poprzednie przejście było

gładkie?

Zmiana położenia

kątowego?

Poprzednie przejście było

gładkie?

Tak Nie Tak

Pierwszy segment kontynuuje

trajektorię gładką

Pierwszy segment nie należy do

trajektorii gładkiej

Pierwszy segment kończy trajektorię

gładką

Nie

Zapis do pamięci

Analizowano ostatni

segment?

Tak Nie

59

Rysunek 22 przedstawia schemat działania opisanego powyżej algorytmu służącego

do wykrywania styczności pomiędzy segmentami toru narzędzia.

4.2.4. Wyznaczanie profilów przyspieszenia

Etapem wstępnym koniecznym do wyznaczania profilów przyspieszenia jest

obliczenie długości odcinka lub łuku, po którym ma odbywać się ruch w danym

segmencie. Dotyczy to również przemieszczeń w stole uchylno – obrotowym, przy

czym przemieszczenia te są wyrażone w mierze kątowej. Dodatkowo Generator

Trajektorii wyznacza najkrótszą drogę dla obrotu C wokół osi 0Z – obrót ten ma

nieograniczony zakres ruchu i przejazd np. od do

powinien być zrealizowany przez

obrót o

a nie o

. Pozwala to zmniejszyć czas pracy robota hybrydowego, a co za

tym idzie zwiększyć jego efektywność produkcyjną. W tym etapie generator wyznacza

również cztery parametry kierunkowe, które będą konieczne do odtworzenia

trajektorii w trybie on-line. Jeśli analizowany segment jest wykonywany z interpolacją

kołową, to używane są następujące parametry kierunkowe: trzy pierwsze parametry

odpowiadają za położenie środka obrotu wyznaczonego względem początkowego

punktu segmentu, czwarty parametr to wartość kąta początkowego, od którego ma

rozpocząć się wykonywanie okręgu. Dla odcinka liniowego tymi parametrami są dwa

kąty α i β definiujące usytuowanie wektora jednostkowego kierunku ruchu: kąt α jest

wyznaczany na płaszczyźnie XY od dodatniego kierunku osi X, oraz kąt β obliczany jako

kąt nachylenia wektora do płaszczyzny XY – zgodnie ze sferycznym układem

współrzędnych (Rysunek 23a). Kolejny kąt γ definiuje w analogiczny sposób wektor

jednostkowy przemieszczenia kątowego stołu.

a)

b)

Rysunek 23 Graficzna ilustracja sposobu wyznaczania parametrów kierunkowych dla liniowej interpolacji przestrzennej w przypadku przemieszczenia liniowego (a) i przemieszczenia kątowego (b).

C [rad] Z [m]

Y [m]

β

A [rad]

γ

X [m]

α

60

Ponieważ przemieszczenie kątowe realizowane jest w co najwyżej dwóch osiach

obrotu, do poprawnego zdefiniowania kierunku tego przemieszczenia wystarczy podać

wartość jednego kąta (Rysunek 23b). Kąt γ określa położenie wektora jednostkowego

rotacji w układzie współrzędnych przemieszczeń kątowych AC, licząc od dodatniego

kierunku osi A. Dodatkowo zostaje wyznaczony kąt , którego tangens określa

stosunek przemieszczenia liniowego do przemieszczenia kątowego. Wartość kąta

wyznaczono z równania (46):

(46)

gdzie:

– przemieszczenie liniowe w danym segmencie,

– przemieszczenie kątowe w danym segmencie.

Obliczenie tego kąta pozwala na uzyskanie synchronicznego ruchu przy realizacji

pojedynczego segmentu z wykorzystaniem robota równoległego i stołu uchylno –

obrotowego. Na podstawie wyznaczonego dla danego segmentu kąta oraz wartości

granicznych prędkości i przyspieszeń dla obu manipulatorów robota hybrydowego

obliczane zostają zastępcze wartości przemieszczenia i jego dwóch kolejnych

pochodnych.

a)

b)

c)

Rysunek 24 Graficzna ilustracja sposobu wyznaczania zastępczych wartości przemieszczenia (a), prędkości (b)

i przyspieszenia (c) dla ruchu synchronicznego robota równoległego i stołu uchylno – obrotowego.

Tę heurystyczną metodę zaprezentowano na Rysunku 24. Rysunek 24a przedstawia

sposób graficznej interpretacji kąta oraz jego wyznaczania w zależności od

Przemieszczenia liniowego oraz przemieszczenia kątowego

Prę

dko

ść

linio

wa

[m/s

]

Prz

emie

szcz

enie

linio

we

[m]

Przemieszczenie kątowe [rad]

sTr

sRot

Prędkość kątowa [rad/s]

vTmax

vRmax

Prz

ysp

iesz

enie

linio

we

[m/s

2 ]

Przyspieszenie kątowe [rad/s2]

aTmax

aRmax

61

narzędzia w globalnym układzie współrzędnych. Jako wartość zastępczą

przemieszczenia przyjmuje się długość wektora . Na rysunku 24b zaprezentowano

sposób wyznaczania prędkości zastępczej.

Rysunek 25 Schemat działania algorytmu wyznaczającego zastępcze wartości przemieszczenia, prędkości i przyspieszenia.

Linia

Łuk

Obliczenie przemieszczenia całkowitego oraz kąta

Obliczenie przemieszczenia

liniowego

Obliczenie przemieszczenia kątowego

Wartości maksymalne

Tak Nie

Obliczenie prędkości kątowej

Parametry danego

segmentu

Obliczenie wstępne przyspieszenia liniowego

Obliczenie

Obliczenie wstępne prędkości liniowej

Tak Nie

Obliczenie przyspieszenia kątowego

Obliczenie

Koniec pod - programu

Tak Nie

Analizowano ostatni

segment?

Zapis do pamięci

62

Przy zachowaniu wartości kąta wyznaczany jest wektor , którego długość, a

zarazem i wartość prędkości zastępczej, jest ograniczona przez wartości maksymalnych

prędkości dla poszczególnych manipulatorów: – prędkość posuwu definiowana

za pomocą poleceń G-kodu i – maksymalna prędkość kątowa, jaką osiąga stół

uchylno – obrotowy. Analogicznie wyznaczana jest wartość zastępcza przyspieszenia

jako długość wektora (Rysunek 24c). Schemat działania algorytmu wyznaczania

wartości zastępczych przedstawiono na Rysunku 25. Zastosowanie tej metody

pozwoliło dla każdego możliwego wariantu ruchu wyznaczyć możliwie najwyższe

wartości graniczne prędkości i przyspieszenia, jakie mają być uwzględnione przy

realizacji danego segmentu.

Rysunek 26 Profil przyspieszenia z podziałem na siedem faz dla jednego segmentu.

Na podstawie obliczonych w ten sposób wartości zastępczych wyznaczane są czasy

trwania siedmiu faz w profilu przyspieszenia (Rysunek 26). Jak to opisano w Rozdziale

4.3.1, jako sposób interpolacji czasowej przyjęto interpolację z trapezoidalnym

przebiegiem przyspieszenia. Przyjęto też założenie, że czas trwania fazy pierwszej f1

(47) oraz trzeciej f3 są takie same, analogicznie długości faz piątej f5 i siódmej f7

również są sobie równe.

(47)

gdzie:

– przyspieszenie zastępcze obliczone jako długość wektora ,

– maksymalna wartość dżerku.

Jeśli maksymalne wartości przyspieszenia podczas przyspieszania i hamowania (przy

założonym stałym poziomie dżerku) są takie same, to faza piąta jest równa co do

f1 f2 f3 f4 f5 f6 f7

63

długości fazie pierwszej: f1 = f3 = f5 = f7. Następnie obliczane są czasy trwania faz drugiej

f2 (48) i szóstej f6 (49):

(48)

(49)

gdzie:

– prędkość zastępcza posuwu wyznaczona jako długość wektora vzast,

– wartość prędkości posuwu na początku segmentu,

– wartość prędkości posuwu na końcu segmentu.

Jak można zauważyć, gdy prędkości liniowe na obu końcach segmentu są równe,

czasy trwania etapów drugiego i szóstego również będą identyczne. Ostatni nieznany

etap oblicza się z równania (50):

(50)

gdzie:

– obliczone przemieszczenie zastępcze jakie ma do pokonania narzędzie robota

hybrydowego do frezowania wyznaczone jako długość wektora .

4.2.5. Korekta prędkości liniowej

Po wyznaczaniu profilów przyspieszeń konieczne jest sprawdzenie, czy w

segmentach trajektorii, które zostały zakwalifikowane przez algorytm wykrywania

styczności opisany w rozdziale 4.2.3 jako segmenty o gładkich przejściach, chwilowa

prędkość liniowa również ma przebieg gładki. Może zaistnieć sytuacja, gdy w ostatnim

segmencie trajektorii gładkiej (Seg2 na Rysunku 27) dystans, jaki ma przejechać

narzędzie, jest zbyt krótki, by wyhamować jego ruch z początkowej prędkości posuwu z

64

ograniczonym dżerkiem. W takim przypadku dokonywana jest korekcja końcowej

prędkości segmentu poprzedniego. Ponownie wyznaczany jest profil przyspieszenia

segmentu wcześniejszego (Seg1 na rysunku 27) z uwzględnieniem niższej wartości

chwilowej prędkości liniowej, na której generowana trajektoria ma zakończyć

korygowany segment.

Rysunek 27 Przykład korekcji prędkości posuwu; linia przerywana – przed korekcją, linia ciągła – po korekcji.

Po wykonaniu tych czynności parametry konieczne do generowania trajektorii w

trybie on-line zostają zapisane w pamięci, a następnie analizowana jest kolejna partia

pliku tekstowego zawierająca polecenia G-kodu. Na rysunku 28 przedstawiono

schemat blokowy opisanego algorytmu do korekcji prędkości. Po zakończeniu pracy

tego podprogramu zostają zapisane w pamięci (w postaci tablicy danych) wartości

parametrów opisujących każdy z przeanalizowanych segmentów toru. Wśród

zapisywanych danych znajdują się współrzędne punktu docelowego, które zostały

zidentyfikowane w G-kodzie, rodzaj docelowej interpolacji przestrzennej oraz prędkość

obrotowa narzędzia skrawającego. Zapisywane są również dane dotyczące interpolacji

czasowej; do opisu interpolacji z trapezoidalnym profilem przyspieszenia konieczne

jest podanie wartości następujących danych:

f1 – czas trwania fazy ruchu z jednostajnym dżerkiem podczas etapu

rozpędzania,

f2 – czas trwania fazy ruchu z jednostajnym przyspieszaniem podczas etapu

rozpędzania,

Seg1 Seg2

wartość nominalna

wartość skorygowana

65

f4 – czas trwania fazy ruchu z jednostajną prędkością,

f5 – czas trwania fazy ruchu z jednostajnym dżerkiem podczas etapu

hamowania,

f6 – czas trwania fazy ruchu z jednostajnym ujemnym przyspieszeniem,

– prędkość początkowa od której rozpoczyna się faza ruchu dla danego

segmentu,

– wartość dżerku w danym segmencie.

W zależności od interpolacji przestrzennej stosowany jest inny zestaw parametrów

określających kształt interpolowanego toru ruchu. Dla interpolacji liniowej

parametrami opisującymi kształt toru ruchu są:

– kąt, którego tangens opisuje stosunek przemieszczenia liniowego do

przemieszczenia kątowego,

– kąt pomiędzy osią OX a rzutem wektora jednostkowego (stycznego do

toru ruchu liniowego) na płaszczyznę XY,

– kąt pomiędzy płaszczyzną XY a wektorem jednostkowym stycznym do

toru ruchu,

– kąt pomiędzy osią OA a wektorem jednostkowym stycznym do toru

ruchu kątowego w płaszczyźnie AC.

Interpolacja kołowa również wymaga czterech parametrów:

– współrzędna X położenia środka okręgu względem punktu początkowego

danego segmentu,

– współrzędna Y położenia środka okręgu względem punktu początkowego

danego segmentu,

– współrzędna Z położenia środka okręgu względem punktu

początkowego danego segmentu,

– wartość początkowa kąta obrotu dla danego segmentu (np. wyznaczana

od osi OX dla interpolacji kołowej w płaszczyźnie równoległej do płaszczyzny

XY).

Algorytm części off-line działa aż do czasu przeanalizowania całego pliku

tekstowego. Gdy wykryty zostanie koniec pliku tekstowego, do tablicy zapisywanej w

pamięci dodawany jest kolejny wiersz z oznaczeniem zakończenia działania programu.

66

Dodatkowo jest zapisywana i wyświetlana całkowita liczba analizowanych instrukcji G-

kodu, oraz szacunkowy czas realizacji całego programu NC.

Rysunek 28 Schemat działania algorytmu dokonującego sprawdzenia i ewentualnej korekty prędkości w segmentach trajektorii gładkiej.

Trajektorie o gładkich przejściach

Nr segmentów końcowego i

początkowego

Tak

Nie

Obliczenie prędkości: v1 - Końcowej segmentu i-1go v2 - Początkowej segmentu i-tego v3 - Końcowej segmentu i-tego v4 - Początkowej segmentu i+1go Obliczenie interpolacji

czasowej i-tego segmentu z prędkością

początkową = v1 i prędkością końcową = v4

Obliczenie prędkości: v2 - Początkowej segmentu i-tego v3 - Końcowej segmentu i-tego

Tak

Nie

Zapis do pamięci

i = nr segmentu końcowego

Parametry trzech

segmentów

Czy są inne trajektorie

gładkie?

Tak

Koniec pod - programu

Nie

Zapis do pamięci

67

4.3. Część on-line Generatora Trajektorii

Wszystkie opisywane dotychczas algorytmy Generatora Trajektorii służyły

przygotowaniu danych dla części on-line. Na podstawie tych danych algorytm części

on-line jest w stanie odtworzyć zaprogramowany kształt trajektorii. Rysunek 29

przedstawia uproszczony schemat działania algorytmu części on-line Generatora

Trajektorii.

Rysunek 29 Uproszczony schemat algorytmu części on-line.

Głównym zadaniem tego algorytmu jest wygenerowanie pełnej trajektorii

(położenie, prędkość i przyspieszenie) dla pięciu osi obróbczych robota hybrydowego

do frezowania w czasie każdego cyklu próbkowania, dlatego też kluczowe staje się

START

Obsługa sygnałów sterujących

Parametry danego

segmentu

Tryb pracy?

Program NC

Ręczny

Awaria

Obliczenie parametrów

trajektorii

Interpolowanie w dziedzinie czasu

Parametry trajektorii pobrane z pamięci

Interpolowanie w przestrzeni

Przesłanie obliczonej trajektorii

STOP

Ustawienie sygnałów sterujących

Polecenie zatrzymania

napędów

68

wybranie takich typów interpolacji czasowej i przestrzennej, których realizacja zajmie

wystarczająco mało czasu.

4.3.1. Interpolacja czasowa

Dla dokładniejszej oceny poszczególnych typów interpolacji czasowej opisanych w

rozdziale 3.1 wybrano trzy parametry, które w istotny sposób je określają. Pierwszy

parametr dotyczy czasu, którego potrzebuje platforma sprzętowa na wykonanie

obliczeń w trybie on-line dla przykładowego segmentu toru narzędzia. Z racji tego, iż w

zależności od typu interpolacji czasowej pojedynczy segment składa się z kilku faz, w

których obliczenia się różnią, koniecznym było uśrednienie wartości czasu

wykonywania obliczeń całej ścieżki tak, by wyznaczyć średni czas obliczeń

pojedynczego punktu trajektorii dla danego profilu przyspieszenia. Sprawdzana tu jest

szybkość algorytmu obliczającego przemieszczenie wraz z jego pochodnymi. Do testów

każdego typu interpolacji użyto tego samego komputera klasy PC. Dodatkowo

porównano te czasy względem interpolacji z trapezoidalnym kształtem prędkości, którą

potraktowano jako wzorcową.

Kolejny parametr to czas, jaki zajmie obrabiarce przemieszczenie narzędzia wzdłuż

zadanego toru zgodnie z danym typem interpolacji przy zachowaniu maksymalnych

wartości granicznych parametrów ruchu. Dzięki temu porównaniu można ocenić

efektywność wyznaczonej trajektorii, co ma znaczenie w osiąganiu wysokiej

produktywności procesu frezowania. W tym przypadku również dodatkowo

porównano wszystkie przykłady względem najszybszej interpolacji z trapezoidalnym

profilem prędkości.

Ostatni parametr to energia składowych wysokoczęstotliwościowych sygnału.

Sygnałem, który posłużył do obliczania energii, było przyspieszenie, ponieważ to jego

kształt najistotniej wpływa na powstawanie drgań podczas procesu frezowania [20].

Energię sygnału dyskretnego określa równanie (51).

(51)

gdzie:

– energia sygnału dyskretnego,

69

– badany sygnał dyskretny,

– numer próbki sygnału.

Jednak równanie (51) dotyczy sygnału dyskretnego w dziedzinie czasu, konieczna

jest modyfikacja pozwalająca na wyznaczenie energii sygnału w dziedzinie

częstotliwości, gdzie możliwe będzie ograniczenie badanego pasma tylko do wysokich

częstotliwości. Do przejścia z dziedziny sygnału dyskretnego na dziedzinę częstotliwości

skorzystano z dyskretnej transformaty Fouriera (52):

(52)

gdzie:

– ciąg harmonicznych ( ),

– jednostka urojona,

– numer harmonicznej,

– liczba próbek.

Następnie korzystano z przekształcenia wzoru Persevala (53), który pozwala obliczyć

pożądaną energię sygnału.

(53)

Energię obliczono z transformaty Fouriera wyznaczonej dla wybranego pasma

częstotliwości (100Hz ÷ 5kHz), by uwzględnić tylko składowe mogące doprowadzić do

drgań układu narzędzie – przedmiot obrabiany, poza tym pasmem tłumienie

konstrukcji jest duże. Obliczono przebiegi czasowe pojedynczego segmentu dla każdej

z wymienionych w rozdziale 3.1 interpolacji czasowych, używając tego samego zestawu

wartości granicznych:

– całkowite przemieszczenie,

– maksymalna chwilowa prędkość liniowa,

70

– maksymalne chwilowe przyspieszenie liniowe,

– maksymalna wartość dżerku,

– maksymalna wartość udaru, czyli pochodnej dżerku w

czasie.

Poszczególne typy interpolacji czasowej wymagały tylko niezbędnych wartości

granicznych. Wszystkie wartości graniczne wykorzystywane są jedynie przez

interpolację z trapezoidalnym profilem dżerku. Wartości trzech parametrów

wyznaczonych dla każdej z interpolacji czasowych zebrano i przedstawiono w Tabeli 1.

Tabela 1 Zestawienie parametrów wybranych dla porównania różnych interpolacji czasowych

Jednostka Typ interpolacji

Czas obliczeń pojedynczego punktu

trajektorii

Czas realizacji zadanego toru

ruchu

Energia sygnału przyspieszenia

w paśmie 100Hz-5kHz

s %* s %*

Trapezoidalny profil prędkości

1,56E-06 100,00% 2,6675 100,00% 4,69E-06

Eksponencjalny profil prędkości

1,81E-06 116,09% 3,5000 131,21% 1,89E-09

Trapezoidalny profil przyspieszenia

1,69E-06 108,27% 2,7667 103,72% 1,05E-09

Profil dojazdowy 1,57E-06 100,52% 2,6679 100,01% 3,30E-06

Sinusoidalny profil przyspieszenia

2,7623E-04 17677,83% 2,7806 104,24% 5,17E-10

Trapezoidalny profil dżerku

2,21E-06 141,53% 2,7915 104,65% 5,44E-12

* względem wartości uzyskanej dla interpolacji z trapezoidalnym profilem prędkości

Na podstawie zebranych wyników można ocenić, który spośród zaprezentowanych

typów interpolacji czasowej nadaje się najlepiej do generowania trajektorii dla robota

hybrydowego do frezowania. Interpolacja z wykorzystaniem trapezoidalnego przebiegu

prędkości jest obliczana najszybciej oraz ma najkrótszy czas realizacji przejazdu. Z

drugiej strony ten typ interpolacji charakteryzuje się najwyższym poziomem energii

sygnału przyspieszenia dla wysokich częstotliwości, co w przypadku obróbki typu HSM

może znacząco zmniejszyć dokładność i precyzję frezowanych przedmiotów. Zaletą

kolejnej interpolacji z ekspotencjalnym profilem prędkości jest znacznie niższy poziom

energii składowych wysokoczęstotliwościowych. Ta interpolacja czasowa jest obliczana

przez algorytm, którego czas wykonywania jest dłuższy o około 16% w porównaniu do

interpolacji referencyjnej. Niestety, realizacja trajektorii wyznaczonej za pomocą tego

71

algorytmu trwa o ponad 30% dłużej niż w wariancie odniesienia, co też kwalifikuje ten

typ interpolacji czasowej jako najwolniej realizujący zadany toru ruchu. Natomiast typ

interpolacji z trapezoidalnym kształtem przyspieszenia charakteryzuje się trajektorią o

poziomie energii składowych wysokoczęstotliwościowych przyspieszenia niższym niż w

przypadku ekspotencjalnego profilu prędkości. Dodatkowo czas realizacji przejazdu jest

o niecałe 4% dłuższy od najkrótszego. Jedyny mankament to nieznacznie dłuższy czas

wykonywania obliczeń numerycznych – w porównaniu do przykładu wzorcowego jest o

ponad 8% wyższy. Następny typ interpolacji z dojazdowym profilem prędkości pod

względem wyznaczonych parametrów jest bardzo zbliżony do pierwszej z omawianych

interpolacji. Zarówno czas wykonywania obliczeń, jak i czas realizacji zadanego toru

ruchu są sobie prawie równe. Wprawdzie udało się nieco ograniczyć energię

przyspieszenia w wysokich częstotliwościach, jednak w porównaniu do pozostałych

typów interpolacji wynik jest zdecydowanie zbyt wysoki. Kolejny przykład interpolacji z

sinusoidalnym kształtem przyspieszenia, jest algorytmem, który dobrze redukuje

energię przyspieszenia w wysokich częstotliwościach, a realizacja wyznaczonej przez

niego trajektorii zajmuje ponad 4% więcej czasu niż wartość najkrótsza. Jednakże duża

złożoność obliczeniowa dyskwalifikuje ten typ interpolacji jako algorytm pracujący w

czasie rzeczywistym. Czas potrzebny na obliczenie pojedynczego punktu trajektorii jest

o dwa rzędy wielkości większy niż dla interpolacji z trapezoidalnym profilem prędkości.

Interpolacja z trapezoidalnym profilem dżerku jest typem interpolacji czasowej, która

najbardziej redukuje poziom energii składowych wysokoczęstotliwościowych

przyspieszenia. Dodatkowo, efektywny czas przejazdu z wykorzystaniem interpolacji z

trapezoidalnym profilem dżerku jest niewiele większy od czasu najkrótszego (wzrost o

4.65%). Mankamentem jest wyższa złożoność algorytmu, przez co obliczanie trajektorii

trwa prawie o 41% dłużej niż w przypadku interpolacji z trapezoidalnym profilem

prędkości.

Na podstawie powyższej analizy do zastosowania w opracowanym generatorze

trajektorii dla robota hybrydowego wybrano interpolację czasową z trapezoidalnym

profilem przyspieszenia. Interpolacje z trapezoidalnym profilem prędkości oraz z

dojazdowym profilem prędkości mogą wzbudzać niepożądane drgania, obniżające

dokładność wykonywanego elementu lub też mogące prowadzić do jego uszkodzenia.

Trajektoria wyznaczana z pomocą ekspotencjalnego profilu prędkości jest realizowana

72

w zbyt długim czasie w porównaniu do pozostałych metod interpolacji czasowej.

Trygonometryczny profil przyspieszenia również nie spełnia wymagań stawianych

interpolacji czasowej dla wyznaczania trajektorii z wysokimi częstotliwościami

próbkowania Układu Sterowania. Interpolacja czasowa z trapezoidalnym profilem

dżerku mogłaby być brana pod uwagę, gdyby w dalszym ciągu pojawiały się drgania

wysokoczęstotliwościowe mimo zastosowania wybranego profilu przyspieszeń. Jednak

trzeba uwzględnić konieczność użycia bardziej wydajnej platformy obliczeniowej, by

sprostać wymaganiom obliczeniowym tej interpolacji.

Interpolacja w każdej z faz w trapezoidalnym profilu przyspieszenia jest wyznaczana

z wykorzystaniem poniższych równań:

(54)

gdzie:

– wartości początkowe przemieszczenia, prędkości i przyspieszenia, od

których rozpoczyna się aktualna faza danego segmentu toru ruchu,

– chwilowe wartości przemieszczenia, prędkości,

przyspieszenia i dżerku wyznaczane dla aktualnej chwili czasu .

Równania (54) wyznaczają chwilowe wartości przemieszczenia i jego pochodnych

wg danych obliczonych w części off-line (opisano je w rozdziale 4.2.4). Przyjęto

założenie, iż na początku każdego segmentu toru początkowe przemieszczenie oraz

przyspieszenie jest równe zero. Początkowa prędkość zależy od tego, czy przejście

pomiędzy aktualnym segmentem a segmentem poprzednim było przejściem gładkim.

Wartość dżerku, stosowana w danym segmencie, jest wyznaczana w części off-line

generatora trajektorii. Domyślnie jest używana maksymalna wartość dżerku. Na

rysunku 30 zaprezentowano schemat blokowy algorytmu wyznaczającego wartości

chwilowe interpolacji czasowej. Na początku wczytywany jest zestaw parametrów

konieczny do odtworzenia trapezoidalnego profilu przyspieszenia. Na podstawie

73

długości faz obliczane są czasy , które służą jako wyznaczniki

momentów, w których ma nastąpić zmiana fazy na kolejną.

Rysunek 30 Schemat blokowy algorytmu obliczania interpolacji czasowej w części on-line.

Czasy wyznaczono za pomocą równań:

(55)

gdzie:

– czasy trwania poszczególnych faz profilu przyspieszenia w danym

segmencie toru,

Długości faz: , prędkość – ,

dżerk -

Rozpoczęto nową fazę?

Obliczenie czasów zmiany faz:

Tak

Nie

Nie

W zależności od numeru fazy

obliczenie

START

Obliczenie wartości dla danej chwili

zapamiętanie

Dane dla interpolacji

przestrzennej

STOP

Tak

74

– czasy w których następuje zmiana fazy w trapezoidalnym profilu

przyspieszenia.

Gdy rozpoczynana jest nowa faza, konieczne jest wyznaczenie parametrów

początkowych, które będą wykorzystywane do obliczania wartości chwilowych

przemieszczenia i jego pochodnych. Do ich obliczenia wykorzystywane są równania

(54) z czasem równym długości aktualnie zakończonej fazy. W ten sposób obliczone

wartości chwilowe interpolacji czasowej są gotowe do wyznaczenia dla nich

interpolacji przestrzennej.

4.3.2. Interpolacja przestrzenna

Dla opracowanego robota hybrydowego do frezowania najistotniejszymi

wymaganiami dla interpolacji przestrzennej są szybki czas wykonywania obliczeń (przez

wzgląd na wysoką częstotliwość próbkowania Układu Sterowania) oraz możliwość

wyznaczenia dokładnych wartości prędkości i przyspieszeń dla każdej z osi (jako

wymaganie Sterownika Śledzenia Trajektorii). Za pomocą dwóch pierwszych z

wymienionych sposobów interpolacji przestrzennej (interpolacja liniowa oraz

interpolacja kołowa) możliwe jest wyznaczenie położeń, prędkości oraz przyspieszeń

dla wszystkich pięciu osi robota. Te typy interpolacji są również stosunkowo proste

obliczeniowo i wykonują się na tyle szybko, iż pozostaje zapas mocy obliczeniowej na

wykonanie pozostałych operacji arytmetycznych, które muszą być wykonane z

częstotliwością próbkowania Układu Sterowania. Pozostałe dwa z omówionych typów

interpolacji przestrzennej (interpolacja krzywą wielomianową typu splajn oraz

interpolacja krzywą B-sklejaną) są realizowane przez dużo bardziej złożone algorytmy,

które potrzebują znacznie więcej czasu na wykonanie wszystkich obliczeń.

Tabela 2 Zestawienie czasów obliczeń pojedynczego punktu trajektorii z wykorzystaniem różnych interpolacji przestrzennych.

Jednostka Typ interpolacji

Czas obliczenia jednego punktu trajektorii

s %*

Liniowa 8,50E-08 100%

Kołowa 4,90E-07 576%

Krzywa typu splajn 5,20E-05 61176%

Krzywa B-sklejana 1,83E-04 215294%

* względem trajektorii uzyskanej dla interpolacji liniowej

75

W Tabeli 2 zaprezentowano wyniki porównania średniego czasu potrzebnego do

wyznaczenia współrzędnych położenia, prędkości i przyspieszenia dla pojedynczej

próbki z całej obliczonej trajektorii (dla celów porównania długość przemieszczenia w

danym segmencie toru czy też kierunek ruchu dobrano dowolnie). Każdy z typów

interpolacji tworzy trajektorię w innym kształcie, nie ma więc możliwości ujednolicenia

trajektorii dla każdego z typów interpolacji. Wyznaczone trajektorie uwzględniają

obróbkę pięcioosiową. Wszystkie symulacje były wykonywane na tym samym

komputerze typu desktop PC. Przedstawione wartości są równe czasowi potrzebnemu

do wyznaczenia całej trajektorii podzielonemu przez liczbę wygenerowanych punktów

trajektorii. Ponieważ każdy z typów interpolacji przestrzennej określa inny kształt toru

ruchu, testowane segmenty różniły się między sobą. Dla interpolacji liniowej i kołowej

były to pojedyncze segmenty wykonane z tego typu interpolacją. Dla interpolacji

wielomianowej oraz krzywymi B-sklejanymi tor był złożony z kilku segmentów i na ich

podstawie wyznaczono kształt toru narzędzia. Podobnie jak w przypadku

porównywania interpolacji czasowych, również w tym zestawieniu dodano kolumnę

porównującą czas obliczeń każdej z interpolacji przestrzennych względem interpolacji

liniowej – wybranej jako referencyjną, gdyż obliczenia wykonywane z jej pomocą

trwały najkrócej. Dla interpolacji liniowej czas ten wyniósł około 85ns. Dla interpolacji

kołowej czas potrzebny do obliczenia pojedynczego punktu trajektorii jest równy około

490ns. W porównaniu do interpolacji liniowej wykonanie obliczeń trwało 5,77 razy

dłużej. Mimo to ten typ interpolacji, podobnie jak i interpolacja liniowa, obliczany jest

na tyle szybko, by sprostać wymaganej częstotliwości próbkowania Układu Sterowania,

równej 10kHz. Kolejny typ interpolacji przestrzennej, z wykorzystaniem krzywych typu

splajn, wyznacza pojedynczy punkt trajektorii średnio w 52µs. Ta wartość jest równa

połowie okresu próbkowania, jednakże poza samym obliczaniem interpolacji

przestrzennej Generator potrzebuje wykonać inne działania, które mogą spowodować,

iż Układ Sterowania nie będzie otrzymywał kolejnych punktów trajektorii na czas.

Dodatkową niewiadomą jest wydajność obliczeniowa platformy sprzętowej, na której

cały Układ Sterowania będzie zaimplementowany. Do opracowania wyników symulacji

użyto wydajnego komputera PC typu desktop, więc obliczenia na docelowej platformie

sprzętowej mogą być realizowane wolniej. Ostatni z zaprezentowanych typów

interpolacji przestrzennej, z wykorzystaniem krzywych B-sklejanych, obliczany był

76

najwolniej. Wytyczenie pojedynczego punktu trajektorii z wykorzystaniem tego typu

interpolacji zajęło 183µs, co już zdecydowanie przekracza założony okres próbkowania

w Układzie Sterowania. Mimo iż interpolacja krzywymi B-sklejanymi jest często

używana w nowoczesnych obrabiarkach sterowanych numerycznie pracujących z

wysokimi prędkościami posuwu, to jednak dla robota hybrydowego do frezowania

interpolacja przestrzenna wykorzystująca krzywe B-sklejane nie może być

zastosowana. Jako obsługiwane typy interpolacji przestrzennych dla robota

hybrydowego do frezowania wybrano interpolacje liniową oraz kołową, gdyż tylko one

są wystarczająco mało wymagające dla zachowania wymaganej częstotliwości

próbkowania Układu Sterowania.

Interpolacja przestrzenna jest wykonywana na bazie parametrów kierunkowych

obliczonych w części off-line (opisanych w rozdziale 4.2.4) oraz na chwilowych

wartościach otrzymanych z profilu przyspieszenia danego segmentu toru ruchu. Celem

interpolacji przestrzennej jest wyznaczenie wartości bezwzględnych w Globalnym

układzie współrzędnych kolejnych punktów trajektorii. W pierwszej kolejności, w

przypadku ruchu synchronicznego, ruch jest rozdzielany na poszczególne

manipulatory. Do tego celu wykorzystano kąt wyznaczony w części off-line dla

danego segmentu. Poniżej zaprezentowano równania pozwalające wyznaczyć wartości

chwilowe zmiennych kinematycznych ruchu dla robota równoległego (56) i dla stołu

uchylno – obrotowego (57):

(56)

(57)

gdzie:

– wartości przemieszczenia wzdłuż toru ruchu od początku

segmentu, chwilowej prędkości liniowej i chwilowego przyspieszenia liniowego

wyznaczone z profilu przyspieszenia na etapie interpolacji czasowej,

77

– kąt, którego tangens określa stosunek przemieszczenia liniowego do

przemieszczenia kątowego w danym segmencie,

– wartości przemieszczenia wzdłuż toru ruchu od początku segmentu,

chwilowej prędkości liniowej i chwilowego przyspieszenia liniowego dla ruchu

translacyjnego,

– wartości przemieszczenia wzdłuż toru ruchu od początku segmentu,

chwilowej prędkości liniowej i chwilowego przyspieszenia liniowego dla ruchu

rotacyjnego.

Następnie wartości obliczone wzdłuż toru ruchu są rzutowane: translacji na osie X, Y

i Z oraz rotacji na obroty wokół osi OX i OZ globalnego układu współrzędnych. W

zależności od typu interpolacji przestrzennej rzutowanie oblicza się za pomocą innych

równań. Poniżej przedstawiono równania służące wyznaczeniu wartości przemieszczeń

(58), prędkości (59) i przyspieszeń (60) we współrzędnych globalnego układu

współrzędnych dla interpolacji liniowej:

(58)

(59)

(60)

78

gdzie:

– współrzędne punktu początkowego danego segmentu w

globalnym układzie współrzędnych,

– pozycja i orientacja narzędzia w globalnym układzie współrzędnych,

– prędkości liniowe i kątowe w globalnym układzie współrzędnych,

– przyspieszenia liniowe i kątowe w globalnym układzie

współrzędnych,

– kąt kierunkowy będący rzutem wektora jednostkowego, stycznego do toru

ruchu w danym segmencie, na płaszczyznę XY – mierzony od dodatniego kierunku osi

OX w układzie współrzędnych XYZ,

– kąt kierunkowy będący kątem pomiędzy wektorem jednostkowym, stycznym do

toru ruchu w danym segmencie, a płaszczyzną XY mierzony od poziomu płaszczyzny w

układzie współrzędnych XYZ,

– kąt kierunkowy będący kątem pomiędzy wektorem jednostkowym, stycznym do

przemieszczenia kątowego w danym segmencie, a dodatnim kierunkiem osi A w

układzie współrzędnych AC.

Interpolacja kołowa może być wykonywana na dwa sposoby: zgodnie z kierunkiem

ruchu wskazówek zegara i przeciwnie do ruchu wskazówek zegara.

Rysunek 31 Rysunek poglądowy wyznaczania punktów w przestrzeni z wykorzystaniem kołowej interpolacji przestrzennej [21].

79

Istnieją osobne polecenia G-kodów G02 i G03 służące do oznaczenia interpolacji

danego segmentu jako obrót w odpowiednim kierunku. Do wyznaczenia punktów

trajektorii z interpolacją kołową konieczne jest podanie współrzędnych: punktu

początkowego – , wektora jednostkowego jednoznacznie określającego kierunek osi

obrotu wokół której kreślony jest łuk – , oraz punktu leżącego na osi obrotu –

(Rysunek 31) [21]. W pierwszej kolejności należy wyznaczyć współrzędne punku

będącego punktem centralnym interpolowanego łuku. Przyjęto wektor ,

współrzędne środka okręgu wyznaczono z równania (61):

. (61)

Następnie wyznaczono parametr będący długością promienia łuku.

Parametryczna reprezentacja interpolacji kołowej w układzie współrzędnych x1y1z1

dana jest w postaci(62):

, (62)

gdzie:

– chwilowa wartość kąta obrotu promienia wodzącego.

Współrzędne punktu wyrażone w globalnym układzie współrzędnych jako punkt

dane są z równania (63):

(63)

gdzie:

– macierz rotacji wyznaczona jako .

W celu uproszczenia i przyspieszenia wykonywanych obliczeń przez algorytm części

on-line, niektóre z wymienionych danych są wyznaczane w trybie off-line i przesyłane

w postaci parametrów kierunkowych: współrzędne środka obrotu , długość

promienia – oraz współrzędne wektora jednostkowego . Wektor za pomocą

języka G-kodów może być opisany tylko na trzy sposoby: – dla interpolacji łuku

80

w płaszczyźnie równoległej do płaszczyzny XY globalnego układu współrzędnych –

funkcja G17, – dla interpolacji łuku w płaszczyźnie równoległej do płaszczyzny

ZX globalnego układu współrzędnych – funkcja G18 oraz – dla interpolacji łuku

w płaszczyźnie równoległej do płaszczyzny YZ globalnego układu współrzędnych –

funkcja G19.

Rysunek 32 Schemat blokowy algorytmu wyznaczania kołowej interpolacji przestrzennej.

W oparciu o algorytm, którego schemat blokowy przedstawiono na Rysunku 32,

obliczane są położenie narzędzia, jego chwilowa prędkość liniowa oraz chwilowe

przyspieszenie liniowe wymagane przez Sterownik Śledzenia Trajektorii. Poniżej

przedstawiono sposób obliczania interpolacji kołowej wykonanej na płaszczyźnie

Parametry kierunkowe z trybu off-line

START

Obliczenie wartości:

STOP

G02

G03

Kierunek interpolacji

G17 G18 G19

Płaszczyzna interpolacji

Obliczenie współrzędnych

punktu

Obliczenie współrzędnych wektora

prędkości

Obliczenie współrzędnych wektora

przyspieszenia

Dane dla Sterownika Śledzenia Trajektorii

81

równoległej do płaszczyzny XY w ruchu zgodnie z kierunkiem wskazówek zegara.

Chwilowa wartość kąta obrotu promienia wodzącego dana jest równaniem:

(64)

gdzie:

– przemieszczenie kątowe po okręgu wyrażone w mierze łukowej, wyznaczane

z profilu przyspieszenia podczas interpolacji czasowej,

– promień wykonywanego łuku.

Wprowadzono dwie zmienne pomocnicze:

(65)

gdzie:

– wartość kąta promienia wodzącego na początku segmentu.

Otrzymuje się następujące zależności, pozwalające wyznaczyć chwilową pozycję i

orientację narzędzia w globalnym układzie współrzędnych:

(66)

gdzie:

– promień wykonywanego łuku,

– współrzędne środka obrotu na płaszczyźnie XY względem położenia

punktu początkowego segmentu,

– współrzędne punktu początkowego danego segmentu w globalnym

układzie współrzędnych.

82

Prędkości oraz przyspieszenia liniowe i kątowe w globalnym układzie współrzędnych

dane są równaniami:

(67)

(68)

gdzie:

– chwilowa prędkość liniowa i chwilowe przyspieszenie liniowe wyznaczone z

profilu przyspieszenia podczas interpolacji czasowej,

– prędkości liniowe i kątowe w globalnym układzie współrzędnych,

– przyspieszenia liniowe i kątowe w globalnym układzie

współrzędnych.

Początkowo obliczana jest miara przyrostu kąta, jaką ma osiągnąć narzędzie w

trakcie wykonywania ruchu po okręgu (64) do bieżącej chwili czasu, następnie

obliczona wartość jest odejmowana od wartości kąta początkowego. Po tym

wyznaczane są wartości funkcji trygonometrycznych sinus i cosinus z otrzymanej

różnicy (65). Na podstawie tych danych obliczane są wartości przemieszczeń (66),

prędkości (67) i przyspieszeń (68) w ruchu po okręgu na danej płaszczyźnie.

Interpolacja kołowa jest wykonywana przy niemodyfikowanych wartościach obrotu

wokół osi OX i OZ przez wzgląd na konieczność zachowania zadanej prędkości liniowej

posuwu narzędzia skrawającego podczas procesu frezowania. W przypadku ruchu

przeciwnego do kierunku ruchu wskazówek zegara niektóre równania (65)(67)(68)

ulegają modyfikacji do postaci:

83

(69)

Pozostałe wartości wyznaczane są w identyczny sposób jak w przypadku ruchu w

kierunku zgodnym z ruchem wskazówek zegara. W przypadku wyznaczania interpolacji

kołowej dla płaszczyzny równoległej do płaszczyzny ZX globalnego układu

współrzędnych, dla ruchu zgodnego z kierunkiem ruchu wskazówek zegara, stosowane

są równania w następującej postaci:

(70)

gdzie:

– promień wykonywanego okręgu,

– współrzędne środka obrotu na płaszczyźnie ZX względem położenia

punktu początkowego segmentu,

– współrzędne punktu początkowego danego segmentu w globalnym

układzie współrzędnych

– zmienne pomocnicze wyznaczone z równania (65).

Prędkości oraz przyspieszenia liniowe i kątowe w globalnym układzie współrzędnych

dane są równaniami:

(71)

84

(72)

gdzie:

– chwilowa prędkość liniowa i chwilowe przyspieszenie liniowe wyznaczone z

profilu przyspieszenia podczas interpolacji czasowej.

– prędkości liniowe i kątowe w globalnym układzie współrzędnych,

– przyspieszenia liniowe i kątowe w globalnym układzie

współrzędnych.

Dla interpolacji kołowej w płaszczyźnie równoległej do płaszczyzny ZX globalnego

układu współrzędnych w kierunku przeciwnym do kierunku ruchu wskazówek zegara

modyfikowane są niektóre z równań (65)(71)(72) do postaci:

(73)

Pozostałe wartości wyznaczane są z wykorzystaniem równań używanych w

obliczeniach dla interpolacji kołowej w kierunku zgodnym z kierunkiem ruchu

wskazówek zegara.

Ostatni wariant kołowej interpolacji przestrzennej dotyczy przypadku, gdy

interpolowany łuk znajduje się na płaszczyźnie równoległej do płaszczyzny YZ

globalnego układu współrzędnych. Współrzędne położenia dla tego wariantu obliczane

są z następujących równań:

85

(74)

gdzie:

– promień wykonywanego okręgu,

– współrzędne środka obrotu na płaszczyźnie YZ względem położenia punktu

początkowego segmentu,

– współrzędne punktu początkowego danego segmentu w globalnym

układzie współrzędnych,

– zmienne pomocnicze wyznaczone z równania (65).

Prędkości oraz przyspieszenia liniowe i kątowe w globalnym układzie współrzędnych

dane są równaniami:

(75)

(76)

gdzie:

– chwilowa prędkość liniowa i chwilowe przyspieszenie liniowe wyznaczone z

profilu przyspieszenia podczas interpolacji czasowej,

– prędkości liniowe i kątowe w globalnym układzie współrzędnych,

86

– przyspieszenia liniowe i kątowe w globalnym układzie

współrzędnych.

Analogicznie jak w poprzednich dwóch przypadkach, dla interpolacji kołowej w

płaszczyźnie równoległej do płaszczyzny YZ globalnego układu współrzędnych, w

kierunku przeciwnym do kierunku ruchu wskazówek zegara, modyfikowane są niektóre

z równań (65)(75)(76) do postaci:

(77)

Na podstawie przedstawionych w tym podrozdziale równań obliczane są dane

wymagane przez Sterownik Śledzenia Trajektorii: położenie i orientacja narzędzia,

chwilowa prędkość liniowa oraz chwilowe przyspieszenie liniowe. Opisane metody

interpolacji liniowej i kołowej zostały zaimplementowane w opracowanym

Generatorze Trajektorii.

87

5. Implementacja opracowanego Generatora Trajektorii

W niniejszym rozdziale opisany zostanie sposób zaimplementowania Generatora

Trajektorii na potrzeby testów symulacyjnych oraz w Układzie Sterowania na

docelowej platformie sprzętowej. Do implementacji opracowanych algorytmów

wybrano język programowania C. Wybór ten został podyktowany możliwością

uruchamiania aplikacji napisanych w tym języku na różnych platformach systemowych

bez potrzeby znacznych modyfikacji kodu programu. W pierwszej kolejności program

Generatora Trajektorii został zaimplementowany w środowisku symulacyjnym systemu

Matlab/Simulink, co zostało opisane w pierwszym podrozdziale. Następnie w kolejnym

podrozdziale opisano implementację algorytmu Generatora Trajektorii w układzie

FPGA, który jest docelową platformą sprzętową Układu Sterowania.

5.1. Implementacja w środowisku symulacyjnym

Algorytmy Generatora Trajektorii zostały zaimplementowane w postaci dwóch

głównych programów. Odpowiadają one za realizację działań osobnych części: off-line i

on-line Generatora. Program z częścią off-line uruchamiany jest jako program

wykonawczy, do którego parametrami wejściowymi są: nazwa pliku tekstowego

zawierającego instrukcje G-kodu oraz wartości współrzędnych koniecznych do

obliczenia kompensacji różnicy w sposobie zamocowania przedmiotu (tę procedurę

opisano w podrozdziale 4.2.2). Kompensacja polega na transformacji układu

współrzędnych związanego z obrabianym przedmiotem (w którym tworzony jest

program w G-kodzie) na globalny układ współrzędnych, w którym pracuje robot

hybrydowy do frezowania. Główną cechą tego programu jest fakt, iż uruchamiany jest

tylko raz, przed rozpoczęciem operacji frezowania z wykorzystaniem robota

hybrydowego. Program off-line wykonuje czasochłonne i skomplikowane działania

arytmetyczne i przesyła wynik tych obliczeń do algorytmu części on-line. Dzięki temu w

części on-line wykonywane są jedynie niezbędne działania służące do generacji

trajektorii z częstotliwością próbkowania Jądra Układu Sterowania.

Część off-line jako wynik działań zwraca tablicę zawierającą dane, które określają w

jaki sposób ma być wykonany każdy z segmentów toru narzędzia, zdefiniowanego w

programie NC (G-kodzie). Dane te są obliczane z użyciem zmiennych typu double, tzn.

88

każda wartość jest reprezentowana za pomocą liczby zmiennoprzecinkowej podwójnej

precyzji (64 bity). Użycie tego typu zmiennych pozwoliło uzyskać wysoką precyzję

obliczeń numerycznych i tym samym wartości otrzymanych danych wygenerowanych

w trybie off-line. Wspomniana wcześniej tablica danych, zwracana jako wynik działania

programu części off-line, zawiera następujące wartości (dane) potrzebne do opisania

pojedynczego segmentu (Tabela 3):

Tabela 3 Opis wartości (danych) obliczanych w części off-line generatora trajektorii.

Lp. Liczba wartości

Opis

1. 1 Oznaczenie numeryczne funkcji G-kodu. 2. 5 Współrzędne docelowej pozycji i orientacji narzędzia po wykonaniu

danego segmentu. 3. 5 Czasy trwania poszczególnych faz profilu przyspieszenia dla interpolacji

czasowej ruchu narzędzia (fazy f1, f2, f4, f5 i f6). 4. 1 Wartość początkowej chwilowej prędkości liniowej narzędzia v0

(posuwu) od jakiej robot ma rozpocząć wykonywanie aktualnego segmentu.

5. 1 Wartość dżerku d dla danego segmentu. 6. 2 Wartości funkcji sinus i kosinus dla kąta , którego tangens wyznacza

stosunek między ruchem translacyjnym a rotacyjnym dla aktualnego segmentu.

7. 6 Współczynniki kierunkowe pozwalające określić sposób rzutowania przemieszczenia, prędkości i przyspieszenia dla danej interpolacji przestrzennej na poszczególne osie globalnego układu współrzędnych robota hybrydowego.

8. 1 Wartość zadanej prędkości obrotowej wrzeciona dla aktualnego segmentu.

Te 22 parametry pozwalają jednoznacznie określić dokładny kształt toru ruchu oraz

sposób jego realizacji w czasie, czyli trajektorię ruchu. Dla skrócenia czasu

wykonywania obliczeń trygonometrycznych przez część on-line, czasochłonne

obliczenia wartości funkcji trygonometrycznych wykonywane są, gdy to możliwe,

wcześniej i przesyłane w tablicy – zaprezentowano to np. dla parametrów nr 6 w Tabeli

3. Wartości te później zostają wykorzystane wprost w równaniach (56) i (57)

używanych do interpolacji przestrzennej. Sprawdzenie poprawności działania

programu off-line nastąpiło przez zapis obliczanej tablicy do pliku tekstowego i

zweryfikowanie odczytanych wartości numerycznych. Sposób weryfikacji opisany jest

w rozdziale 6.

89

Test programu z częścią on-line Generatora Trajektorii wykonano za pomocą

środowiska obliczeniowego Matlab/Simulink (Matlab w wersji R2012b (8.0.0.783) 64-

bit). Pozwoliło ono na symulowanie działania pozostałych części Układu Sterowania z

krokiem symulacji równym okresowi próbkowania rzeczywistego Układu. Dodatkową

zaletą tego środowiska obliczeniowego jest możliwość uruchomienia programów

napisanych w języku C wprost w programie symulacyjnym Simulink jako blok S-funkcji

oraz łatwa archiwizacja i dalsza analiza wyników otrzymanych w trakcie symulacji. Na

Rysunku 33 przedstawiono schemat wykorzystany podczas testowania części on-line

Generatora Trajektorii w środowisku symulacyjnym. Zielonym kolorem oznaczono blok

części on-line Generatora Trajektorii, kolorem jasnobłękitnym oznaczono sygnały i

wartości przekazywane i wysyłane do Panelu Operatorskiego.

Rysunek 33 Schemat wykorzystany podczas testowania części on-line Generatora Trajektorii w programie Matlab/Simulink.

Kolorem granatowym oznaczono sygnały otrzymywane z Menadżera Zadań, są to

sygnały binarne dotyczące wykonania procedury kalibracji robota oraz ewentualne

wystąpienie błędów w pracy robota hybrydowego. Pozwoliło to na przetestowanie

90

dodatkowych funkcji Generatora Trajektorii: umożliwiające współdziałanie z

pozostałymi częściami Układu Sterowania, realizujące procedurę kalibracji robota,

realizujące procedurę obsługi błędów pojawiających się w trakcie generacji trajektorii z

programu NC (funkcje te i tryby zostaną omówione w rozdziale 5.2). Ostatnim

wejściem do bloku Generatora Trajektorii jest informacja dostarczana ze Sterownika

Śledzenia Trajektorii o aktualnej pozycji, w jakiej znajdują się oba manipulatory.

Zrealizowano to za pomocą sprzężenia zwrotnego z użyciem bloku Memory (kolor

pomarańczowy) podając wartość obliczoną na wyjściu jako aktualną pozycję

manipulatorów. W tym samym kolorze oznaczono sygnały i parametry wyjściowe

wysyłane do wspomnianych wcześniej części Układu Sterowania. Do Sterownika

Śledzenia Trajektorii trafia pełna trajektoria, czyli wyznaczone wartości przemieszczeń

(XYZAC), prędkości (VXYZAC) oraz przyspieszeń (AXYZAC) dla wszystkich pięciu osi

robota hybrydowego do frezowania. Do części Panelu Operatorskiego zwracane są

wartości sygnałów informacyjnych o aktualnie realizowanym segmencie programu NC.

Dodatkowo oznaczono kolorem czerwonym sygnały i wartości wyznaczane dla

Sterownika Wrzeciona.

Dodatkową cechą symulacji numerycznej w porównaniu do docelowej platformy

sprzętowej jest fakt, iż program Matlab/Simulink działa wyłącznie na zmiennych

podwójnej precyzji – typ double (ten sam typ zmiennej był wykorzystywany w

programie części off-line), natomiast program części on-line realizowany za pomocą

platformy sprzętowej posługuje się zmiennymi pojedynczej precyzji – typ float.

Zastosowanie innego typu zmiennych zostało podyktowane przez wzgląd na konieczną

szybkość wykonywanych obliczeń przez Układ Sterowania. Jednocześnie trajektoria

uzyskana w symulacji posłużyła jako wzorzec, do którego porównano trajektorię

otrzymaną na docelowej platformie sprzętowej. Pozwoliło to wyznaczyć błędy

numeryczne wynikające z zastosowania innego typu zmiennych. Testy te zostały

omówione w kolejnym podrozdziale, dotyczącym implementacji Generatora Trajektorii

na docelowej platformie sprzętowej Układu Sterowania.

91

5.2. Implementacja na docelowej platformie sprzętowej

Generator Trajektorii jest jednym z komponentów całego Układu Sterowania robota

hybrydowego do frezowania. W związku z tym konieczne było dopasowanie wielu

aspektów programu w taki sposób, aby współdziałał on z pozostałymi komponentami.

Część on-line jest zaimplementowana bezpośrednio w Jądrze Układu Sterowania

robota hybrydowego, który pracuje w jednym z trzech trybów:

1. interpretacji zapisanego w pliku tekstowym programu NC zawierającego

instrukcje G-kodu (zgodnego z normą ISO 6983 [17]) z możliwością

wprowadzenia korekcji układu współrzędnych;

2. ręcznym, w którym ścieżka narzędzia jest zadawana przez operatora w panelu

operatorskim – realizowana jest wtedy odpowiadająca temu instrukcja G-kodu

generowana bezpośrednio w Jądrze Układu Sterowania;

3. awaryjnym – po wystąpieniu błędów działania frezarki lub uruchomieniu stopu

awaryjnego.

Tryb pracy i generowane trajektorie zależą od wartości sygnałów wejściowych

(Rysunek 34) otrzymywanych z Panelu Operatorskiego, Menadżera Zadań oraz ze

Sterownika Śledzenia Trajektorii.

Rysunek 34 Sygnały wejściowe i wyjściowe części on-line Generatora Trajektorii.

GEN

ERA

TOR

TR

AJE

KTO

RII

ON

– L

INE

PANEL OPERATORSKI

Dane z części off – line

Sygnały sterujące

Pozycja zadana przez operatora

Długość frezu

Numer linii programu

Sygnały błędów i kalibracji

MENADŻER ZADAŃ

Aktualna pozycja

Pozycja X Y Z A C

STEROWNIK ŚLEDZENIA TRAJEKTORII

Prędkość X Y Z A C

Przyspieszenie X Y Z A C

Numer aktualnej instrukcji

Sygnały informacyjne

STEROWNIK WRZECIONA

Sygnały sterujące

92

Są to informacje z przycisków obsługiwanych przez operatora: czy program ma

wykonać ruch ustawczy, czy też ma rozpocząć generowanie trajektorii zapisanej w

pliku tekstowym. Analizowane są również sygnały od Menadżera Zadań: o wykonaniu

kalibracji robota i zgłaszające wystąpienie błędów w procesie frezowania. Generator

Trajektorii obsługuje dwa typy błędów; pierwszy błąd zgłaszany przez Menadżera

Zadań to błąd pozycji. Występuje on, gdy trajektoria zadana z generatora i rzeczywista,

realizowana przez robota, za bardzo się różnią, np. gdy robot wpada w oscylacje

podczas śledzenia trajektorii. Gdy taka sytuacja wystąpi, po zakończeniu wykonywania

aktualnego segmentu generowana jest trajektoria powodująca odjechanie narzędziem

nad przedmiot obrabiany, przejazd do pozycji spoczynkowej i zgłoszenie wystąpienia

błędu. Istnieje wtedy możliwość przejazdu do innego punktu wybranego przez

operatora lub odwołanie alarmu i kontynuowanie frezowania z ponownym

wykonaniem segmentu, w którym wystąpił błąd pozycji. Drugi typ błędu zgłaszany

przez Menadżera Zadań to błąd krytyczny, np. kolizja lub wciśnięcie przycisku

bezpieczeństwa. Generator w tym przypadku zeruje wartości zadanych prędkości i

przyspieszeń, a na wyjściu pozycji zadanej podaje wartość aktualną. Taki stan

występuje również po włączeniu zasilania do czasu zakończenia kalibracji robota. W

tym trybie generowana trajektoria nie jest używana.

Część on-line wymaga do poprawnej pracy danych obliczonych w części off-line

Generatora Trajektorii. Znajdują się wśród nich informacje dotyczące:

typu wykonywanej operacji,

współrzędnych pozycji i orientacji punktu docelowego, do których manipulatory

mają za zadanie się przemieścić,

czasu trwania poszczególnych faz profilu przyspieszenia dla interpolacji

czasowej ruchu robota hybrydowego,

wartości prędkości początkowej, od jakiej robot ma rozpocząć wykonywanie

aktualnego segmentu,

wartości dżerku w aktualnym segmencie,

współczynników kierunkowych pozwalających wyznaczyć tor ruchu dla danej

interpolacji przestrzennej,

kąta pozwalającego synchronizować ruch translacyjny i rotacyjny,

wartości zadanej prędkości obrotowej wrzeciona dla aktualnego segmentu.

93

Kolejnym sygnałem wejściowym jest aktualna pozycja, w jakiej znajduje się robot

równoległy i stół uchylno – obrotowy. Informacje te są uzyskiwane ze Sterownika

Śledzenia Trajektorii. Aby wykonać ruchy ustawcze (jednorazowe przejazdy narzędzia

w trybie ręcznym, zadawane przez operatora za pomocą Panelu Operatorskiego), do

Generatora muszą być dostarczone z Panelu Operatorskiego współrzędne punktu, do

którego robot hybrydowy ma przemieścić frez/narzędzie. Jest to pięć liczb zadawanych

przez Operatora, z których trzy pierwsze to współrzędne X, Y i Z w metrach, a dwie

kolejne liczby to kąty obrotu wokół osi OX i OZ w radianach oznaczane jako A i C w

globalnym układzie współrzędnych. Podawana długość frezu to parametr

wprowadzany przez operatora po każdej wymianie narzędzia. Wartość tę można

modyfikować, jeśli robot hybrydowy jest w pozycji postojowej (Pozycja, do której

przejeżdża robot po zakończeniu procedury kalibracji). Kolejną daną wejściową jest

numer linii programu, od której program on-line ma rozpocząć generowanie trajektorii

na podstawie programu NC.

Głównym wynikiem pracy Generatora jest trajektoria opisująca realizację toru ruchu

narzędzia w czasie. Na wyjściu Generatora Trajektorii otrzymywane są wartości

położenia, prędkości i przyspieszenia, jakie aktualnie narzędzie oraz stół uchylno –

obrotowy powinny osiągać. Stanowią one dane wejściowe dla Sterownika Śledzenia

Trajektorii. Ponadto na wyjściu części on-line Generatora otrzymuje się:

Numer aktualnie wykonywanej linii programu NC,

Kod numeryczny aktualnie wykonywanego zadania,

Sygnały dla Sterownika Wrzeciona: sygnał włącz/wyłącz, sygnał kierunku

obrotów i prędkości obrotowej wrzeciona oraz dodatkowy sygnał włączający

chłodzenie miejsca skrawania,

Sygnały informacyjne, sygnalizujące zakończenie wykonywania programu NC

oraz zakończenie aktualnego segmentu.

Poniżej opisano zasadę działania części on-line. Generator Trajektorii analizuje

sygnały sterujące odebrane od Menadżera Zadań, począwszy od sygnału kalibracji

manipulatorów. Jeżeli robot hybrydowy nie jest skalibrowany, to Generator Trajektorii

pracuje w trybie awaryjnym i podaje na wyjściu niezmienioną, aktualną pozycję robota

równoległego i orientację stołu uchylno – obrotowego. Wartości prędkości i

przyspieszenia liniowe oraz kątowe są w tej sytuacji równe zeru.

94

Rysunek 35 Schemat blokowy części on-line Generatora Trajektorii.

Na tym etapie możliwe jest modyfikowanie przez Użytkownika numeru linii

programu NC, od której ma rozpocząć się generowanie trajektorii po zakończeniu

procesu kalibracji robota. Po otrzymaniu informacji o zakończeniu procedury kalibracji,

Pozycja zadana przez

Operatora

Sygnały sterujące

Tak

Nie

Sygnały błędu i

kalibracji

Aktualna pozycja

Tak Nie

Wyzerowanie prędkości i

przyspieszeń

Czy wykonano kalibrację?

Czy wystąpił błąd?

Obsługa trybu błędu

Czy uruchomiono wykonywanie programu NC?

Wyznaczenie profilu

przyspieszenia ruchu ustawczego

Rzutowanie chwilowych wartości na osie globalnego układu

współrzędnych z uwzględnieniem rotacji wokół osi OX i OZ

Nie

Tak Dane z części

off-line

Obliczenie chwilowej wartości

przemieszczenia, prędkości i

przyspieszenia

Ustawienie sygnałów

sterujących

Trajektoria dla Sterownika

Śledzenia Trajektorii

Sygnały sterujące

95

Generator Trajektorii przechodzi w tryb ręczny. W tym trybie manipulatory wykonują

ruchy ustawcze od pozycji aktualnej do pozycji zadanej przez Użytkownika.

Wykonywane jest to przez wygenerowanie pojedynczej instrukcji G-kodu z interpolacją

liniową. Ruchy te są wykonywane z maksymalną zadeklarowaną prędkością posuwu.

Wyznaczany jest podstawowy profil przyspieszenia z uwzględnieniem przemieszczenia,

jakie ma przebyć narzędzie przy założeniu, że prędkości początkowa i końcowa są

równe zero. Przygotowywany jest pakiet danych w takiej samej formie, w jakiej

przygotowywane są polecenia instrukcji G-kodu w pliku tekstowym generowanym w

części off-line. W zależności od sygnałów sterujących pracą Generatora analizowany

jest odpowiedni pakiet danych i realizowany jest albo ruch ustawczy, albo

wykonywane są polecenia programu NC. W obu tych przypadkach kolejnym etapem

jest wyznaczanie chwilowych wartości przemieszczenia, prędkości posuwu i

przyspieszenia liniowego – czyli interpolacja czasowa. Następnie te dane są rzutowane

na kształt segmentu w globalnym układzie współrzędnych, tworząc ostateczną

trajektorię dla Sterownika Śledzenia Trajektorii. Na Rysunku 35 przedstawiono schemat

blokowy działania części on-line Generatora Trajektorii.

Zadaniem Generatora Trajektorii jest również reagowanie na aktualną sytuację

podczas frezowania, np. w przypadku wystąpienia błędów w odwzorowaniu trajektorii

przez manipulatory. Gdy Menadżer Zadań zgłosi wystąpienie błędu w odwzorowaniu

pozycji, to Generator przechodzi w tryb obsługi sygnału błędu. Ta procedura składa się

z sekwencji ruchów, które manipulatory wykonują, dojeżdżając do pozycji postojowej:

oczekiwanie na dokończenie ruchu w aktualnym segmencie i

zatrzymanie dalszego wykonywania poleceń G-kodu,

wysłanie sygnału wyłączającego wrzeciono, przejazd z pozycji aktualnej

w kierunku osi Z na wysokość pozycji postojowej,

przejazd robota równoległego do pozycji postojowej,

ustawienie stołu uchylno – obrotowego w pozycji postojowej,

oczekiwanie na działania operatora. Jeżeli możliwa jest kontynuacja

procesu frezowania, to można zresetować błąd, a następnie wykonać

ruch ustawczy lub rozpocząć powrót do wykonywania instrukcji

programu NC,

96

w przypadku wyboru kontynuowania wykonywania poleceń programu

NC stół uchylno – obrotowy wykonuje rotację z pozycji postojowej do

pozycji odczytanej z programu NC w instrukcji poprzedzającej tę, w

której wystąpił błąd i włączane jest wrzeciono,

robot równoległy wykonuje przejazd translacyjny nad położenie

odczytane z instrukcji programu NC poprzedzającej wystąpienie błędu,

robot równoległy wykonuje ruchu wzdłuż osi Z do położenia

docelowego i rozpoczynane jest wykonywanie dalszej trajektorii zgodnie

z poleceniami programu NC.

Ruchy te mają na celu umożliwienie przejazdu narzędzia bez ryzyka uszkodzenia

frezu lub obrabianego przedmiotu. Sekwencja ta jest wykorzystywana również w

przypadku wywołania instrukcji wymiany narzędzia podczas frezowania.

Po wyznaczeniu wszystkich wartości chwilowych parametrów trajektorii są one

przekazywane do Kontrolera Śledzenia Trajektorii. Tym sposobem Generator

Trajektorii oblicza w każdym okresie próbkowania Układu Sterowania potrzebne

wartości trajektorii wzorcowej. Opracowane algorytmy realizują wszystkie założenia,

jakie postawiono przed Generatorem Trajektorii. Pozwalają na synchroniczną

współpracę dwóch manipulatorów wchodzących w skład robota hybrydowego,

umożliwiają korektę lokalizacji zamocowania obrabianego przedmiotu i obliczają

ostateczną trajektorię w globalnym układzie współrzędnych związanym z konstrukcją

robota. Algorytmy te również pozwalają na współpracę z pozostałymi elementami

Układu Sterowania. Przewidziane tryby pracy Generatora Trajektorii realizują

wymaganie dotyczące obsługi takich zdarzeń jak procedura kalibracji czy wystąpienie

sytuacji awaryjnych. Wyprowadzone wzory pozwalają obliczać wartości chwilowe

przemieszczeń i ich dwóch kolejnych pochodnych dla trzech osi translacyjnych oraz dla

dwóch rotacyjnych. Kolejnym etapem jest implementacja opracowanych algorytmów

w środowisku symulacyjnym oraz na docelowej platformie sprzętowej.

Jednym z ważniejszych aspektów działania części on-line jest wspominany w

poprzednim podrozdziale typ używanych zmiennych. Cała część Układu Sterowania,

pracująca w czasie rzeczywistym, wykonuje obliczenia używając zmiennych

pojedynczej precyzji typu float. Wykorzystując typ zmiennych double nie udawało się

osiągać wymaganej częstotliwości próbkowania Układu Sterowania przez wzgląd na

97

zbyt czasochłonne obliczenia numeryczne, co m.in. związane jest z rozmiarem

magistrali pamięci na docelowej platformie sprzętowej wynoszącym 32bity. Drugim

aspektem jest odpowiednia optymalizacja kodu programu pod kątem wydajności

obliczeniowej. Jednym z wymagań Układu sterowania jest ograniczona liczba operacji

matematycznych, jakie mogą być wykonywane przez Jednostkę Arytmetyczno -

Logiczną (z ang. ALU - Arithmetic Logic Unit). Do wykonywanych operacji zalicza się

działania: dodawania, odejmowania, mnożenia, dzielenia, pierwiastkowania, funkcje

trygonometryczne sinus i cosinus oraz funkcję signum.

Rysunek 36 Platforma sprzętowa z układem FPGA Staratix III firmy Altera wykorzystana do docelowej implementacji generatora trajektorii [43].

Optymalizacja kodu programu części on-line polegała na przekształceniu równań

pozwalających na obliczenie trajektorii robota hybrydowego w taki sposób, by

wykorzystywały jedynie dostępne operacje arytmetyczne, na przykład w podrozdziale

4.2.4 opisano sposób wyznaczania kąta (46) potrzebnego do wyznaczania przejazdu

synchronicznego robota równoległego i stołu uchylno – obrotowego. W części on-line

to równanie również jest wykorzystywane w przypadku trybu ręcznego. W równaniu

98

(46) powinna być użyta funkcja arcustangens, zamiast tego wyznaczane są wartości

funkcji sinus i cosinus kąta . Następnie w takiej postaci są przekazywane do

interpolacji przestrzennej, gdzie zostają wprost użyte w równaniach (56) i (57).

Koniecznym było dopracowanie algorytmów Generatora Trajektorii tak, by możliwym

było uruchomienie programu części on-line na tej platformie sprzętowej. Docelowa

platforma sprzętowa, na której zaimplementowano Układ Sterowania jest oparta na

przedstawionej na Rysunku 36 płycie DE3 z układem FPGA (Field – Programmable Gate

Array) Stratix III 3SL150 firmy Altera zawierającym 142 tysiące elementów logicznych

[43, 44, 45]. Z drugiej strony również funkcje wykorzystywane przez Generator

Trajektorii i niedostępnych w standardowej bibliotece kompilatora języka C wymuszały

realizację ich w inny sposób. W FPGA został zbudowany system wieloprocesorowy,

wykorzystujący 32 bitowe mikroprocesory Nios II, których ALU zostało rozbudowane o

akceleratory sprzętowe pozwalające szybko obliczyć wartości funkcji: pierwiastka

kwadratowego, sinus, cosinus i signum.

Implementacja opracowanych algorytmów w środowisku symulacyjnym pozwoliła

na sprawne testowanie programu pod kątem funkcjonalności i realizacji zadań przez

poszczególne algorytmy. Środowisko symulacyjne Matlab/Simulink pozwoliło na

szybkie wprowadzanie modyfikacji i skuteczne wykrywanie błędów, które pojawiały się

w procesie opracowywania kodu w języku C. Tak przygotowany program został

wdrożony na docelowej platformie sprzętowej. W kolejnym rozdziale zaprezentowane

zostaną wyniki eksperymentów generowania trajektorii dla robota hybrydowego do

frezowania, jakie osiągnięto z wykorzystaniem zarówno środowiska symulacyjnego

Matlab/Simulink, jak i platformy sprzętowej FPGA.

99

6. Testowanie Generatora Trajektorii

Niniejszy rozdział prezentuje wyniki testów, jakie przeprowadzono na dwóch

platformach: symulacyjnej i sprzętowej, na których zaimplementowano algorytmy

Generatora Trajektorii. Punkty generowanej trajektorii – będące wektorami

składającymi się z wartości położenia, prędkości i przyspieszenia wyznaczanych dla obu

manipulatorów – w globalnym układzie współrzędnych obliczane są w zadanych

odstępach czasu, równych okresowi próbkowania Układu Sterowania. By ocenić jakość

generowanej trajektorii, porównano ze sobą trajektorie wyznaczone na obu

platformach dla tego samego programu NC, przy czym trajektorię wyznaczoną w

środowisku symulacyjnym potraktowano jako wzorcową przez wzgląd na używanie w

tym środowisku liczb zmiennoprzecinkowych o wyższej precyzji (typu double).

6.1. Weryfikacja otrzymanej trajektorii w środowisku

symulacyjnym

Testy symulacyjne generacji trajektorii wykonano na komputerze klasy desktop z

wykorzystaniem pakietu obliczeniowego Matlab/Simulink (w wersji R2012b (8.0.0.783)

64-bit). Jako przykładowy program NC użyto pliku zawierającego 49 poleceń G-kodu,

służącego do wykreślenia trzech liter „AGH”. Każda z liter jest wykonana na

płaszczyźnie umieszczonej pod innym kątem w stosunku do pozostałych. Ten

stosunkowo prosty przykład pozwala na zaprezentowanie możliwości Generatora

Trajektorii, przy czym ilość danych otrzymanych w wyniku symulacji pozwala na

sprawne przeprowadzenie ich analizy. Na przykładzie symulacji numerycznej

omówiony zostanie również wpływ algorytmów części off-line (opisanych w rozdziale

4.2) na otrzymane wyniki.

Na Rysunku 37 przedstawiono otrzymany z symulacji za pomocą programu

Matlab/Simulink tor narzędzia, generowany na podstawie zadanego programu NC.

Dodatkowo na wykres naniesione zostały punkty określone przez instrukcje G-kodu –

odczytane przez algorytm interpretujący G-kod. Oznaczono je na Rysunku 37 jako

czarne znaczniki „X”. Każdy z segmentów rozpoczyna się i kończy w punkcie, który

określił Użytkownik w programie NC. Działanie algorytmu kompensacji pozycji

(podrozdział 4.2.2) można zaobserwować w wartościach współrzędnych, w jakich

100

została wygenerowana trajektoria. Oryginalne program NC jest definiowany w układzie

współrzędnych obrabianego przedmiotu, a w przypadku zaprezentowanego przykładu

układ ten został przesunięty o wektor [-0.083, -0.0335, 0.367]m wzdłuż osi X, Y i Z w

stosunku do globalnego układu współrzędnych robota hybrydowego do frezowania.

Współrzędne wektora przesunięcia wybrano tak, by wyśrodkować obliczaną trajektorię

w centrum płaszczyzny XY globalnego układu współrzędnych na wysokości

uwzględniającej mocowanie obrabianego przedmiotu.

Rysunek 37 Wykres toru ruchu narzędzia otrzymanego z wykorzystaniem symulacji numerycznej.

Na przykładzie litery „G” w wyznaczanej trajektorii zostanie zaprezentowane

działanie algorytmu wykrywania segmentów stycznych, opisanego w podrozdziale

4.2.3. Obrys litery „G” składa się między innymi z siedmiu segmentów kołowych: 4

segmentów zewnętrznych wykonywanych zgodnie z ruchem wskazówek zegara oraz 3

segmentów wewnętrznych wykonywanych przeciwnie do ruchu wskazówek zegara.

Oba te fragmenty toru ruchu narzędzia zostały opisane w G-kodzie w taki sposób, by

poszczególne segmenty były ze sobą połączone w sposób styczny, zapewniający

ciągłość wykonywanej trajektorii. Algorytm poprawnie zakwalifikował te fragmenty

jako styczne i wygenerowana trajektoria realizowana jest bez wyhamowań pomiędzy

poszczególnymi segmentami kołowymi. Zostało to zaprezentowane na Rysunku 38. W

pierwszej kolejności wykonywany jest przejazd liniowy. Następnie, po wyhamowaniu

101

do zera w punkcie nieciągłości, prędkość liniowa przyspiesza do wartości zadanej w

trakcie wykonywania zewnętrznego łuku litery „G”. Prędkość liniowa posuwu narzędzia

jest utrzymywana przez przejazd czterech segmentów kołowych, a następnie

wyhamowuje i zostaje wykonany krótki odcinek liniowy łączący oba fragmenty łukowe.

Rysunek 38 Wykres chwilowej prędkości liniowej ruchu narzędzia w trakcie wykonywania litery "G" w przykładowej trajektorii.

W następnej kolejności wykonywane są trzy segmenty kołowe, również jako

trajektoria ciągła, bez wyhamowań pomiędzy poszczególnymi segmentami. Kolejne

segmenty są już niestycznymi odcinkami liniowymi, na końcu ich realizacji chwilowa

prędkość liniowa posuwu za każdym razem jest wyhamowywana do zera.

Ostatni etap generacji trajektorii to już obszar działania programu on-line. W tym

etapie następuje obliczenie wartości chwilowych profilu przyspieszenia (dokończenie

interpolacji czasowej) oraz rzutowanie ich na translacje i rotacje w poszczególnych

osiach układu współrzędnych (interpolacja przestrzenna). Jako przykład poprawnego

przeprowadzenia etapu interpolacji przestrzennej zaprezentowano na Rysunkach 39,

40 i 41 fragmenty wygenerowanej trajektorii dla ruchu translacyjnego wzdłuż

poszczególnych osi globalnego układu współrzędnych. Na rysunkach 42 i 43

zaprezentowano fragmenty trajektorii dla ruchu rotacyjnego wokół osi OX i OZ

globalnego układu współrzędnych. Zgodnie z celem niniejszej pracy, obliczana jest

pełna trajektoria ruchu, a więc położenie, prędkość i przyspieszenie dla każdego cyklu

pracy (okresu próbkowania) Układu Sterowania. Wykresy położenia i prędkości są

wykresami ciągłymi i gładkimi. Wykres przyspieszenia jest wykresem ciągłym z

102

zachowaniem trapezoidalnego kształtu przebiegu. W Tabeli 4 przedstawiono

parametry wygenerowanej trajektorii.

Tabela 4 Parametry trajektorii wygenerowanej na potrzeby testu.

Parametry trajektorii Wartość

Liczba segmentów w trajektorii 49 (42 segmenty z interpolacją liniową i 7

segmentów z interpolacją kołową)

Czas realizacji trajektorii 22.8745s

Liczba wygenerowanych punktów 228746

Ilość danych 13 724kB dla zmiennych typu float

27 449kB dla zmiennych typu double

Rysunek 39 Wykresy fragmentu wygenerowanej trajektorii dla ruchu translacyjnego wzdłuż osi X.

103

Rysunek 40 Wykresy fragmentu wygenerowanej trajektorii dla ruchu translacyjnego wzdłuż osi Y.

Rysunek 41 Wykresy fragmentu wygenerowanej trajektorii dla ruchu translacyjnego wzdłuż osi Z.

104

Rysunek 42 Wykresy fragmentu wygenerowanej trajektorii dla ruchu rotacyjnego wokół osi OX.

Rysunek 43 Wykresy fragmentu wygenerowanej trajektorii dla ruchu rotacyjnego wokół osi OZ.

Wyznaczone wartości prędkości i przyspieszeń nie przekraczają założonych wartości

granicznych dla danego typu napędów (dla symulowanej trajektorii przyjęto

105

). Rysunki 42 i 43 prezentują fragmenty wygenerowanej

trajektorii, która ma być realizowana przez stół uchylno – obrotowy. Dotyczą one ruchu

rotacyjnego wokół osi OX i OZ globalnego układu współrzędnych. Zaprezentowany

został jeden z czterech segmentów w całej przykładowej trajektorii, w którym

wykorzystywany jest ruch obrotowy powodujący zmianę orientacji o

. Ruch ten jest wykonywany również z trapezoidalnym profilem

przyspieszenia. Przebieg położenia kątowego i prędkości kątowej są ciągłe i gładkie, a

przyspieszenie kątowe jest ciągłe. Wartości graniczne, wyznaczane w oparciu o

algorytm opisany w podrozdziale 4.2.4, nie zostały przekroczone. Po weryfikacji

symulacyjnej, gdy wszystkie założenia co do kształtu i przebiegu czasowego trajektorii

zostały sprawdzone, przetestowano poprawność generowania trajektorii przez

program części on-line Generatora na docelowej platformie sprzętowej, czyli na płycie

DE3.

6.2. Weryfikacja otrzymanej trajektorii na docelowej platformie

sprzętowej

Zważywszy na wyższy poziom precyzji obliczeń przeprowadzanych w sposób

symulacyjny z wykorzystaniem środowiska Matlab/Simulink, wyniki tej symulacji

posłużyły do weryfikacji poprawności trajektorii generowanej na docelowej platformie

sprzętowej. Istotną kwestią było odpowiednie dobranie sygnałów sterujących w taki

sposób, by w obu środowiskach testowych pojawiały się one w tym samym momencie.

Różnica w czasie nawet o jeden okres próbkowania Układu Sterowania powodowała

otrzymywanie różnic w rezultatach. Dodatkowo czas wykonywania danego segmentu,

jaki jest obliczany wewnątrz programu on-line realizowanego przez środowisko

symulacyjne Matlab/Simulink, musi również być wyznaczany z użyciem liczby

zmiennoprzecinkowej pojedynczej precyzji – typu float. Gdy używano do tego celu

zmiennej typu double zdarzało się, iż w wyniku błędów numerycznych niektóre

segmenty trwały o jedną próbkę dłużej niż w przypadku obliczeń wykonywanych na

liczbach typu float. Powodowało to przesunięcie w czasie dalszego ciągu trajektorii, co

bardzo utrudniało poprawną interpretację wyników.

106

Rysunek 44 Wykresy błędów numerycznych dla ruchu translacyjnego wzdłuż osi X dla trajektorii wygenerowanej na docelowej platformie sprzętowej.

Rysunek 45 Wykresy błędów numerycznych dla ruchu translacyjnego wzdłuż osi Y dla trajektorii wygenerowanej na docelowej platformie sprzętowej.

107

Rysunek 46 Wykresy błędów numerycznych dla ruchu translacyjnego wzdłuż osi Z dla trajektorii wygenerowanej na docelowej platformie sprzętowej.

Rysunek 47 Wykresy błędów numerycznych dla ruchów rotacyjnych wokół osi OX dla trajektorii wygenerowanej na docelowej platformie sprzętowej.

108

Rysunek 48 Wykresy błędów numerycznych dla ruchów rotacyjnych wokół osi OZ dla trajektorii wygenerowanej na docelowej platformie sprzętowej.

Na Rysunkach 44 48 przedstawiono wykresy błędów w generowanej trajektorii,

jakie wprowadza używanie przez platformę sprzętową liczb zmiennoprzecinkowych

pojedynczej precyzji w porównaniu do wyników otrzymanych w symulacji z

wykorzystaniem zmiennych o wyższym poziomie precyzji. Błędy zostały wyznaczone

dla położeń liniowych i kątowych oraz dla ich dwóch kolejnych pochodnych i

zestawione w Tabelach 5 i 6.

Tabela 5 Zestawienie maksymalnych wartości błędów numerycznych dla ruchu translacyjnego wygenerowanej trajektorii.

Wartość Osie

X Y Z

Położenie 3,40e-08 5,01e-07 7,33e-08

Prędkość

2,93e-07 2,25e-07 1,13e-06

Przyspieszenie

4,47e-08 1,00e-07 1,00e-07

Największy błąd numeryczny z osiąganych położeń ruchu translacyjnego

zanotowano dla ruchu wzdłuż osi Y globalnego układu współrzędnych (Rysunek 45),

109

wynosi on , jednak trzeba zwrócić uwagę na kształt wykresu błędu

numerycznego dla tej osi. Wysokie wartości błędów są osiągane jedynie w dwóch

miejscach trajektorii (segmenty wykonywane z interpolacją kołową, dla punktów w

których zmienna pomocnicza w równaniu (65) była bliska zeru), poza nimi

poziom błędu jest o rząd niższy. Błąd ten wynikał z sposobu realizacji przez ALU funkcji

sinus dla małych wartości kątów. W przypadku prędkości liniowej największy błąd

wynosi

, zarejestrowano go dla ruchu translacyjnego wzdłuż osi Z. Największy

błąd przyspieszenia liniowego odnotowano dla ruchów translacyjnych wzdłuż osi Y i Z

globalnego układu współrzędnych, wartość maksymalna błędu numerycznego wynosi

.

Tabela 6 Zestawienie maksymalnych wartości błędów numerycznych dla ruchu rotacyjnego wygenerowanej trajektorii.

Wartość Osie

A C

Położenie 6,43e-08 1,13e-07

Prędkość

3,00e-06 4,47e-08

Przyspieszenie

3,20e-07 3,20e-07

Wyniki zaprezentowane na Rysunkach 47 i 48 prezentują wykresy błędów

numerycznych trajektorii obliczanych dla ruchów rotacyjnych wokół osi OX i OZ

globalnego układu współrzędnych. Błąd położenia kątowego nie przekroczył wartości

osiągniętej dla ruchu obrotowego wokół osi OZ. Największy błąd prędkości

kątowej był mniejszy od

, wystąpił on dla obrotu wokół osi OX. Błąd

przyspieszenia kątowego wynosił mniej niż

i był identyczny dla ruchu

rotacyjnego wokół osi OX i OZ globalnego układu współrzędnych.

Kolejnym aspektem, który sprawdzono i był on kluczowy dla testów z

zastosowaniem platformy sprzętowej FPGA, jest czas, w jakim wykonywano obliczenia

numeryczne części on-line Generatora Trajektorii. Pomiary przeprowadzono w

zależności od typu interpolacji przestrzennej oraz dodatkowo zbadano czas

wykonywania obliczeń podczas pracy w trybie ręcznym. Dla danego typu interpolacji

wyznaczano czas wykonywania obliczeń dla każdego okresu próbkowania Układu

110

Sterowania, po czym zapisywany był wynik najwyższy. Pomiary wykonano dla tego

samego programu NC, który posłużył do wyznaczenia błędów numerycznych.

Maksymalne czasy realizacji obliczeń:

Dla przejazdów serwisowych – 63µs,

Dla interpolacji liniowej – 52µs,

Dla interpolacji kołowej – 55µs.

Wyznaczone błędy numeryczne w generowanej trajektorii są o dwa rzędy wielkości

niższe niż zakładana dokładność pozycjonowania robota hybrydowego do frezowania

równa 10µm. Wartości tych błędów wynikające z zastosowania w części on-line

Generatora Trajektorii liczb zmiennoprzecinkowych pojedynczej precyzji są na

akceptowalnym poziomie. Czasy obliczeń numerycznych dla każdego typu

zaimplementowanej interpolacji przestrzennej są mniejsze niż 100µs, jest to graniczna

wartość czasu trwania jednego cyklu pracy Układu Sterowania, by uzyskać

częstotliwość próbkowania równą 10kHz. W związku z tym Generator Trajektorii

spełnia założenia dotyczące implementacji na docelowej platformie sprzętowej.

111

7. Podsumowanie i wnioski

W ramach realizacji pracy opracowano Generator Trajektorii obliczający zadaną

trajektorię w przestrzeni kartezjańskiej w oparciu o instrukcje podawane w postaci G-

kodu przez Operatora lub z wykorzystaniem programów NC. Trajektoria ta jest

trajektorią pełną, czyli każdy jej punkt jest wektorem zawierającym położenie i jego

dwie kolejne pochodne w czasie dla każdej osi obrabiarki z założoną maksymalną

częstotliwością próbkowania, a więc założony cel pracy został osiągnięty. Żaden z

opisanych w literaturze generatorów trajektorii nie spełniał wymaganych przez robota

hybrydowego do frezowania założeń, np. generowana była pełna trajektoria z wysoką

częstotliwością, jednak dla manipulatora poruszającego się w tylko wzdłuż dwóch osi X

i Y lub opisywano generatory trajektorii dla obrabiarek pięcioosiowych, które

generowały trajektorię uwzględniającą tylko położenie i prędkość narzędzia oraz

pracujące z częstotliwościami próbkowania rzędu 100Hz. W ten sposób udowodniono

tezę, iż możliwym było opracowanie algorytmu Generatora Trajektorii spełniającego

jednocześnie wszystkie postawione przed nim założenia, tj.: generacja pełnej trajektorii

(położenia i jego dwóch pochodnych po czasie) dla pięciu osi w przestrzeni

kartezjańskiej, stosowanie interpolacji czasowej z trapezoidalnym profilem

przyspieszenia, implementacja on-line z wysoką częstotliwością próbkowania.

W ramach prac nad Generatorem Trajektorii opracowano i przetestowano

najpopularniejsze w literaturze przykłady interpolacji czasowej, a następnie na

podstawie wyników testów wybrano spośród nich profil z trapezoidalnym profilem

przyspieszenia jako typ interpolacji czasowej, który zastosowano w Generatorze

Trajektorii. Ten typ interpolacji jest kompromisem pomiędzy czasem realizacji

zadanego toru narzędzia, złożonością obliczeniową algorytmu oraz poziomem energii

wysokoczęstotliwościowych składowych sygnału przyspieszenia wpływających na

pojawianie się niekorzystnych drgań podczas realizacji procesu skrawania. Do realizacji

w programie Generatora Trajektorii wybrano dwa rodzaje interpolacji przestrzennej –

liniową i kołową – gdyż tylko te dwie obliczane są na tyle szybko, by podołać

wymaganemu czasowi trwania pojedynczego cyklu Układu Sterowania. Opracowano i

przetestowano również algorytmy realizujące interpolacje z wykorzystaniem krzywych

wielomianowych oraz krzywych B-sklejanych, jednak te typy interpolacji są typami,

112

których złożoność powoduje zbyt długi czas wykonywania obliczeń numerycznych dla

pojedynczego punktu trajektorii. By zwiększyć szybkość pracy Generatora Trajektorii,

podzielono go na dwie części: część off-line i on-line, dzięki czemu wszystkie

czasochłonne obliczenia, które można wykonać wcześniej, są realizowane zanim robot

hybrydowy rozpocznie pracę. W ramach działania części off-line Generator Trajektorii:

interpretuje polecenia G-kodu, wyznacza pozycje punktów początkowych i końcowych

w poszczególnych segmentach toru w globalnym układzie współrzędnych z

uwzględnieniem współczynników korygujących umocowanie przedmiotu na stole

uchylno – obrotowym, identyfikuje segmenty toru ruchu styczne względem siebie (co

pozwala na niewytracanie prędkości przy ich wykonywaniu), wyznacza profil

przyspieszenia dla każdego segmentu z uwzględnieniem wartości granicznych. Część

on-line działa z taką samą częstotliwością próbkowania jak Jądro Układu Sterowania.

Ta część Generatora Trajektorii odpowiedzialna jest za wyznaczanie w każdej chwili

czasowej wartości pełnej trajektorii, czyli położenia, prędkości i przyspieszenia, a

następnie rzutowanie tych wartości odpowiednio na położenia liniowe i kątowe wokół

osi globalnego układu współrzędnych. Sposób rzutowania wybierany jest w zależności

od zadeklarowanego typu interpolacji przestrzennej, który był przyjęty dla danego

segmentu. Opracowane algorytmy zostały zaimplementowane w języku C, co pozwoliło

na ich realizację (wykonywanie) w symulacyjnym środowisku Matlab/Simulink oraz na

docelowej platformie sprzętowej, czyli w układzie FPGA z rodziny Stratix III firmy

Altera. Przeprowadzone testy symulacyjne wykazały poprawność generowanej

trajektorii oraz właściwej pracy opracowanych algorytmów. Pozwoliło to

przeprowadzić test porównawczy z wynikami uzyskanymi za pomocą docelowej

platformy sprzętowej.

Układ Sterowania pięcioosiowego robota hybrydowego do frezowania, by sprostać

wymaganiom technologicznym obróbki skrawaniem typu HSM, musi pracować z

wysokimi częstotliwościami próbkowania dla osiągania mikrometrycznej dokładności

ruchu przy zachowaniu dużej przestrzeni roboczej oraz wysokich prędkości posuwu

narzędzia, jednak sprostanie temu wymaganiu pociąga za sobą ograniczenia dotyczące

złożoności możliwych do realizacji interpolacji przestrzennych. W opracowanym

Generatorze Trajektorii ten problem rozwiązano poprzez opracowanie algorytmu

wykrywającego segmenty, pomiędzy którymi przejście może być realizowane jako

113

gładkie. Dotychczas detekcja przejść pomiędzy segmentami, które należy wykonać bez

zmniejszania prędkości liniowej narzędzia do zera, spoczywała na Operatorze

tworzącym program NC. W opracowanym Generatorze Trajektorii procedura

wyznaczania stycznych przejść pomiędzy segmentami toru narzędzia jest wykonywana

w pełni automatycznie. Opracowany Generator Trajektorii został przez autora

przystosowany do realizacji ruchów ustawczych i pomocniczych przejazdów

serwisowych pozwalających na wymianę narzędzia frezującego poprzez generowanie

na bieżąco odpowiednich poleceń G-kodu. Dzięki temu Generator Trajektorii (a

przynajmniej jego zasadnicza część) pracuje w tej samej postaci cały czas, gdy robot

hybrydowy jest włączony.

Generator trajektorii został opracowany dla robota hybrydowego do frezowania, ale

możliwe są z jego wykorzystaniem, po zmianie narzędzia, inne rodzaje obróbki, np.

cięcie wodne lub laserowe. Generator pracuje w globalnym układzie współrzędnych w

przestrzeni kartezjańskiej, więc może być wykorzystany dla innych struktur

kinematycznych urządzeń do mikromanipulacji, w tym obrabiarek, jeśli znane są dla

nich rozwiązania zadania odwrotnego kinematyki.

Postęp w opracowywanych technologiach produkcji wzrasta nieustannie. Dąży się

do coraz wyższych dokładności obróbki skrawaniem, jak i do zwiększania efektywności

produkcji. Algorytm Generatora Trajektorii można bardziej rozbudować o algorytm

wygładzający przejścia pomiędzy dwoma dowolnymi segmentami, a nie tylko tymi

stycznymi do siebie. Możliwym również jest zaadaptowanie opracowanego Generatora

Trajektorii do współpracy z innego typu manipulatorem. Gdyby wymagany czas

próbkowania nie był tak restrykcyjny, można by było zaimplementować w Generatorze

Trajektorii interpolacje przestrzenne, które nie mogły być użyte w robocie

hybrydowym do frezowania. Autor rozpoczął prace nad rozbudowaniem

opracowanego Generatora Trajektorii o wielomianową interpolację przestrzenną,

jednak jej praktyczne zastosowanie wymaga również intensywnych prac nad znaczną

akceleracją obliczeń niezbędnych dla tej interpolacji przestrzennej (takich jak:

obliczenia macierzowe oraz numeryczne metody wyznania miejsc zerowych funkcji

wielomianowych).

114

8. Bibliografia

[1] Cédric Clévy, Micky Rakotondrabe, and Nicolas Chaillet. Signal Measurement and

Estimation Techniques for Micro and Nanotechnology. Springer-Verlag New York,

2011.

[2] Toshio Fukuda, Fumihito Arai, and Masahiro Nakajima. Micro-Nanorobotic

Manipulation Systems and Their Applications. Springer Science & Business

Media, 2013.

[3] Okuma Corp. Cosmo Center PM-600.

http://www.metrom.com/index.php?id=16&L=1. online, 2015.

[4] Benov I.2000. Company profile of Toyoda Machine Works Ltd.:Related Products.

ParalleMIC. http://www.parallemic.org/ WhosWho/Companies/Profile002.html.

online, 2015.

[5] Metrom Mechatronische Mashinen GmbH. Metrom model P 1000.

http://www.metrom.com/index.php?id=16&L=1. online, 2015.

[6] Design World Staff. Tricept parallel kinematic machine system.

http://www.designworldonline.com/Tricept-T9000-Parallel-Kinematic-Machine-

System/. online, 2015.

[7] Robot Worx. ABB IRB 940 Robot. Robot Works 2015. http://www.used-

robots.com/used-abb-robots.php?robot=irb+940. online, 2015.

[8] Lapic LLC. Design and production coordinate measuring machines and measuring

equipment. http://www.lapic.ru/eng/. online, 2015.

[9] S. Sona, T. Kimb, S. E. Sarmac, and A. Slocumc. A hybrid 5-axis CNC milling

machine. Precision Engineering, 33:430 – 446, 2009.

[10] Y. Shneor. Stiffness of 5-axis machines with serial, parallel, and hybrid

kinematics: Evaluation and comparison. CIRP Annals - Manufacturing Tech,

59:409 – 412, 2010.

[11] J. Guo, L. Zhao, L. Dong, and Z. Sheng. The Analysis on the Processing Dexterity of

a 3-TPT Parallel Machine Tool. Procedia Engineering, 15:298 – 302, 2011.

[12] Myriam Terrier, Arnaud Dugas, and Jean-Yves Hascoët. Qualification of parallel

kinematics machines in high-speed milling on free form surfaces. International

Journal of Machine Tools & Manufacture, (44), 2004.

115

[13] G. Karpiel, M. Petko, and T. Uhl. Three-arm parallel manipulator, Patent RP nr PL

203 631 B1 (B25J18/00), 04 2009.

[14] G. Karpiel, M. Petko, and T. Uhl. Triple articulated joint, Patent RP nr PL 207 396

B1 (F16C11/00), 07 2010.

[15] G. Karpiel, M. Petko, and T. Uhl. Tripod parallel micromanipulator, Patent RP nr

PL 208 563 B1 (B25J18/04; B25J18/02), 11 2010.

[16] Seamus Gordon and Michael T. Hillery. Development of a high-speed CNC cutting

machine using linear motors. Journal of Materials Processing Technology,

166(3):321 – 329, 2005.

[17] Automation systems and integration – Numerical control of machines – Program

format and definitions of address words – Part 1: Data format for positioning,

line motion and contouring control systems, ISO 6983-1:2009.

[18] Suk-Hwan Suh, Seong-Kyoon Kang, Dae-Hyuk Chung, and Ian Stroud. Theory and

Design of CNC Systems. Springer, 2008.

[19] Sylvain Lavernhe, Xinbin Duan, Christophe Tournier, and Claire Lartigue.

Performance of off-line polynomial CNC trajectories within the context of HSM.

In ASME Design Engineering Technical Conferences, 2004.

[20] Luigi Biagiotti and Claudio Melchiorri. FIR filters for online trajectory planning

with time - and frequency - domain specifications. Control Engineering Practice,

(20):1385–1399, 2012.

[21] Luigi Biagiotti and Claudio Melchiorri. Trajectory Planning for Automatic

Machines and Robots. Springer Berlin Heidelberg, 2008.

[22] Kaan Erkorkmaz and Yusuf Altintas. High speed CNC system design. Part I: jerk

limited trajectory generation and quintic spline interpolation. International

Journal of Machine Tools & Manufacture, (41):1323–1345, 2001.

[23] Roque Alfredo Osornio-Rios, René de Jesús Romero-Troncoso, Gilberto Herrera-

Ruiza, and Rodrigo Castañeda-Miranda. FPGA implementation of higher degree

polynomial acceleration profiles for peak jerk reduction in servomotors. Robotics

and Computer-Integrated Manufacturing, (25):379–392, 2009.

[24] Leszek Rutkowski, Andrzej Przybył, and Krzysztof Cpałka. Novel Online Speed

Profile Generation for Industrial Machine Tool Based on Flexible Neuro-Fuzzy

Approximation. IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS, 59(2), 2012.

116

[25] Li Huiying and He Hongxia. Overview on common interpolation methods used in

cnc systems. In International Conference on Computer Application and System

Modeling, 2010.

[26] Hongke Li, Huaishu Li, Lizhong Song, Yang Yin, Linshu Huang, and Wenyan Li.

Design of Global Sliding-Mode Controlled AC Servo Controller Based on

Exponential Acceleration/Deceleration Algorithm. In International Conference on

Mechatronics and Automation, 2010.

[27] Laurent Tapie, Kwamiwi Mawussi, and Bernard Anselmetti. Circular tests for HSM

machine tools: Bore machining application. 2009.

[28] Bo Li, Qingjian Liu, Lu Liu, Jingchuan Dong, and Taiyong Wang. Study of

trigonometric function improved acceleration -deceleration algorithm and

velocity look-ahead control. Key Engineering Materials, 584:184–188, 2014.

[29] Guoyong Zhao, Yugang Zhao, and Shijun Wang. The Acceleration/Deceleration

Control Algorithm Based on Trapezoid-Curve Jerk in CNC Machining. Research

Journal of Applied Sciences, Engineering and Technology, 5(5), 2013.

[30] Maszyny sterowane numerycznie – Osie współrzędnych i zwroty ruchów – Nazwy

i oznaczenia PN-M-55251:1993.

[31] LiXian Zhang, RuiYong Sun, XiaoShan Gao, and HongBo Li. High speed

interpolation for micro-line trajectory and adaptive real-time look-ahead scheme

in CNC machining. Sci China Tech Sci, 2011.

[32] Dong Jingchuan, Liu Qingjian, and Wang Taiyong. Research on Subdivision

Interpolation for High Speed CNC Applications. Applied Mechanics and Materials,

141:449–454, 2012.

[33] Armelle Affouard, Christophe Tournier, Sylvain Lavernhe, and Claire Lartigue.

Description formats of tool trajectory suited to High-speed machining.

International Conference on High Speed Machining, 2004.

[34] Zhang Ke, Guo Jian-Xin, and Gao Xiao-Shan. Cubic spline trajectory generation

with axis jerk and tracking error constraints. INTERNATIONAL JOURNAL OF

PRECISION ENGINEERING AND MANUFACTURING, 14(7):1141–1146, july 2013.

[35] B. Sencer, Y. Altintas, and E. Croft. Feed optimization for five-axis CNC machine

tools with drive constraints. International Journal of Machine Tools &

Manufacture, (48):733–745, 2008.

117

[36] L. Luo, L. Wang, and J. Hu. On the Modeling and Analysis of an Improved CNC

Interpolation Algorithm. Materials Science Forum, 626-627:459–464, 2009.

[37] Xun Xu. Integrating Advanced Computer-Aided Design, Manufacturing, and

Numerical Control: Principles and Implementations. IGI Global, 2009.

[38] Les Piegl and Wayne Tiller. The NURBS Book (2Nd Ed.). Springer-Verlag New York,

Inc., New York, NY, USA, 1997.

[39] Hacene Ameddah and Mekki Assas. NURBS Interpolation Strategies of Complex

Surfaces in High Speed Machining. International Journal of CAD/CAM, 11(1),

2011.

[40] Yongzhang Wang, Xiongbo Ma, Liangji Chen, and Zhenyu Han. Realization

Methodology of a 5-axis Spline Interpolator in an Open CNC System. Chinese

Journal of Aeronautics, (20):362–369, 2007.

[41] Alexander Yuen, Ke Zhang, and Yusuf Altintas. Smooth trajectory generation for

five-axis machine tools. International Journal of Machine Tools & Manufacture,

(71):11–19, 2013.

[42] J. Giergiel and T. Buratowski. Podstawy Robotyki. Number ISBN 83-916598-0-1.

Akademia Górniczo-Hutnicza w Krakowie, 2004.

[43] ALTERA, 101 Innovation Drive, San Jose, CA 95134. Stratix III Development Kit

User Guide, August 2008.

[44] René de Jesús Romero-Troncoso, Gilberto Herrera-Ruiz, Iván Terol-Villalobos,

and Juan Carlos Jáuregui-Correa. FPGA based on-line tool breakage detection

system for CNC milling machines. Mechatronics, 14(4):439–454, 2004.

[45] Dongdong Li, Liming Wu, Gaofeng Zhu, and Nanhui Lai. Design of reconfigurable

CNC system based on FPGA. In Electronic Measurement and Instruments, 2007.

ICEMI’07. 8th International Conference on, pages 4–857. IEEE, 2007.