Generacja trajektorii dla mikromanipulacji za pomocą robotów
Transcript of Generacja trajektorii dla mikromanipulacji za pomocą robotów
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.