Grzegorz Karpiel - Praca Doktorska - Biblioteka...

AKADEMIA GÓRNICZO-HUTNICZA

im. Stanisława Staszica w Krakowie

Wydział InŜynierii Mechanicznej i Robotyki

Katedra Robotyki i Mechatroniki

PRACA DOKTORSKA

ZASTOSOWANIE PODEJŚCIA MECHATRONICZNEGO W

PROJEKTOWANIU ROBOTÓW RÓWNOLEGŁYCH

mgr inŜ. Grzegorz Karpiel

Promotor pracy

prof. dr hab. inŜ. Tadeusz Uhl

Kraków 2006

Serdeczne podziękowania składam promotorowi Panu

prof. Tadeuszowi Uhlowi za pomoc, opiekę naukową i

cenne konsultacje udzielane mi podczas tworzenia pracy.

Wyrazy podziękowania dla wszystkich współpracowników

z Katedry Robotyki i Mechatroniki AGH, którzy przyczynili

się do powstania pracy w trakcie wielu interesujących

dyskusji i rozmów.

Kraków 2006 autor

Grzegorz Karpiel – Praca Doktorska

- 3 -

Spis Treści

1. Wstęp................................................................................................................................... 5

2. Cel i zakres Pracy ................................................................................................................ 9

3. Mechatronika w projektowaniu robotów........................................................................... 11

3.1. Podejście mechatroniczne........................................................................................... 13

3.2. Procedura projektowania robotów.............................................................................. 16

3.3. Szybkie prototypowanie ............................................................................................. 17

3.4. Implementacja w FPGA ............................................................................................. 19

3.5. Implementacja jako „System-on-a-Programmable-Chip”.......................................... 22

4. Manipulatory równoległe .................................................................................................. 25

4.1. Manipulatory oparte na platformie ............................................................................. 26

4.2. Manipulatory oparte na równoległobokach................................................................ 27

4.3. Wybrane manipulatory równoległe ............................................................................ 28

5. Automatyczna generacja kodu C....................................................................................... 29

5.1. Typy liczb................................................................................................................... 29

5.2. Generator Kodu .......................................................................................................... 32

5.2.1. ZałoŜenia dla generator ....................................................................................... 32

5.2.2. Metoda generacji kodu ........................................................................................ 33

5.2.3. Typy bloków Simulinka moŜliwych do przetransformowania na kod C++........ 37

5.2.4. Rezultat działania opracowanego generatora ...................................................... 45

5.2.5. Ograniczenia programu ....................................................................................... 46

5.3. Przykład zastosowania opracowanego generatora ..................................................... 48

5.3.1. Obiekt sterowania ................................................................................................ 48

5.3.2. śądany algorytm sterowania ............................................................................... 48

5.3.3. Automatyczna implementacja sterownika........................................................... 50

6. Projekt robota równoległego ............................................................................................. 52

6.1. Rozwiązanie konstrukcyjne robota równoległego...................................................... 52

6.1.1. Robot równoległy przeznaczony do paletyzacji.................................................. 53

6.1.2. Robot równoległy jako konstrukcja wsporcza obrabiarki ................................... 59

6.2. Model.......................................................................................................................... 62

6.2.1. Zadanie odwrotne kinematyki ............................................................................. 62

6.2.2. Zadanie proste kinematyki................................................................................... 66

6.2.3. Pierwsza i druga pochodna równania kinematyki odwrotnej.............................. 70


- 4 -

6.2.4. ZaleŜność miedzy siłami w napędach a siłą zredukowaną do środka przegubu

głównego w układzie kartezjańskim.............................................................................. 75

6.2.5. Wpływ siły grawitacji na siły generowane w napędach...................................... 81

6.2.6. Model dynamiki odwrotnej robota ...................................................................... 90

6.3. Układ sterowania projektowanego robota równoległego ........................................... 92

6.3.1. Algorytm sterowania ........................................................................................... 92

6.3.2. Wirtualne prototypowanie i szybkie prototypowanie.......................................... 93

6.3.3. Sprzętowo-programowa implementacja algorytmów sterowania....................... 97

6.3.4. Realizacja obliczeń stałoprzecinkowych........................................................... 101

6.3.5. Realizacja obliczeń zmiennoprzecinkowych..................................................... 104

6.3.6. Struktura zaprojektowanaego układu FPGA ..................................................... 108

6.3.7. System wymiany danych pomiędzy PC a FPGA .............................................. 110

6.3.8. Wyniki implementacji sterownika w układzie FPGA....................................... 113

6.3.9. Rezultat działania sterownika............................................................................ 115

7. Wnioski............................................................................................................................ 120

8. Bibliografia...................................................................................................................... 122


- 5 -

1. Wstęp

Rozwój technologiczny oraz postęp cywilizacyjny pociąga za sobą zwiększanie

potrzeb, co do automatyzacji systemów produkcyjnych. Rosnące wymagania klientów, ich

zróŜnicowanie, oraz konkurencyjność na rynku wymuszają na producentach podnoszenie

jakości wytwarzanych produktów, oraz nieustanne poszerzanie ich gamy oferowanych

towarów. Z kolei taka sytuacja zmusza projektantów urządzeń produkcyjnych do

projektowania maszyn oferujących duŜą elastyczność i wszechstronność w realizacji

postawionych zadań. W przypadku manipulatorów lub robotów objawia się to ciągłym

podnoszeniem parametrów eksploatacyjnych takich jak dokładność oraz szybkość działania,

a róŜnorodność zastosowań wymaga nieustannego rozbudowywania systemów

nadzorujących proces pracy robota. Zwiększenie dokładności i szybkości działania moŜna

uzyskać między innymi poprzez zastosowanie robotów równoległych, w których podane

właściwości wynikają z ich konstrukcji. JednakŜe specyficzna budowa takich robotów

wpływa na trudność procesu projektowania. Połączenia między członami mechanicznymi

tych robotów muszą być specjalnie projektowane, napędy pracują często w nietypowych

warunkach, a równania opisujące zaleŜności geometryczne i dynamiczne są skomplikowane

a często bardzo trudne do wyprowadzenia, co znacznie utrudnia proces projektowania ich

sterowania. Systemy nadrzędne kontrolujące zadania grup robotów muszą brać pod uwagę

cechy, które nie występują przy robotach o strukturze otwartej, jak choćby wewnętrzne

kolizje i związane z tym skomplikowane przestrzenie robocze. Łatwo, więc wysunąć

wniosek, Ŝe uŜyteczność dokładności i szybkości działania robotów równoległych będzie

korzystna, jeŜeli moŜliwość ich projektowania i ich zastosowania będzie porównywalna z

klasycznymi rozwiązaniami.

Robot równoległy z racji specyfiki swej budowy posiada kilka ramion połączonych

w wspólnym punkcie zwanym bazą. Znając bogatą grupę mechanizmów zamkniętych

wydaje się, więc Ŝe projektowanie takich konstrukcji jest proste i zasadniczą sprawą jest

poprawne uwzględnienie aspektów mechanicznych. Taki pogląd moŜna łatwo obalić

poprzez pokazanie róŜnic pomiędzy mechanizmem zamkniętym a robotem równoległym z

punktu widzenia napędów i sterowania. Po pierwsze bardzo często ramiona zawierają w

sobie napędy elektryczne, które to wchodzą w skład ich konstrukcji nośnej. Oznacza to, Ŝe

faworyzując zagadnienia związane z mechaniką moŜe okazać się, Ŝe nie istnieją na rynku

napędy, które posiadają odpowiednią wytrzymałość i zapas mocy potrzebny do realizacji

ruchu robota. Z kolei, jeśli rozpocznie się projektowanie od strony elektrycznej i bazując na


- 6 -

dostępnych parametrach elektrycznych napędów dobierać elementy mechaniczne, to moŜe

się okazać się, Ŝe nie istnieją materiały, z których moŜna wykonać przeguby lub inne

elementy robota, które to będą w stanie przenieść występujące obciąŜenia. Dodatkowo

sprawę komplikuje fakt, Ŝe nawet, jeśli istnieje kompromis pomiędzy elementami

mechanicznymi i elektrycznymi dla danej konstrukcji to równania matematyczne opisujące

takiego robota mogą być na tyle skompilowane, Ŝe zbudowanie układu sterowania stanie się

nie moŜliwe. Bo albo nie będzie moŜliwe wykonanie obliczeń w czasie rzeczywistym, albo

dokonane ich uproszczenia będą na tyle duŜe, Ŝe uŜycie ich w układzie sterowania znacznie

pogorszy istotne parametry robota takie jak np. dokładność. W rezultacie moŜe nastąpić

dylemat, co do celowości budowania danego robota równoległego.

Jak pokazują dotychczasowe doświadczenia autora osiągniecie wysokiej

efektywności w projektowaniu robotów moŜna osiągnąć poprzez zastosowanie podejścia

mechatronicznego. Oczywiste jest, Ŝe bazując na podejściu mechatronicznym, które zakłada

jednoczesne uwzględnienie róŜnych aspektów z dziedzin takich jak mechanika, elektronika i

sterowanie, poprawia się jakość projektowanego produktu. Jedyne pytanie, jakie moŜe się

nasunąć to czy dostępne narzędzia wspomagające projektowanie pozwolą w pełni

wykorzystać walory podejścia mechatronicznego i czy moŜliwe jest wdroŜenie procedur

takiego projektowania dla projektowanego urządzenia.

Patrząc na współczesne trendy wydaje się, Ŝe dobrym i skutecznym kierunkiem jest

stworzenie odpowiedniego środowiska informatycznego, które pozwoli na prowadzenie

projektu w róŜnych dziedzinach techniki oraz pozwoli na dołączenie innych

specjalistycznych narzędzi, które bardzo często towarzyszą projektowaniu złoŜonych

urządzeń. Takim środowiskiem jest MATLAB wraz z nakładką Simulink. Mnogość

narzędzi pozwalających na współpracę z nim pozwala na jednoczesne badanie róŜnych cech

projektowanego robota na poziomie wirtualnego prototypowania. Pozostaje tylko do

rozwiązania problem zaprojektowania i implementacji układu sterowania. Problem wynika

ze znacznej złoŜoności operacji matematycznych wymaganych do wykonania przez finalny

układ sterowania. Niezbędne obliczenia moŜna zrealizować albo poprzez stosowanie

specjalizowanych układów scalonych albo poprzez konstrukcje wydajnych systemów

obliczeniowych. Specjalizowane układy scalone są opłacalne produkcyjnie w skali

milionów sztuk, a przy takiej ilości trudno jest opracować jeden układ zaspokajający

róŜnorodność potrzeb związanych ze sterowaniem robotów. Z kolei budowanie

zaawansowanych systemów obliczeniowych, wymaga sporych jednostkowych nakładów

finansowych związanych z budową oraz przygotowaniem systemu do pracy jako układ


- 7 -

sterowania. Kompromisem tego problemu mogą być reprogramowalne układy ASIC/FPGA

(ang. Application-Specific Integrated Circuit/Field Programmable Gate Array) o zasobach

pozwalających jednocześnie realizować algorytmy zarówno w postaci sprzętowej jak i

programowej. Elastyczność w doborze dedykowanej konfiguracji pomiędzy sprzętem i

oprogramowaniem jest niezwykle cennym atutem, zwłaszcza w połączeniu z moŜliwością

wielokrotnego zmieniania dobranej konfiguracji. Pozostaje tylko do rozwiązania problem, w

jaki sposób powiązać specjalistyczne oprogramowanie dla układów FPGA z

oprogramowaniem wszechstronnym tj MATLAB pozwalającym realizować zadania

mechatroniczne, oraz jak naleŜy przeprowadzać proces projektowania układu sterowania,

aby cały czas mieć moŜliwość jego testowania wraz z tworzonymi w ramach projektu

elementami zawierających mechanikę i elektronikę.

Dotychczasowe prace na temat wykorzystania podejścia mechatronicznego w

projektowaniu robotów bazują głównie na doświadczeniach w projektowaniu robotów o

strukturze otwartej. Jest to naturalnym wynikiem wysokiej popularności takich robotów w

zastosowaniach przemysłowych. Prostota stosowanych tam rozwiązań w oczywisty sposób

pociąga za sobą krótszy czas projektu i znacznie zmniejsza nakłady finansowe. Nawet

zagadnienia związane z implementacją układu sterowania w układach FPGA dla robotów o

strukturze otwartej, wydają się być łatwiejsze, gdyŜ często bazując na prostych i łatwo

implementowalnych algorytmach uzyskują satysfakcjonujące rezultaty. Niestety metody

projektowania stosowane w projektowaniu robotów o strukturze otwartej nie nadają się do

bezpośredniego uŜycia dla robotów o strukturze zamkniętej, ze względu na ich większą

złoŜoność. I mimo ogromnego potencjału ukrytego we właściwościach robotów

równoległych, trudności związane ze znacznie bardziej rozbudowanym projektem stają się

powaŜną barierą w ich stosowaniu. Ogólnie proponowane rozwiązania zagadnień

związanych z tematyką robotów równoległych zawierają tylko fragmentaryczną wiedzę z

punktu widzenia całego projektu, a precyzyjne informacje związane z implementacją układu

sterowania dla takich konstrukcji są przemilczane. Efektem tego, brak jest prac gdzie byłby

pokazany cały proces projektowania robota równoległego, wykorzystujący podejście

mechatroniczne, z uwypukleniem problemów z tym związanych oraz z przedstawieniem ich

rozwiązań.

Taki stan rzeczy, skłonił autora do przeprowadzenia pracy, w której bazując na

rzeczywistym obiekcie, zostanie przeprowadzona procedura podejścia mechatronicznego,

gdzie nierozwiązane dotąd zagadnienia związane z takim podejściem, ale istotne z punktu

widzenia projektowania robotów równoległych, zostaną podkreślone, oraz zostaną


- 8 -

przedstawione przykładowe rozwiązania wraz z narzędziami pomocnymi w ich realizacji.

PoniewaŜ obecnie dostępne narzędzia nie pozwalają na w pełni wykorzystanie zalet

płynących z podejścia mechatronicznego, to istnieje potrzeba stworzenia takiego narzędzia

informatycznego, które pozwali na integrację w procesie projektowania robotów

równoległych, elementów mechanicznych, układu sterowania oraz elektroniki. Szczególnie

waŜne jest to w procesie implementacji dowolnej struktury układu sterowania. Gdzie brak

jest narzędzi pozwalających projektować układ sterowania wraz z elektroniką, przy

jednoczesnym powstawaniu części mechanicznej.


- 9 -

2. Cel i zakres Pracy

Celem niniejszej pracy jest opracowanie i implementacja układu sterowania dla robota

równoległego typu tripod w oparciu o opracowanie narzędzie informatyczne wspomagające

proces jego implementacji.

W pracy zostały zrealizowane następujące zadania badawcze:

• opracowanie oprogramowania pozwalającego na automatyczną generację kodu C na

podstawie schematu Simulinka zawierającego stałoprzecinkowe operacje

matematyczne

• sformułowanie równań opisujących zadanie proste i odwrotne robota równoległego

• sformułowanie zaleŜności pozwalających na budowę modelu dynamicznego robota

• opracowanie układu sterowania robotem równoległym opartego na jego modelu

dynamicznym

• implementacja algorytmu sterowania w środowisku MATLAB/Simulink

• symulacyjna weryfikacja algorytmu sterowania w procesie wirtualnego

prototypowania

• opracowanie procedur kalibracji oraz systemu bezpieczeństwa pracy robota

• szybkie prototypowanie układu sterowania i jego eksperymentalna weryfikacja na

rzeczywistym obiekcie

• opracowanie metody implementacji algorytmu sterowania z wykorzystaniem

oprogramowania i sprzętu w jednym układzie FPGA

• opracowanie własnych instrukcji wspomagających obliczenia (dla soft-procesora)

• opracowanie oprogramowania będącego interfejsem pomiędzy komputerem PC a

układem FPGA

• implementacja zaprojektowanego układu sterowania robotem równoległym w

pakiecie MATLAB /Simulink z wykorzystaniem układu FPGA

• weryfikacja działania układu sterowania na modelu fizycznym robota

Praca podzielona została na osiem rozdziałów zawierających opis realizacji

wymienionych wyŜej zadań badawczych.

Rozdział pierwszy jest wprowadzeniem w problematykę projektowania układów

sterowania dla robotów równoległych. W rozdziale tym przedstawiono uzasadnienie dla

podjęcia realizacji pracy.


- 10 -

W rozdziale drugim przedstawiony został cel i zakres pracy, a takŜe krótki przegląd

treści poszczególnych rozdziałów.

Rozdział trzeci zawiera wyjaśnienie zastosowanego podejścia mechatronicznego w

projektowaniu robotów oraz przedstawia procedury projektowania moŜliwe do

wykorzystania w projektowaniu mechatronicznym.

W rozdziale czwartym przedstawiono cechy robotów równoległych oraz pokazano

przykładowe rozwiązania konstrukcyjne.

Rozdział piąty i szósty zawiera zasadniczą część pracy. W rozdziale piątym opisano

opracowane przez autora narzędzie informatyczne, które to ułatwia implementacje

algorytmów sterowania w układach FPGA. Narzędzie informatyczne w połączeniu z

dostępnymi komercyjnymi narzędziami, umoŜliwia nieosiągalną dotąd automatyzację

procesu przenoszenia algorytmu sterowania ze środowiska MATLAB /Simulink do układu

FPGA, oraz pozwala na jednoczesną symulację projektowanego sprzętu wraz z częścią

mechaniczną, co uwalnia projektanta od zagłębiania się w szczegóły projektowania

specjalistycznego sprzętu, jakim są układy FPGA i pozwala skupić się na realizacji

głównych wytycznych projektu mechatronicznego. W rozdziale szóstym pokazano

opracowany proces projektowania i implementacji układu sterowania robotem

równoległym. Wyprowadzone w pracy równania i zapisane w postaci algebraicznej,

opisujące najwaŜniejsze zaleŜności geometryczne robota, oprócz tego, Ŝe pozwoliły na

opracowanie algorytmu sterownia, mogą równieŜ posłuŜyć innym późniejszym badaniom,

które obejmują tematykę robotów równoległych. Na uwagę zasługuję równieŜ fakt, Ŝe

przedstawione w rozdziale szóstym konkretne rozwiązania akceleratorów wspomagających

obliczenia, mogą być wykorzystane jako gotowe wzorce w zupełnie odmiennych projektach

gdzie wydajność obliczeniowa ma równie waŜne znaczenie jak w przypadku obliczeń w

sterowaniu robotów równoległych.

Siódmy rozdział pracy zawiera uwagi i wnioski końcowe. W rozdziale tym

zaproponowano moŜliwości przedstawionych badań oraz kierunki dalszych prac. Ostatni

rozdział ósmy zawiera bibliografię związaną z pracą.


- 11 -

3. Mechatronika w projektowaniu robotów

Słowo „mechatronika” pochodzi z Japonii gdzie w 1969 roku Tresuro Mori z

Yakasawa Electric zarejestrował nazwę jako chroniony znak towarowy na całym świecie

[27][28]. W 1982 roku Yakasawa zrezygnowała ze swych praw, aby umoŜliwi ć na

swobodne korzystanie z tej nazwy na całym świecie. Obecnie słowo „mechatronika” nabrało

szerszego znaczenia i jest uŜywane bardziej jako określenie pewnego podejścia w technice

inŜynierskiej niŜ samej techniki [36].

Pojęcie mechatroniki najlepiej obrazuje diagram przedstawiany poniŜej (rys. 3-1).

Mechatronika

Mechanika

Sterowanie

ElektronikaInformatyka

Modelowanie Sensoryka

Rys. 3-1 Diagram przedstawiający pojęcie mechatroniki

Próbuje się teŜ tworzyć definicję pojęcia mechatroniki, ale mimo wielu prób wciąŜ

nie ma jednej najbardziej trafnej. Jedną z przyczyn takiego stanu moŜe być ukierunkowanie

twórców róŜnych definicji na konkretne cechy. Inną przyczyną i chyba najbardziej

prawdopodobną braku jednolitej definicji jest ciągła ewolucja mechatroniki. Coraz to

nowsze techniki dołączane są w zakres projektowania mechatronicznego, co podnosi zakres

stosowania pojęcia mechatroniki i z powodzeniem wspomaga działanie projektantów.

Najczęściej podawaną definicją mechatroniki jest definicja zaproponowana przez

Tomizukę i Fukadę: „Mechatronika jest synergicznym połączeniem mechaniki z elektroniką

i inteligentnym sterowaniem komputerowym w projektowaniu i wytwarzaniu produktów i

procesów wytwórczych” [20]. Z kolei definicja przyjęta przez International Federation for

the Theory of Machine and Mechanism brzmi „Mechatronika jest synergiczną kombinacją

mechaniki precyzyjnej, sterowania elektronicznego i myślenia systemowego w


- 12 -

projektowaniu produktów i procesów wytwórczych” [81] lub krócej „synergiczną integracje

mechaniki, elektroniki, inŜynierii sterowania i informatyki w procesie projektowania i

wytwarzania produktów” [90]. Natomiast definicja przyjęta przez Auslandera i Kempfa w

pracy [5] brzmi „Mechatronika jest zastosowaniem złoŜonego podejmowania decyzji w

działaniu systemów fizycznych”. Jedną z najnowszych definicji podaje Tomizuka [82]:

„Mechatronika jest synergiczną integracją systemów fizycznych, informatyki i złoŜonego

podejmowania decyzji w projektowaniu, wytwarzaniu i eksploatacji produktów i procesów

przemysłowych”, albo w bardzo skróconej wersji: „najlepsza praktyka syntezy dla

inŜynierów róŜnych dziedzin” [82]. W tym wypadku pod pojęciem informatyki rozumiemy

sprzęt komputerowy i sieciowy oraz oprogramowanie, złoŜone podejmowanie decyzji

oznacza: teorię sterowania, sztuczną inteligencję, teorię systemów hybrydowych i

diagnostykę techniczną. Istnieją jeszcze definicje podkreślające fakt, Ŝe mechatronika

zajmuje się głównie metodologią projektowania, jak np. podana przez Shetty’ego i Kolka

[77] „Mechatronika jest metodologią stosowaną do optymalnego projektowania produktów

elektromechanicznych”.

Przedstawione powyŜej definicje mimo prostej i jasnej treści są w zasadzie mało

precyzyjne. Istnieje zagroŜenie, Ŝe opierając się na powyŜszych definicjach do dziedziny

mechatroniki moŜna omyłkowo zaliczyć rzeczy, które do niej nie naleŜą np. projekty gdzie

w zasadzie występują elementy o roŜnej naturze, ale w Ŝaden sposób nie są ze sobą

powiązanie ani na etapie projektowania ani praca Ŝadnego z nich nie wpływa na pozostałe

elementy mimo, Ŝe wyraźny jest korzystny efekt integracji wielu dziedzin. Aby uniknąć

takich błędów próbuje się definiować mechatronikę jako etap projektowania, czyli: „Patrząc

na projektowanie jako iteracyjny proces optymalizacyjny z wielowymiarową przestrzenią

poszukiwań, podejście mechatroniczne charakteryzuje się przeprowadzeniem

przeszukiwania przestrzeni moŜliwych rozwiązań równocześnie w wielu wymiarach z

róŜnych dziedzin, o róŜnej naturze fizycznej (mechanika, elektronika, teoria sterowania …)

w kaŜdej iteracji” i dalej „Innymi słowy, kaŜda decyzja projektowa, nawet ta, odnosząca się

do pojedynczego zagadnienia z jednej dziedziny, musi uwzględniać wszystkie aspekty

projektowanego urządzenia” [67]. Wydaje się, Ŝe ostatnia definicja najbardziej precyzyjnie

wprowadza w istotę mechatroniki.

Jako, Ŝe mechatronika ściśle uzaleŜniona jest od innych dziedzin to rozwój kaŜdej z

nich bezpośrednio wpływa na jej kształt. Mimo znacznego rozwoju dyscyplin wchodzących

w skład mechatroniki w latach siedemdziesiątych, dopiero w kolejnej dekadzie nastąpiła

potrzeba wspólnego projektowania urządzeń zawierających mechanikę, elektronikę i


- 13 -

oprogramowania (hardware/software codesign) [36]. Wynikało to z coraz ostrzejszych

wymagań stawianych produktom przez uŜytkowników. Największy rozwój mechatroniki

nastąpił w latach dziewięćdziesiątych wraz z wprowadzeniem praktycznego wykorzystania

sztucznej inteligencji w produktach i pojawieniem się pierwszych mikronapędów i

mikroczujników.

Do najczęściej wymienianych trendów w rozwoju produktów mechatronicznych w

najbliŜszych latach moŜna zaliczyć [67][11]:

• zmniejszenie ceny nowych generacji produktów i/lub zapewnienie dodatkowej

funkcjonalności

• wzrastająca dominacja oprogramowania w realizacji funkcji i zapewnieniu jakości

systemów

• zastępowanie elementów mechanicznych i hydraulicznych elektrycznymi i

elektronicznymi

• skrócenie czasu realizacji projektu

• wzrost zapotrzebowania na coraz bardziej zindywidualizowane produkty, lepiej

dopasowane do Ŝyczeń i potrzeb poszczególnych odbiorców

• stawanie się interfejsu człowiek-maszyna jednym z głównych czynników

róŜnicujących nowe produkty

• coraz surowsze prawodawstwo dotyczące ochrony środowiska

• wzrastający udział mikrosystemów, jako elementów składowych

Wszystkie wyŜej wymienione czynniki powodują, Ŝe w procesie projektowania

współczesnych produktów, zachodzi konieczność stosowania interdyscyplinarnego

podejścia mechatronicznego.

3.1. Podejście mechatroniczne

Mechatronika zawiera w sobie kilka dziedzin techniki, więc oczywiste staje się, Ŝe

produkt mechatroniczny jest zbyt złoŜony, aby mógł być projektowany przez jedną osobę. Z

kolei posiadanie grupy projektantów z róŜnych specjalizacji wcale nie gwarantuje sukcesu.

Przyczyna tkwi w silnym powiązaniu dziedzin wchodzących w skład mechatroniki, które to

wymusza łatwy przepływ informacji pomiędzy specjalistami w fazie projektowania. Brak

takiej wymiany informacji grozi tworzeniem róŜnych części projektu, które to są wzajemnie

niekompatybilne.


- 14 -

Dotychczasowa praktyka była taka [21], Ŝe produkt zawierający funkcje

mechaniczne, elektryczne i sterowanie był projektowany sekwencyjnie (rys. 3.1-1).

Oznaczało to, Ŝe podczas kolejnych faz projektu, za proces tworzenia odpowiadały kolejne

grupy inŜynierów. Najpierw podstawowa struktura mechaniczna była projektowana i

wykonywana przez inŜynierów mechaników, następnie elektrycy lub elektronicy wyposaŜali

ją w czujniki, napędy i fizyczny sterownik. Na końcu automatycy dostawali zadanie

znalezienia skutecznego prawa sterowania, które często przekazywali programistom do

zakodowania [67]. Widać, Ŝe przy takim podejściu projekt moŜe zatrzymać się na którymś z

etapów i być nie moŜliwy do kontynuacji, mimo, Ŝe moŜe istnieć rozwiązanie postawionych

celów projektu, gdyŜ wybranie konkretnej ścieŜki postępowania przez daną grupę

projektantów ogranicza juŜ moŜliwości w kolejnej fazie projektu.

Potrzeba

Specyfikacja

Projektowaniemechaniczne

Projektowanieelektroniczne

Projektowanieoprogramowania

Produkt

Rys. 3.1-1 Schemat projektowania tradycyjnego - sekwencyjnego

Mechatroniczne podejście do projektowania wymaga pracy zespołowej na kaŜdym

etapie projektu. Dodatkowe korzyści przynosi dołączenie do projektów reprezentantów


- 15 -

innych działów powiązanych z tworzonym produktem np.: specjalistów produkcji,

marketingu, zarządzania. Wtedy poszczególne składniki systemu mogą być projektowane

równolegle - współrzędnie (rys. 3.1-2). NaleŜy przy tym zwrócić uwagę na to, Ŝe

projektowanie równoległe wymaga metody sprawdzenia kompatybilności poszczególnych

elementów projektu. Taką metodą jest wirtualne prototypowanie. Sukces takiej metody leŜy

przede wszystkim w moŜliwości jak najwszechstronniejszego scalenia róŜnych dziedzin

oraz w komunikacji pomiędzy specjalistami. KaŜdy specjalista musi wnieść nie tylko

wiedzę i doświadczenie z własnej dziedziny, ale równieŜ musi umieć dobrze komunikować

się z innymi specjalistami, w przeciwnym razie istniejące bariery w porozumiewaniu mogą

doprowadzić do złej realizacji zadań projektowych.

Potrzeba

Specyfikacja

Projektowaniemechaniczne

Projektowanieelektroniczne

Projektowanieoprogramowania

Produkt

Rys. 3.1-2 Projektowanie mechatroniczne - współbieŜne

Najczęściej podkreślane korzyści wynikające z projektowania współbieŜnego w

zespole mechatronicznym to [67]:

• znaczne skrócenie czasu projektu

• uproszczenie i przyspieszenie implementacji

• moŜliwość elastycznej realizacji poszczególnych funkcji systemu


- 16 -

Opisane wyŜej podejście współbieŜne w projektowaniu mechatronicznym

zastosowano w niniejszej pracy.

3.2. Procedura projektowania robotów

Procedura projektowania mechatronicznego przedstawiono schematycznie na rys.

3.2-1 [67][35]. Wstępnym etapem projektu jest specyfikacja załoŜeń oraz badania,

obejmujące analizę istniejących konstrukcji stosowanych do podobnych zadań, zarówno pod

katem teoretycznym jak i praktycznym. Wynikiem takiej analizy moŜe być albo wstępne

zaakceptowanie istniejącego rozwiązania jako podstawy do dalszych prac, jeŜeli spełnia ono

wymagania projektu, albo wykaz tych jego „słabych punktów”, które decydują o braku

moŜliwości spełnienia załoŜeń. W takim wypadku naleŜy przeanalizować dostępne metody i

zaproponować korekty, albo sformułować wytyczne dla stworzenia konstrukcji oryginalnej.

W wyniku tych działań powinna powstać koncepcja robota, obejmująca kinematykę oraz

podstawowe rozwiązania konstrukcji mechanicznej i sterowania.

badania podstawowe

załoŜenia

koncepcja

projekt symulacja

MODEL

konstrukcja

synteza symulacja

Szybkie prototypowanie

PROTOTYP

IMPLEMENTACJA

sterowanie

Wirtualne prototypow

anie

Rys. 3.2-1 Procedura projektowania robotów równoległych [67][35]

Następnym etapem procedury jest wirtualne prototypowanie, przeprowadzane

iteracyjnie w dwóch pętlach, aŜ do spełnienia załoŜeń. W pierwszej (górnej) pętli

projektowana i modelowana jest część konstrukcyjna robota (elektromechaniczna), a w


- 17 -

drugiej (dolnej) odbywa się synteza sterowania. Stopień spełnienia załoŜeń jest sprawdzany

poprzez symulację modelu robota, przy wymuszaniu w układzie otwartym, jak teŜ ze

sterownikiem w pętli sprzęŜenia zwrotnego. W miarę postępu prac model staje się coraz

bardziej szczegółowy, ale od samego początku obejmuje wszystkie elementy robota:

mechanikę (kinematyka i dynamika), napędy, czujniki, sterowanie oraz oddziaływanie z

otoczeniem (przedmiot manipulowany albo obrabiany, sposób mocowania robota). Jest to

model interdyscyplinarny, są w nim reprezentowane i integrowane wszystkie dziedziny

odgrywające rolę w pracy robota. Ze względu na mnogość i złoŜoność zjawisk o róŜnej

naturze fizycznej występujących w robotach, często istnieje potrzeba tworzenia wielu

modeli o róŜnym stopniu szczegółowości reprezentacji poszczególnych zjawisk,

przeznaczonych do róŜnych celów. Na przykład często podczas syntezy sterowania opartego

na modelu, preferowane są modele prostsze, mniej dokładnie odwzorowujące te cechy

manipulatora, które mają ograniczony wpływ na jakość projektowanego sterownika.

Po osiągnięciu zgodności wirtualnego robota z załoŜeniami, moŜna przystąpić do

wykonania prototypu fizycznego. W pierwszej kolejności jest on wykorzystywany w

eksperymencie identyfikacyjnym do dostrojenia modelu i co za tym idzie, algorytmu

sterowania. W tym miejscu moŜe się ujawnić konieczność modyfikacji konstrukcji robota,

jednakŜe jest to przypadek skrajny, który przy poprawnym i odpowiednio dokładnym

modelowaniu na poprzednim etapie nie powinien wystąpić. Prototyp fizyczny jest

wykorzystywany równieŜ przy szybkim prototypowaniu algorytmu sterowania. Etap ten ma

na celu sprawdzenie efektywności algorytmu podczas pracy w czasie rzeczywistym i

precyzyjne dostrojenie jego parametrów. Gdy osiągi sterowania staną się satysfakcjonujące,

moŜna przystąpić do implementacji algorytmu sterowania na docelowej platformie

sprzętowej [67][35].

3.3. Szybkie prototypowanie

Szybkie prototypowanie (ang. fast prototyping) zaliczane jest do symulacji w czasie

rzeczywistym i obok techniki Hardware-in-the-Loop Simulation (w skrócie HILS) stanowi

istotny element w procesie projektowania i prototypowania układów sterowania. Symulacja

w czasie rzeczywistym ma na celu usprawnić badania eksperymentalne poprzez

zastosowanie wydajnych systemów obliczeniowych, które to w czasie rzeczywistym na

podstawie wcześniej przygotowanych modeli matematycznych potrafią generować sygnały

odpowiadające symulowanemu urządzeniu. W technice Hardware-in-the-Loop na podstawie


- 18 -

wcześniej przygotowanych równań system symuluje rozwaŜane urządzenie mechaniczne lub

jego część poprzez generowanie sygnałów wyjściowych odpowiadających danemu

obiektowi na podstawie otrzymanych informacji wejściowych. Technikę tą moŜna

wykorzystać w przypadku, gdy posiadamy rzeczywisty sterownik, ale istnieje ryzyko

uszkodzenia obiektu, wtedy obiekt zastępowany jest symulatorem. UmoŜliwia to np.

sprawdzenie zachowania rzeczywistego układu sterowania w sytuacjach, które cięŜko

wymusić na rzeczywistym obiekcie ze względu na związane z tym jego uszkodzenie jak np.:

stany związane z defektami lub stany związane z przekroczeniem parametrów

dopuszczalnych. Wtedy symulator zastępujący obiekt rzeczywisty „udaje” daną

konfigurację urządzenia i projektant moŜe sprawdzić zachowanie się układu sterowania.

Zdecydowanie bardziej przydatną i duŜo częściej stosowaną techniką jest sytuacja

odwrotna, kiedy to wirtualny sterownik symulowany na sprzęcie do szybkiego

prototypowania współpracuje z rzeczywistym obiektem. Wówczas mając rzeczywisty obiekt

moŜna sprawdzać i modyfikować dowolnie projektowany sterownik. WaŜnym czynnikiem

będącym podstawą szybkiego prototypowania jest automatyzacja przenoszenia kodu z

wirtualnego pulpitu (np. schematu Simulinka) do urządzenia realizującego szybkie

prototypowanie. Wówczas sprzęt do szybkiego prototypowania, zwykle oparty na

procesorach sygnałowych dokonuje obliczenia związane z prawem sterowania i na tej

podstawie generuje sygnały zadane dla rzeczywistego obiektu. Dodatkowo taki system

często daje dodatkowe korzyści w postaci moŜliwości obserwowania sygnałów związanych

ze sterowaniem, ich rejestracji dla późniejszych analiz, oraz zwykle istnieje moŜliwość

modyfikacji parametrów układu sterowania podczas pracy urządzenia. Przykładowo w

wirtualnym sterowniku PID moŜna w czasie rzeczywistym zmieniać parametry

współczynników wzmocnień dla członu proporcjonalnego, całkującego i róŜniczkującego i

obserwując zachowanie systemu moŜna ustalić satysfakcjonujące wartości, bez

konieczności prowadzenia Ŝmudnych obliczeń.

Do głównych zalet szybkiego prototypowania zalicza się [67]:

• uwolnienie się od problemów związanych z implementacją i moŜliwość

skoncentrowania się na samym algorytmie

• łatwą i szybką eksperymentalną weryfikację struktury i parametrów algorytmu

przetwarzania sygnałów, zanim zostaną podniesione nakłady na implementację

• moŜliwość określenia realistycznych specyfikacji dla docelowej warstwy sprzętowej

(elektronik) sterownika i jego oprogramowania


- 19 -

• sprawdzenie moŜliwości redukcji algorytmu i rzędu modeli w nim zawartych , co

moŜe obniŜyć koszty docelowego sterownika, zwłaszcza przy produkcji masowej

• sprawdzenie poprawności wyboru zastosowanych czujników i pozostałych

elementów toru pomiarowego

• analizę wpływu efektów kwantyzacji

• dobór optymalnego okresu próbkowania

• zmniejszenia kosztów i czasu realizacji projektu

Wadą szybkiego prototypowania jest konieczność posiadania specjalistycznego

sprzętu, który ze względu na swą uniwersalność i moc obliczeniową zwykle jest bardzo

drogi.

3.4. Implementacja w FPGA

Algorytm sterowania, który ma zostać zaimplementowany w FPGA jest zwykle

ciągły w czasie, wykorzystuje ciągłe wartości sygnałów i opisany jest za pomocą równań

matematycznych (schematu blokowego). Opis taki nie nadaje się do bezpośredniego

zastosowania i wymaga kilku transformacji, wpływających nie tylko na sposobu zapisu, ale

takŜe i na sam algorytm i jego własności [91][68][67].

Przede wszystkim wymagana jest dyskretyzacja w czasie a następnie dyskretyzacja

amplitudy (kwantyzacja). Realizacja obliczeń za pomocą operacji stałoprzecinkowych

znacznie upraszcza implementację oraz obniŜa koszty. Teoretycznie kaŜdy algorytm moŜna,

poprzez odpowiednie skalowanie, zamienić na postać wykorzystującą operacje

stałoprzecinkowe bez utraty precyzji obliczeń [51][52]. Problemem jest tylko nakład pracy

potrzebny do dokonania transformacji operacji zmiennoprzecinkowych oraz odpowiednie

dobranie liczby bitów potrzebnych do wykonania danej operacji. Szczególnie problemem

mogą być sygnały o duŜej dynamice, dla których zapis zmiennoprzecinkowy jest bardziej

wskazany. Przejście do operacji stałoprzecinkowych moŜe znacznie ułatwić biblioteka

Simulinka Fixed Point Blockset [16] oraz biblioteka programistyczna A|RT Library [2].

Istotny problem stanowi oprogramowanie stosowane do syntezy układów FPGA.

Akceptuje ono zwykle opis w specjalnym języku opisu sprzętu HDL (ang. Hardware

Description Language), rzadko znanym przez automatyków i trudnym w uŜyciu. Pozostaje,

więc problem zapisu algorytmu stałoprzecinkowego w języku przeznaczonym dla układów

specjalizowanych. Najpopularniejsze z nich to VHDL (Very High Speed Integrated Circuit

Hardware Description Language) i Verilog. Po zakodowaniu algorytmu w notacji VHDL


- 20 -

lub Verilog pozostaje do rozwiązania problem jego symulacji (istniejącego prototypu).

Symulatory tych języków ukierunkowane są głównie na symulacje na najniŜszym poziomie

i testowanie ich z pozostałymi częściami systemu nie jest moŜliwe.

Jednym z rozwiązań moŜe być zapis algorytmu w języku C++ rozszerzonym o

stałoprzecinkowe typy liczb (A|RT Library). Taki algorytm moŜna powtórnie

przesymulować oraz przede wszystkim automatycznie zamienić na język VHDL [1]. Tak

zapisany algorytm moŜna juŜ implementować w układzie FPGA. Procedurę wykorzystującą

takie podejście przedstawiono na poniŜszym rysunku (rys. 3.4-1).

Floating-point,algorytm sterowania

ciągły w czasie

Fixed-point,algorytm z dyskretnymczasem próbkowania

Simulink s-funckja(ART Library)

Algorytm sterowaniazapisany w kodzie C++

(ART Library)

Algorytm sterowaniazapisany według

języka VHDL

Interfejs sprzętu(VHDL)

FPGA/ASICImplementacja

algorytmu sterowania

Ręczna transformacja

Automatyczna transformacja

Rys. 3.4-1 Procedura implementacji algorytmu sterowania w układach ASIC/FPGA

Istnieją równieŜ inne metody pozwalające na implementację algorytmów sterowania

do układów FPGA. Próbuje się tworzyć inne języki wyŜszego poziomu bazujące na kodzie

C, będące pomostem pomiędzy fazą symulacją a fazą implementacji np.: SystemC, Handel-

C, Cbridge [44]. Wadą ich jest małą popularność i znaczne róŜnice w stosunku do C

uniemoŜliwiając uŜycie standardowych kompilatorów.

Najnowszym i bardzo ciekawym rozwiązaniem jest próba budowania struktury

układu FPGA bezpośrednio w Simulinku. Takie rozwiązania promują dwie wiodące firmy

Altera [12] i Xilinx [80]. Oba narzędzia DSP Builder oraz Xilinx System generator for DSP

opierają się na pomyśle bezpośredniej zamiany schematu Simulinka do postaci z której

moŜliwa jest implementacja. Przedstawioną ideę moŜna dokładnie opisać na przykładzie


- 21 -

DSP Builder firmy Altera (rys. 3.4-2). Projektant buduje algorytm sterowania stosując

dodatkową bibliotekę bloków specjalnie przygotowaną dla Simulinka i dołączoną wraz z

narzędziem. Taki schemat moŜna symulować na zasadach ko-symulacji z narzędziami tejŜe

firmy. JeŜeli układ spełnia załoŜenia projektanta, istnieje moŜliwość automatycznego

wygenerowania kodu moŜliwego do zaimplementowania. Podobnie jest z Xilinx System

Generator for DSP z tym Ŝe do budowy systemu uŜywamy biblioteki i narzędzi tejŜe firmy.

Rys. 3.4-2 Idea narzędzia DSP Builder

Przedstawione narzędzia mimo bardzo trafnej idei posiadają znaczące wady. Wady

wynikają z polityki i interesu firm, które je opracowały. OtóŜ narzędzia są przeznaczone dla

konkretnych układów FPGA tych firm i mogą być uŜywane wyłącznie z nimi. Budowany

jest system DSP, a zapis algorytmów sterowania wiąŜe się z uŜyciem specyficznych bloków,

których to nie moŜna uŜywać z narzędziami innych firm, np. podczas bardzo uŜytecznego

szybkiego prototypowania na procesorach sygnałowych. Symulacja dokonywana jest na

zasadzie ko-symulacji z narzędziami specjalizowanymi tych firm i bez nich nie moŜliwa.

Według autora rozwiązaniem mogłoby być przyjęcie standardu, co do nowych

bloków lub stworzenie generatorów VHDL na podstawie podstawowych bloków nie

faworyzujących Ŝadnej z firm, wtedy jeden algorytm mógłby być symulowany i testowany


- 22 -

na zupełnie odmiennych systemach. Innym rozwiązaniem moŜe być zbudowanie

generatorów pozwalających generować kod w językach innych niŜ VHDL.

3.5. Implementacja jako „System-on-a-Programmable-Chip”

System w układzie (ang. System-on-Chip SoC) jest pojedynczym układem scalonym

zawierającym w sobie cały system: mikroprocesor(y), koprocesory, jednostki przetwarzania

sygnałów, układy peryferyjne, pamięci, interfejsy komunikacyjne itd. MoŜliwość

implementacji SoC w układach FPGA, nazywanych wtedy teŜ „System-on-a-

Programmable-Chip” (skrót SoPC) daje dodatkową elastyczność w eksperymentowaniu z

charakterystykami wydajności, nawet w późnych fazach procesu projektowania, oraz

wydłuŜając cykl Ŝycia produktu przez moŜliwość aktualizowania wersji sprzętu i

oprogramowania lub dostosowywania funkcjonalności do specyficznych potrzeb

konkretnego uŜytkownika lub zastosowania, bez modyfikacji płyty drukowanej PCB

(fizycznej warstwy sterownika).

W tradycyjnym podejściu do projektowania SoC, projektant musi w sposób ręczny

stworzyć opis w języku HDL nadrzędnej warstwy systemu, łączącej w całość poszczególne

jego elementy, implementującej wymianę danych, synchronizację i arbitraŜ pomiędzy nimi.

Pochłania to wiele wysiłku i czasu. Pojawiają się w ostatnich latach narzędzia ułatwiające

integrację systemu, takie jak. ISE Xilinxa [92] lub SOPC Builder Altery [73]. UŜycie ich,

uwalnia projektanta od najbardziej pracochłonnych i nuŜących czynności i pozwala

skoncentrować się na architekturze systemu. MoŜliwości takiego podejścia moŜna pokazać

na przykładzie SoPC opartego na układzie FPGA z rodziny Stratix Altery,

wprogramowanego (niewykonanego „na sztywno” w krzemie) 32-bitowego mikroprocesora

Nios II typu RISC i oprogramowania SOPC Builder.

Stosując narzędzia SOPC Builder projektant w interfejsie graficznym wybiera

elementy systemu, a program automatycznie generuje logikę połączeń między nimi.

Uzyskiwane są w efekcie pliki HDL definiujące wszystkie komponenty systemu oraz plik z

kodem najwyŜszego poziomu, definiującym połączenia między tymi komponentami. Celem

SOPC Buildera jest wyabstrahowanie złoŜoności połączeń i pozwolenie projektantowi na

skupienie się na projektowaniu komponentów i architektury systemu.


- 23 -

Rys. 3.5-1 Typowa architektura systemu w programowalnym układzie scalonym (SoPC), opartego na układzie

FPGA firmy Altera i wprogramowanym mikroprocesorze Nios II; bloki z szarym tłem przedstawiają

niestandardowe, własne komponenty projektanta

Typową architekturę przedstawiono na rys. 3.5-1. System jest zbudowany ze

standardowych komponentów (białe tło) oraz z komponentów stworzonych przez

projektanta (szare tło) połączonych magistralą Avalon, zbudowaną ze specjalizowanych

zasobów układu. W systemie moŜe współistnieć wiele mikroprocesorów Nios II

pracujących na wspólnej magistrali lub niezaleŜnie. W układach FPGA o największej

pojemności moŜna zaimplementować do kilkuset takich procesorów, które są

konfigurowalne i występują w wersjach róŜniących się szybkością, mocą obliczeniową i

ilością zajmowanych zasobów układu. Architektura Niosa II umoŜliwia dodawanie

własnych instrukcji, definiowanych przez uŜytkownika. Instrukcje te są jedną z metod na

zwiększenie osiągów systemu przez rozszerzenie jednostki arytmetyczno-logicznej (ALU)

procesora o własne operacje realizowane sprzętowo. Zakres moŜliwych architektur

instrukcji uŜytkownika rozciąga się od prostych operacji kombinatorycznych do rozległych,

wielocyklowych układów sekwencyjnych o zmiennej długości, które mogą komunikować

się z urządzeniami zewnętrznymi w stosunku do FPGA, np. w celu akwizycji lub zapisu

danych. Z punktu widzenia oprogramowania, własne instrukcje widziane są jako

automatycznie generowane makroinstrukcje asemblera lub funkcje języka C. SOPC Builder

generuje pliki konfiguracyjne dla kompilatora C/C++ definiujące aktualną architekturę

systemu. Dla kaŜdej instrukcji uŜytkownika zintegrowane środowisko programistyczne


- 24 -

procesora Nios II tworzy makroinstrukcję zdefiniowaną w nagłówkowym pliku

systemowym. Programista moŜe wywołać taką makroinstrukcję z kodu C/C++ aplikacji tak

jak normalną funkcję, bez potrzeby uŜywania asemblera dla osiągnięcia dostępu do

instrukcji uŜytkownika. Środowisko programistyczne jest oparte na kompilatorze GNU

C/C++ i standardowym zintegrowanym środowisku Eclipse IDE [14].

Inną moŜliwością zwiększenia wydajności systemu jest zastosowanie koprocesorów

(akceleratorów sprzętowych). W przeciwieństwie do własnych instrukcji, koprocesory

działają jako niezaleŜne moduły logiczne, przyjmujące polecenia z centralnej jednostki

sterującej (CPU) procesora przez magistralę Avalon i przetwarzające całe bufory danych

i/lub fragmenty algorytmu bez interwencji procesora. Koprocesory mogą osiągnąć

wydajność większą o kilka rzędów wielkości w porównaniu z zadaniami wykonywanymi

programowo dzięki ich autonomicznej naturze oraz całkowicie sprzętowym wykonywaniu

swych funkcji. Mogą one równieŜ bezpośrednio wymieniać dane z urządzeniami

zewnętrznymi, w tym wykorzystując mechanizm bezpośredniego dostępu do pamięci

(DMA).

System w układzie programowalnym moŜe zawierać równieŜ niezaleŜne własne

komponenty sprzętowe działające asynchronicznie w stosunku do magistrali Avalon

nadzorowanej przez CPU. Takie komponenty mogą być odpowiedzialne za zadania

wspomagające lub wymagające absolutnej odporności na awarie oprogramowania. PoniŜej

poziomu systemowego, akceleratory sprzętowe powinny być projektowane i

implementowane w sposób zgodny z procedurami projektowania mechatronicznego [74], co

pozwala to na symulację części algorytmu realizowanych sprzętowo razem z pozostałą

częścią systemu mechatronicznego w procesie wirtualnego prototypowania.

Opisane wyŜej podejście zostało zastosowane przez autora do implementacji

sterownika dla robota równoległego przedstawionego w niniejszej pracy.


- 25 -

4. Manipulatory równoległe

Definicja robota równoległego mówi, iŜ „Manipulator równoległy jest to mechanizm

o zamkniętym łańcuchu kinematycznym, którego efektor połączony jest z podstawą za

pomocą kilku niezaleŜnych łańcuchów kinematycznych” [45].

PowyŜsza definicja zawiera w sobie znaczną grupę mechanizmów, dlatego ze

względów praktycznych często grupę tą ogranicza się do mechanizmów, w których liczba

łańcuchów kinematycznych i elementów wykonawczych (napędów) równa się liczbie stopni

swobody, a gdy elementy wykonawcze są zablokowane, układ nie moŜe się poruszać.

Często w stosunku do manipulatorów równoległych stosuje się terminologię związaną z

liczbą stopni swobody np.: Tripod (3 stopnie swobody) lub Hexapod (6 stopni swobody).

Do najczęściej podkreślanych zalet manipulatorów równoległych naleŜą: wyŜsza

sztywność, spowodowana większą liczbą łańcuchów kinematycznych związanych z

chwytakiem oraz co za tym idzie większą dynamiką działania wraz z korzystnym

stosunkiem obciąŜalności do masy własnej. Wady manipulatorów równoległych wynikają ze

złoŜoności konstrukcji mechanicznej i wiąŜą się z kłopotliwym projektowaniem połączeń

mechanizmów oraz znaczna złoŜoność równań opisujących takiego robota znacznie utrudnia

budowanie układu sterowania. Dodatkowo w wielu przypadkach przestrzeń robocza

wypełniona jest osobliwościami znacznie utrudniając sterowanie i kalibrację [26][85].

Generalnie manipulatory zbudowane są z nieruchomej podstawy będącej bazą i

ruchomej platformy połączonej za pomocą kilku łańcuchów kinematycznych. W zaleŜności

od budowy i liczby stopni swobody moŜna spotkać dwie grupy manipulatorów.

Manipulatory oparte na platformie i mające zwykle sześć stopni swobody oraz manipulatory

wykorzystujące właściwości równoległoboków w celu uzyskania mniejszej liczby stopni

swobody. Istnieją równieŜ manipulatory, które wykorzystują obie cechy i trudno je

zakwalifikować, jednak ich liczba jest znacznie zmniejsza w stosunku do wymienionych.


- 26 -

4.1. Manipulatory oparte na platformie

Współczesne prace na mechanizmami równoległymi zostały zapoczątkowane przez

Gougha. Opracował on strukturę gdzie platforma połączona była sześcioma teleskopowymi

siłownikami poprzez przeguby kulowe. Prototyp tej maszyny zbudowano w 1955 roku jako

platforma doświadczalna [45]. W 1965 Stewart zmodyfikował układ zmieniając

konfigurację ramion. Jego pomysł nie przyjął się, natomiast paradoksalnie struktura

Gougha, na której bazował w późniejszym okresie została nazwana platformą Stewarta lub

platformą Stewarta-Gougha [79](rys. 4.1-1).

Rys. 4.1-1 Platforma Stewarta-Gougha

Na bazie tej struktury powstało wiele manipulatorów, spełniających rolę ruchomych stołów,

symulatorów oraz obrabiarek (rys. 4.1-2).

Rys. 4.1-2 Przykład zastosowania platform, od lewej kolejno: stół manipulacyjny, symulator, obrabiarka.

Platforma Stewarta-Gougha charakteryzuje się duŜą obciąŜalnością i małym

zakresem ruchów, zwłaszcza takich, przy których narzędzie moŜe przyjmować dowolną

orientację.


- 27 -

4.2. Manipulatory oparte na równoległobokach

Potrzeba stworzenia manipulatorów o mniejszej liczbie stopni swobody

spowodowała, Ŝe zaczęły powstawać konstrukcję, w których kolejne ruchliwości odbierano

poprzez zastosowanie układu równoległoboków. Konstrukcje takie ze względu na mniejszą

liczbę napędów mogły być duŜo tańsze od swoich poprzedników. Konstrukcją, która

odniosła największy sukces to robot Delta [69] (rys. 4.2-1), którego konstrukcja oparta jest

na precyzyjnie wykonanych i lekkich równoległobokach ze specjalnych materiałów.

Rys. 4.2-1 Manipulator Delta [69]

Układ trzech równoległoboków połączonych jednym bokiem do chwytaka a z

drugiej do napędu, który ten bok równoległoboku porusza się w płaszczyźnie równoległej

do podstawy gwarantuje trzy translacyjne stopnie swobody. Dla uzyskania takich trzech

stopni swobody nie ma znaczenia, w jaki sposób jeden z boków równoległoboku będzie

napędzany. Dlatego spotyka się roboty typu Delta z napędami obrotowymi (oryginalna

konstrukcja Delta) i napędami liniowymi (pochodne konstrukcji Delta) (rys. 4.2-2).

Rys. 4.2-2 Manipulatory oparte na równoległobokach. Po lewej z napędami obrotowymi, po prawej z

napędami liniowymi.


- 28 -

Manipulator oparty na równoległobokach charakteryzuje się duŜymi

przyspieszeniami i duŜą przestrzenią roboczą. Do wad moŜna zaliczyć między innymi małą

obciąŜalność. Takie manipulatory stosuje się głownie do manipulacji bardzo lekkimi

obiektami, głownie do paletyzacji.

4.3. Wybrane manipulatory równoległe

Istnieje teŜ grupa manipulatorów, które trudno jednoznacznie zakwalifikować

według przedstawionego podziału. Manipulatory takie mogą być budowane na bazie

robotów o strukturze szeregowej lub manipulatory gdzie liczba stopni swobody mniejszą od

sześciu uzyskano poprzez specyficzne usytuowanie osi członów obrotowych i liniowych

(rys. 4.3-1).

Rys. 4.3-1 Wybrane manipulatory równoległe

Przedmiotem pracy jest manipulator równoległy typu tripod, gdzie podobnie jak w

manipulatorze Delta poprzez trzy ramiona łączące platformę uzyskano trzy stopnie

swobody, zachowując przy tym moŜliwość uzyskania duŜych sił jak w przypadku robotów

opartych na platformie Stewarta. W szczególności skoncentrowano się na implementacji

układu sterownia robotem równoległym przy zachowaniu wymogów podejścia

mechatronicznego.


- 29 -

5. Automatyczna generacja kodu C

Procedurę implementacji układów sterowania (rys. 3.4-1) moŜna znacznie usprawnić

poprzez opracowanie metody pozwalającej na automatyczną generację kodu C na podstawie

schematu Simulinka. NaleŜy przy tym zwrócić uwagę na to, Ŝe w odróŜnieniu od

dostępnych aplikacji [75], które to generują kod zgodny z ANSI C [4] nienadający się do

bezpośredniego przełoŜenia na VHDL, opracowany w ramach pracy generator tworzy pliki

wykorzystujące rozszerzenie języka C do języka C++ [25]. Opracowane rozszerzenie

pozwoliło na uŜycie klas obiektów reprezentujących typy stałoprzecinkowe niebędące

standardowo zaimplementowane w C/C++. Z kolei uŜycie typów stałoprzecinkowych

jednoznacznie odwzoruje operacje arytmetyczne zapisane w schemacie Simulinka oraz

pozwoli na konwersję do VHDL przy pomocy dostępnych programowych narzędzi

komercyjnych.

5.1. Typy liczb

Konstrukcja powszechnych jednostek arytmetyczno logicznych opiera się na

reprezentacji bitowej liczb [13]. Oznacza to, Ŝe stosując zapis zero-jedynkowy tworzony jest

ciąg dwuwartościowych stanów, w których odpowiednia interpretacja wag wyznacza nam

wartość liczbową. Liczba bitów oraz sposób interpretacji wag wyznacza nam typ liczby. Ze

względu na to, Ŝe kaŜdy typ liczb wymaga innej wielkości zasobów sprzętowych do

realizacji działania matematycznego przyjęto kilka standardowych typów.

W zapisie dziesiętnym reprezentacja liczby realizowana jest poprzez usytuowanie

kombinacji dziesięciu znaków będących cyframi 0,1,2…9. Wagę danej pozycji ustalamy

licząc od prawej do lewej. Stosując tą metodę moŜna analogicznie zbudować zapis oparty na

systemie innym niŜ dziesiętny.

Prostota maszyn liczących tkwi w wykorzystuje dwustanową wartość sygnałów, a to

oznacza, Ŝe do reprezentacji liczby mamy tylko dwa znaki. Liczba 2006 w ciągu

dwójkowym wyraŜona jako 11111010110, odpowiada zapisowi:

012345678910 2021212021202121212121 ⋅+⋅+⋅+⋅+⋅+⋅+⋅+⋅+⋅+⋅+⋅ .Z przedstawionego

przykładu nasuwa się jeden waŜny wniosek, liczba pozycji potrzebnych do zapisania danej

liczby zaleŜy nie tylko od samej liczby, ale od przyjętego systemu. Oznacza to, Ŝe przy

zapisie dziesiętnym do zapisania liczby 2006 wystarczyło cztery pozycje, ale juŜ przy

systemie dwójkowym aŜ jedenaście. Projektując jednostkę arytmetyczną naleŜy pamiętać,

Ŝe kaŜda dodatkowy bit zwiększa ilość zasobów układu potrzebnych na daną operację.


- 30 -

Ze względów praktycznych przyjęło się, Ŝe dla standardowych układów typy

liczbowe będą wielokrotnością ośmiu. NaleŜy tutaj dodać, Ŝe typ 8-bitowy powstał z

podwojenia typu 4-bitowego, który był stosowany w pierwszych maszynach cyfrowych

[23]. Jako Ŝe 4-bitowy typ juŜ nie jest stosowanym, za podstawowy uznaje się typ 8-bitowy

nazywany bajtem. I tak procesor, którego jednostka arytmetyczno-logiczna wykonuje

operacje na ośmiu bitach nazywamy 8-bitowym. Zakres ośmiobitowy pozwala na zapis liczb

w zakresie 0-255. Dla wielu zastosowań taki zakres jest niewystarczający. Z kolei potrzeba

wykorzystania większych typów kolidowała z moŜliwościami układów cyfrowych.

Kompromisem było stosowanie jednostek 8-bitowych do realizacji zadań matematycznych

na typach większych. I tak grupując bajty w jeden blok uzyskano typy 16-bitowe i 32-

bitowe. Znaczenie bajtu w takim szeregu moŜe być róŜne. Obecnie konkurują ze sobą dwa

standardy promowane przez Intela i Motorolę, które to określają pozycję starszego i

młodszego bajtu [9].

Problem zapisu liczb ujemnych rozwiązano poprzez umowne załoŜenie, Ŝe jeden z

bitów (najstarszy) będzie reprezentował znak. Wartość 1 oznaczać będzie liczbę ujemną.

Taki zapis ma jedną wadę, istnieje kombinacja, która odpowiada liczbie -0. Wadę tę nie

posiada zapis w kodzie uzupełnienia do dwóch (U2) [37]. W językach wysokiego poziomu

takich jak C ustalono kilka typów dla liczb całkowitoliczbowych. Typy te zebrano w tabeli

1.

Nazwa typu Zakres Przykład ciągu bitów Odpowiadająca

liczba dziesiętna

unsigined char 0…255 11111011 251

signed char lub

char -128…127 11111011 -5

unsigned int 0…65535 lub

0… 4294967295 0000000011111011 251

signed int lub

int

-32768…32767 lub

-2147483648… 2147483647 0000000011111011 251

unsigned long 0… 4294967295 00000000000000000000000011111011 251

signed long lub

signed long int

lub long

-2147483648… 2147483647 00000000000000000000000011111011 251

Tabela 1 Typy całkowitoliczbowe w języku C.


- 31 -

Dla liczb zawierających ułamki w językach wysokiego poziomu zaproponowano typ

zmiennoprzecinkowy [17]. Charakteryzuję się on tym, Ŝe na stałej umownej liczbie bitów,

zapisuje się podstawę liczby oraz wykładnik potęgi dwa. Taki sposób znacznie uelastycznia

zapis liczb, ale wiąŜe się z dwoma problemami. Pierwszy to dokładność operacji

matematycznych na typach zmiennoprzecinkowych związana jest z miejscem kropki, oraz

drugi problem wynika z wyŜszych wymagań sprzętowych jednostki arytmetycznej (w tym

wypadku koprocesora) do realizacji operacji matematycznych. Podstawowe operacje, które

w zapisie całkowitoliczbowym są łatwe do implementacji, dla zmiennego przecinka są juŜ

na tyle skomplikowane, Ŝe często producenci procesorów rezygnują z ich sprzętowej

realizacji na rzecz emulacji za pomocą odpowiednich bibliotek.

PoniŜej przedstawiono dwie liczby zapisane w notacji zmiennoprzecinkowej dla

pojedynczej precyzji 32-bit (w języku C typ float), wg kalkulatora [22]:

2006 = 0 10001001 11110101100000000000000

2006.75 = 0 10001001 11110101101100000000000

znak wykładnik podstawa

Dla liczb ujemnych bit znaku (pozycja 31) przyjmuje wartość 1. Wykładnik (bity 30-23) jest

zwiększony o 127.

Okazuje się, Ŝe zapis liczb zawierających część ułamkową moŜna równieŜ wykonać

poprzez rozszerzenie typu całkowitoliczbowego do stałoprzecinkowego. Idea tkwi w

dodatkowym (stałym dla danego typu) określeniu miejsca kropki.

Typ stałoprzecinkowy jest niezwykle uŜyteczny w praktycznych zastosowaniach,

pozwala definiować dowolne długości słów oraz elastycznie modyfikować miejsce kropki.

W rzeczywistych układach sygnał wejściowy i wyjściowy ograniczony jest zwykle przed

dokładność przetworników lub liczników sygnałów enkoderowych. Oznacza to, Ŝe stosując

zapis stałoprzecinkowy moŜna manipulować precyzją obliczeń. Ma to istotne znaczenie w

algorytmach sterowania gdzie dokładność ma bezpośredni wpływ na jakość. Jednak w

językach wysokiego poziomu tj C brak jest znormowanych definicji takich typów.

Spowodowane jest to tym, Ŝe kompilatory tych języków nastawione są na procesory o stałej

i znanej długości słowa w jednostce arytmetyczno-logicznej. Konsekwencją tego jest to, Ŝe

generatory kodu zgodne z ANSI C, nie są w stanie wykorzystać zalet typu

stałoprzecinkowego. I mimo Ŝe narzędzia takie jak Simulink pozwalają na projektowanie

algorytmów opartych na arytmetyce stałoprzecinkowej, to juŜ wbudowany w

MATLAB/Simulink generator kodu C [75], wygeneruje taki kod, aby spełniał standard


- 32 -

ANSI C. To oznacza, Ŝe typ stałoprzecinkowy zostanie zaemulowany za pomocą typów

całkowitoliczbowych.

Istnieje na rynku biblioteka dla języka C++, która dostarcza nam typów

stałoprzecinkowych [2]. Dodatkowo narzędzie tej samej firmy pozwala na automatyczną

zamianę kodu korzystającego z tej biblioteki na opis w języku VHDL w sposób bardzo

wydajny [1]. Jedyny problem, na jaki moŜe natknąć się projektant to brak generatorów

pozwalających zamienić automatycznie schemat Simulinka na kod C++ wykorzystujący

bibliotekę stałoprzecinkową. Brak takiego narzędzia spowodował, Ŝe autor pracy podjął się

opracowania metody oraz zbudowania takiego generatora, pozwalającego na automatyczną

generację kodu C++ dla algorytmu zapisanego w schemacie Simulinka opartego na

arytmetyce stałoprzecinkowej.

5.2. Generator Kodu

Zadaniem programu opracowanego przez autora pracy jest generowanie poprawnego

kodu C++ , realizującego algorytm sterowania na podstawie schematu w Simulinku

zapisanego w pliku z rozszerzeniem „mdl” (plik tekstowy zawierający spis wykorzystanych

bloków Simulinka wraz z listą połączeń).

5.2.1. ZałoŜenia dla generator

Uzyskanie minimalnej ingerencji uŜytkownika w procesie automatyzacji

generowania kodu, wymaga spełnienia kilku warunków. Warunki te wynikają przede

wszystkim z ograniczeń i moŜliwości oprogramowania, z którymi generator ma

współpracować. Są to: standardy powiązane z formatem Simulinka, moŜliwości

kompilatorów języka C, oraz ograniczenia narzędzi tłumaczących kod C++ na język VHDL.

Spełnienie wszystkich warunków wymaga określenia załoŜeń będących kompromisem

pomiędzy tym, co moŜna stworzyć przy pomocy bloków Simulinka, a tym, co jest moŜliwe

do zaimplementowania w układzie FPGA.

Przyjęto następujące załoŜenia:

• program będzie generował kod na podstawie działającego schematu opartego na

bibliotece bloków „Fixed-Point Blockset” [16] w wersji 2.0

• źródłem przeznaczonym do konwersji na kod C++ będzie plik z rozszerzeniem

„mdl” (standardowy plik Simulinka zawierający opis wykorzystanych bloczków i

ich połączeń)


- 33 -

• wykorzystanie zostanie biblioteka „fxp.h” (A|RT Library) firmy Frontier Design [2]

(definiująca typy stałoprzecinkowe dla języka C++)

• program będzie dołączał niezbędne funkcje (wg standardu mex-pliku) w takiej

postaci aby wygenerowany kod C++ moŜna było skompilować jako S-funkcję [43]

przeznaczoną dla Simulinka

• nazwy bloków Fixed-Point będą uŜywane jako zmienne w operacjach

arytmetycznych

• kaŜdy blok Fixed-Point będzie jednoznacznie identyfikowany w kodzie C++ poprzez

posiadanie nazwy z dołączoną ścieŜką podsystemu i komentarzem

• wejściach i wyjściach funkcji realizującej sterownik będą decydować bloki Gateway

In/Out

• schemat blokowy moŜe zawierać dowolną liczbę roŜnie zagnieŜdŜonych

podsystemów

• schemat zawarty w pliku z Simulinka będzie wygenerowany w postaci jednej funkcji

w kodzie C++

• generowane będą tylko bloki operujące na wartościach skalarnych

• wartości stałych uŜywanych przez róŜne bloki mogą być wpisane w postaci nazw,

ale wówczas muszą mieć jednoznacznie określony typ (opcja: „Use Specified

Scaling”)

• program będzie generował odpowiedni kod dla bloków z ustawionym nasyceniem

wartości minimalnych i maksymalnych (opcja „Saturate to max or min when

overflows occur”)

• kod C++ musi posiadać taką strukturę aby moŜna go było poprawnie

przetransformować na język VHDL poprzez program A|RT Builder

• program nie będzie posiadał Ŝadnego interfejsu graficznego a uruchamiać się go

będzie z linii poleceń (takie podejście ułatwia umieszczenie programu w skryptach

automatyzujących proces projektowania)

5.2.2. Metoda generacji kodu

Podstawowym problem podczas generacji kodu, jest opisanie równoległego

przepływu danych stosowanego w Simulinku, w postaci sekwencyjnej wymaganej przez

jezyk C++. Dodatkowe utrudnienie wynika z faktu, Ŝe nie moŜna zbyt mocno przebudować

struktury generowanego algorytmu, dlatego Ŝe kod C++ będzie w późniejszej fazie


- 34 -

zamieniany na VHDL, w którym zapis połączeń równoległych jest poŜądany i który

owocuje zwiększeniem wydajności obliczeń w finalnym układzie logicznym.

Rozpatrując schemat Simulinka naleŜy zauwaŜyć, Ŝe kaŜdy standardowy blok jest

funkcją a co z tym idzie ma zawsze jedno wyjście. Liczba wejść moŜe być róŜna, ale kaŜde

z wejść moŜe być podłączone tylko do jednego wyjścia (rys. 5.2-1). Z tego wejścia

pobierana jest zmienna do obliczeń danego bloku. Wystarczy, więc opracować dla kaŜdego

bloku równorzędną operację matematyczną a następnie uŜyć zmiennych reprezentujących

wyjścia do wyliczenia danej funkcji.

Rys. 5.2-1 Przykładowy schemat Simulinka

Przykładowo z powyŜszego rysunku wynika, Ŝe istnieją zaleŜności (celowo zapisane w

losowej kolejności):

DelayInSum

SumDelay

MulOut

SumConstMul

+==

=+=

:

:

:

:

(5.1)

RozwaŜając taki zapis od razu nasuwa się myśl, Ŝe z punktu widzenia kodu sekwencyjnego

kolejność zapisu ma bezpośredni wpływ na poprawność odwzorowania. O ile pierwsze dwa

równania niejako wskazują poprawną kolejność o tyle wystąpienie pętli sprzęŜenia

zwrotnego komplikuje sprawę. PoniewaŜ generator ma za zadanie generować kod dla

algorytmów z dyskretnym czasem próbkowania to zawsze w pętli sprzęŜenia zwrotnego

musi wystąpić blok opóźniający. Blok, którego aktualna wartość zaleŜy nie od jego

aktualnego wejścia, ale od historii jego wejść. Dlatego wystarczy pamiętać tą historię (w

pamięci) i odpowiednio ją aktualizować:


- 35 -

><=

+=><

Stop

SumDelay

DelayInSum

Start

Delaymemory

:

...

:

...

""

(5.2)

Innym typem bloków, które przerywają pętlę to bloki, których wyjście nie zaleŜy od

wejścia, ale od parametru podanego przez uŜytkownika. Przykładowo dla bloku typu stała,

któremu uŜytkownik nadał wartość 3 wystarczy napisać:

3=Const (5.3)

Pozostaje tylko do rozwiązania, w jaki sposób ustawić w odpowiedniej kolejności

zapis obliczeń matematycznych tak, aby uzyskać poprawny kod sekwencyjny. MoŜna to

zrobić wg algorytmu zaproponowanego przez autora przedstawionego na rys. 5.2-2.

Zgodnie z nim schemat analizowany jest od końca, czyli od bloków wyjściowych.

Wykorzystując technikę rekurencji wystarczy tylko sprawdzać czy dany blok, który podlega

analizie ma wszystkie znane wejścia, czyli czy bloki poprzedzające zostały juŜ

wygenerowane. Algorytm pobiera pierwsze wyjście schematu a następnie ustala blok

poprzedzający, jeśli dany blok juŜ był generowany to powraca generując zapis

przyporządkowujący wynik dla danego wyjścia i przechodzi do kolejnego z wyjść. JeŜeli

dany blok nie został wygenerowany to generator najpierw sprawdza czy dany blok ma znane

wszystkie wyjścia i jeśli tak to generuje jego matematyczny opis, jeśli nie to skacze kolejno

do bloków poprzedzających

Dla przedstawionego schematu na rys. 5.2-1 proces rozpoczyna się od bloku Out.

Sprawdzamy czy blok Mul ma znane wszystkie wyjścia, poniewaŜ Ŝadne z nich nie było

generowane to wybieramy kolejno bloki poprzednie i je generujemy. Pierwszy z nich jest

blokiem typu stała, który nie ma wejść. Generowany jest, więc kod dla niego, blok

oznaczany jest flagą „wygenerowany” i procedura powraca do bloku, który wywołał jego

generację, czyli Mul. Kolejnym wejściem do Mul jest wyjście z Sum. Analiza bloku Sum

wskazuje, Ŝe pierwsze jego wejście (In) jest znane gdyŜ jest to główne wejście do

algorytmu, drugie wejście zaleŜy od Delay. Opis Delay moŜna wygenerować, poniewaŜ stan


- 36 -

zaleŜy od wartości w poprzednim cyklu, naleŜy tylko pamiętać, aby na koniec kodu

zaktualizować zmienne tak, aby były one poprawne w następnym toku obliczeń.

Wygenerowany kod dla Delay pozwala wygenerować kod Sum a następnie generator wraca

do bloku, który „zaŜądał” wyjścia od Sum czyli Mul. PoniewaŜ wszystkie sygnały dla Mul

są juŜ znane moŜna wygenerować jego kod i przejść do kolejnego z wyjść.

Rys. 5.2-2 Procedura generowania kodu C ze schematu.


- 37 -

Jeśli przyjąć, Ŝe pracujemy na typach całkowitoliczbowych typu int to

wygenerowany kod przedstawiałby się jako:

void Kod(int In, int &Out)

{

int Const=3;

static int Delay=0; zmienna inicjalizująca pobierana jest z bloku

int Sum=In+Delay;

Mul=Const+Sum;

Out=Mul;

Delay=Sum; //przygotowanie zmiennych do następnego cyklu

}

Opracowany algorytm oprócz tego, Ŝe jest prosty w implementacji posiada jeszcze

inną zaletę. W sposób automatyczny pomija gałęzie, które nie biorą udziału w obliczeniach i

jeśli ktoś omyłkowo poukładał bloki, które nie są związane z generowanym algorytmem, a

to oznacza, Ŝe nie są powiązane z wyjściami to ich kod nie zostanie wygenerowany.

Do poprawnego generowania kodu wymagana jest jeszcze znajomość typów (w

przedstawionym przykładzie dla uproszczenia wybrano int). Tutaj problem jest prostszy, bo

typy bibliotek stałoprzecinkowych Simulinka maja jednoznaczne odwzorowanie w zapisie

wg A|RT library.

W dalszej części pracy pokazano szczegółowego sposób generowania kodu dla

róŜnych bloków Fixed-Point. Podczas opisu przejęto iŜ:

• Nazwa wynik reprezentuje dowolną nazwę bloku uŜywaną jako wyjście bloku

• Nazwa zrodlo reprezentuje dowolną nazwę bloku, z którego pobierany jest sygnał

dla tego bloku

• Pozostały ciąg instrukcji jest identyczny dla kaŜdego bloku o danym typie.

5.2.3. Typy bloków Simulinka moŜliwych do przetransformowania na kod C++

Program generujący kod w języku C++ opiera się na załoŜeniu, iŜ kaŜdemu blokowi

Simulinka z biblioteki Fixed-Point moŜna jednoznacznie przyporządkować ciąg instrukcji w

C++ . Ten ciąg instrukcji obejmuje operację arytmetyczną realizowaną w danym bloku. W

tabeli 2 przedstawiono bloki Simulinka moŜliwe do konwersji na język C++.


- 38 -

Nazwa bloku Symbol Kod C++ Uwagi

Stała - Constant

// Constant: wynik

Fix<16,10> wynik_=5.5;

Wartość stałej pobierana jest na podstawie

ustawień w oknie w Siumulinku. Zalecane jest

ustawienie opcji „Use Specified Scaling”.

Rzutowanie typu

- Conversion

// Conversion: wynik

Fix<16,10> wynik_=zrodlo_;

lub

// Conversion: wynik

Fix<16,10> wynik_ =oqc((zrodlo_),

fxpOqc::saturated());

W procesie generacji uwzględniana jest opcja

kontroli nasycenia („Saturate to max or min

when overflows occur”)

Filtr - FIR

// FIR: wynik

// main function in include "FIR_wynik_.h"

Fix<16,10> wynik_=FIR_wynik_(zrodlo_);

Na początku dodawany jest kod:

#include "FIR_wynik_.h"

extern inline Fix<16,10> FIR_wynik_(Fix<6,2>);

Dodatkowo generowany jest nowy plik o nazwie „FIR_wynik_.h”:

inline Fix<16,10> FIR_wynik_(Fix<6,2> value)

{

return 0;

}

Blok ten reprezentuje filtr i dla tego bloku nie jest

generowany pełny kod C++, ale tylko interfejs

komunikacji z funkcją filtra.


- 39 -

Wzmocnienie -

Gain

// Gain: wynik

Fix<16,10> wynik_=(Fix<16,10>(4.0))*zrodlo_;

lub

// Gain: wynik

Fix<16,10> wynik_=oqc(((Fix<16,10>(4.0))*zrodlo_),


Z zalecane jest ustawienie opcji „Use Specified

Scaling” Przy generacji bloku uwzględniana jest

opcja „Saturate to max or min when overflows

occur”.

Wejście -

Gateway IN

void algorytm

(

Fix<16,10> wynik

)

{

// Gateway In: wynik

Fix<16,10> wynik_=wynik;

}

Blok ten jest traktowany w sposób specyficzny.

W procesie generacji na kod C++ blok ten

decyduje o typie argumentów dla funkcji

realizującej przetwarzany schemat. Liczba bitów

w tym bloku określa wielkość słowa dla danego

wejścia w generowanej funkcji.

Wyjście -

Gateway OUT

void algorytm

(

Fix<16,10> &wynik

)

{

// Gateway Out: wynik

#ifdef __SYNTHESIS__

#pragma OUT wynik

#endif

wynik=zrodlo_;

}

W procesie transformacji na kod C++ blok ten

decyduje o wyjściach z generowanej funkcji. Typ

wyjściowy określany jest na podstawie bloku

poprzedzającego. Dla zachowania standardu

C++ wyjście przekazywane jest przez referencję

(operator „&”). Jako Ŝe referencja moŜe

oznaczać równieŜ wejście, zastosowano

polecenie dla generatora VHDL w postaci

„#pragma OUT nazwa_zmiennej_wyjściowej”

dla jednoznacznego określenia wyjścia.


- 40 -

Operator

logiczny -

Logical Operator

// Logical: wynik

Uint<1> wynik__1=1;

if (zrodlo1_==(Uint<1>(0))) {wynik__1=0;};

Uint<1> wynik__2=1;


Uint<1> wynik__3=1;


Uint<8> wynik_=(wynik__1 & wynik__2 & wynik__3);

Dla operacji NAND powyŜsza linia przedstawi się następująco:

Uint<8> wynik_=Uint<1>(1)^(wynik__1 & wynik__2 &

wynik__3);

Operacja OR:

Uint<8> wynik_=(wynik__1 | wynik__2 | wynik__3);

Oparacja NOR:

Uint<8> wynik_=Uint<1>(1)^(wynik__1 | wynik__2 |

wynik__3);

Oparacja XOR:

Uint<8> wynik_=(wynik__1 ^ wynik__2 ^ wynik__3);

Operacja NOT (w całości)

// Logical: wynik

Uint<1> wynik__1=1;

if (zrodlo_==(Uint<1>(0))) {wynik__1=0;};

Uint<8> wynik_=Uint<1>(1)^(wynik__1);

ZłoŜoność tej funkcji mimo prostoty działania

wynika z faktu, iŜ Fixed-Point dokonuje operacji

logicznej, a w układzie FPGA moŜemy

zaimplementować tylko operacje bitowe.

Operacje bitowe wymagają identycznej wielkości

wszystkich typów argumentów, co jest roŜne z

realizacją w Fixed-Point. NaleŜy zaznaczyć, iŜ

mimo rozległego opisu operacji realizującej ten

blok sam proces jest bardzo prosty i nie wymaga

wielu zasobów układu FPGA (uŜyty jest tylko

jeden bit).


- 41 -

Jednoargument

owa funkcja

ztablicowana -

Look-UP Table

W miejscu uŜycia tego bloku, generowany jest kod:

// LookUp: wynik

// main function in include "LookUp_wynik_.h"

Fix<16,10> wynik_=LookUp_wynik_(zrodlo_);

Na początku kodu dodawany jest deklaracja funkcji:

#include "LookUp_wynik_.h"

extern inline Fix<16,10> LookUp_wynik_(Fix<6,2>);

Oraz generowany jest nowy plik o nazwie „LookUp_wynik_.h”, w którym

zawarto definicję funkcji w postaci:

inline Fix<16,10> LookUp_wynik_(Fix<6,2> value)

{return 0;}

Blok ten jest specyficznym blokiem i dla tego

bloku generowany jest kod będący interfejsem

do tablicy. W wielu przypadkach układy FPGA

wyposaŜone są w tablice LookUpTable, co w

połączeniu z przedstawionym sposobem

generacji umoŜliwia efektywne wykorzystanie

części sprzętowej.

Dwu-

argumentowa

funkcja

ztablicowana -

Look-UP Table

(2D)

W miejscu uŜycia:

// LookUp2: wynik

// main function in include "LookUp2_wynik_.h"

Fix<16,10> wynik_=LookUp2_wynik_(zrodlo1_,zrodlo2);

Do początku kodu dodawany jest:

#include "LookUp2_wynik_.h"

extern inline Fix<16,10>

LookUp2_wynik_(Fix<6,2>,Fix<6,2>);

Dodatkowo generowany jest nowy plik o nazwie „LookUp2_wynik_.h”, w

którym zawarto:

inline Fix<16,10> LookUp2_wynik_(Fix<6,2> value1,

Fix<6,2> value2)

{return 0;}

Analogicznie jak LookUpTable jednowymiarowa


- 42 -

MnoŜenie -

Product

// Product: wynik

Fix<16,10> wynik_=(zrodlo1_*zrodlo2_*zrodlo3_);

W przypadku gdy zaznaczono opcje „Saturate ...” kod C++ wygląda

następująco:

// Product: wynik

Fix<16,10> wynik_=oqc((zrodlo1_*zrodlo2_*zrodlo3_),


Liczba wejść jest podawana jako parametr

bloku. Dla tego bloku realizowane jest mnoŜenie

tylko wielkości skalarnych.

Porównanie -

Relational

Operator

// Relational operator (<=):wynik

Ufix<8,0> wynik_=0;

if (zrodlo1_<=zrodlo2_) {wynik_=1;};

MoŜliwe operatory do wykorzystania

== jest równy ...

~= jest róŜny od ... (ten operator jest zamieniany

na !=)

< mniejszy niŜ ...

<= mniejszy lub równy ...

>= większy lub równy ...

> większy niŜ ...

Komparator z

histerezą -

Relay

// Relay: wynik

static Fix<16,10> wynik_=-1;

if (zrodlo_>=(Fix<16,10>(2))) {wynik_=1;}

else if (zrodlo_<=(Fix<16,10>(0))) {wynik_=-1;};

Parametry histerezy pobierane są z właściwości

bloku.

Nasycenie -

Saturation

Fix<16,10> wynik_=zrodlo_;

if (wynik_>(Fix<16,10>(0.5))) {wynik_=0.5;}

else if (wynik_<(Fix<16,10>(-0.5))) {wynik_=-0.5;};

Nasycenia z wykorzystaniem funkcji

dostarczonych z A|RT Library ograniczają się

tylko do wartości ekstremalnych danego typu

dlatego jedyną moŜliwością zrealizowania tej

operacji jest uŜycie konstrukcji warunkowej IF-

ELSE


- 43 -

Suma - Sum

// Sum: wynik

Fix<16,10> wynik_=+zrodlo1_-zrodlo2_+zrodlo3_;

W przypadku gdy zaznaczono opcje „Saturate ...” kod C++ wygląda

następująco:

// Sum: wynik

Fix<16,10> wynik_=oqc((+zrodlo1_-zrodlo2_+zrodlo3_),


Liczbę i znak sumowanych argumentów określa

się w oknie bloku.

Przełącznik -

Switch

Dla wejścia 1 o typie Ufix<10,6> , wejścia 2 o typie Fix <16,10> i wejścia 3

o typie Fix <16,15> kod C++ przedstawia się następująco:

// Switch: wynik

Fix<20,15> wynik_;

if (zrodlo2_<(Fix<16,10>(0.5))) {wynik_=zrodlo3_;}

else {wynik_=zrodlo1_;};

W zaleŜności od porównania na wyjściu

otrzymujemy wartość z wejścia 1 lub z wejścia

3. Podczas symulacji wejścia 1 i 3 mogą mieć

roŜny typ. Dla poprawnej generacji kodu

generator sam dobiera typ wyjściowy i miejsce

kropki tak, aby dobrany typ bezbłędnie zawierał

w sobie wartości wejścia 1 i wejścia 2, przy

moŜliwie minimalnym wykorzystaniu ilości bitów.

Opóźnienie -

Unit Delay

Ze względu na specyficzne działanie bloku „Unit delay” naleŜy

wygenerować kod C++ w dwóch miejscach.

W miejscu uŜycia:

// Unit Delay: wynik

static Fix<16,10> wynik_=-0.5;

I na końcu funkcji:

// Refresh wynik

static Fix<16,10> wynik_hold=zrodlo_;

static Uint<16> wynik_clock=(0.1/0.01)+1;

wynik_clock-=Uint<1>(1);

W bibliotece Fixed-Point nie moŜna określić typu

wyjściowego dla tego bloku. Przyjęty zostaje typ

bloku z którego pobierany jest sygnał.

Wymagane jest aby wartość „sample time” była

wielokrotnością globalnego czasu próbkowania.

Wielokrotność nie moŜe przekraczać wartości

65535 (ze względu na wielkość typu licznika).


- 44 -

if (wynik_clock==(Uint<1>(0)))

{

wynik_clock=(0.1/0.01);

wynik_hold=zrodlo_;

};


{wynik_=wynik_hold;}

Pamięć - Zero

Order Hold

// Zero-Order Hold: wynik

static Fix<16,10> wynik_=zrodlo_;

static Uint<16> wynik_clock=(0.1/0.01)+1;

wynik_clock-=Uint<1>(1);


{

wynik_clock=(0.1/0.01);

wynik_=zrodlo_;

};

Wymagane jest aby wartość „sample time” była

wielokrotnością globalnego czasu próbkowania.

Wielokrotność nie moŜe przekraczać wartości

65535 (ze względu na typ licznika).

Tabela 2 Wykaz bloków stałoprzecinkowych moŜliwych do konwersji na język C++ przez opracowany generator.


- 45 -

W przypadku dwóch ostatnich bloków (tabela 2) naleŜy zwrócić na ich róŜnicę w działaniu

(rys. 5.2-3).

Rys. 5.2-3 RóŜnica w działaniu bloku Unit Delay (po lewej) i Zero-Order Hold (po prawej)

Standardowe bloki Simulinka (np. Scope, Step) podczas generacji kodu C++ są pomijane,

natomiast bloki niestandardowe z innych bibliotek (Toolbox’y róŜne od Fixed-Point

Blockset) są generowane i oznaczane jako „undefined” Dla tych bloków generowany jest

kod C++ w postaci:

// Undefined Block: wynik

Fix <2,1> wynik_=0;

5.2.4. Rezultat działania opracowanego generatora

Program moŜna uruchomić z linii poleceń:

> mdl2c plik

Gdzie plik to na nazwa pliku z rozszerzeniem „mdl” zapisanego w Siumulinku.

Lub poprzez dodatkową aplikacją opracowaną przez autora pracy zawierającą wygodny

interfejs graficzny (rys. 5.2-4)

Rys. 5.2-4 Okno programu ułatwiającego uruchomienie generatora.


- 46 -

Rezultatem działania opracowanego programu jest utworzenie plików:

• plik.cxx – główny plik zawierający wygenerowany kod w języku C++ wraz z

interfejsem niezbędnym do wykorzystania go jako S-funkcji.

• src.cxx – plik zawierający odwołanie do głównego pliku sterownika (w postaci:

#include „plik.cxx”). Ten plik przeznaczony jest dla programu A|RT Builder, który

wymaga, aby kaŜdy projekt zawarty był w pliku „src.cxx”

• pliki z rozszerzeniem „.h” dla specyficznych bloków: FIR, LookUp i LookUp2. Pliki

te przeznaczone są do późniejszego wykorzystania przez projektanta

5.2.5. Ograniczenia programu

Opracowany generator został zbudowany tak, aby jak najmniej modyfikować

zamierzenia projektanta, oraz aby wygenerowany kod był jednoznaczny i zgodny ze

składnią C++ przy zachowaniu koncepcji struktury zawartej w schemacie Simulinka.

Spełnienie takich wymagań powoduje kilka ograniczeń:

• nazwy bloków uŜywane są jako nazwy zmiennych i nie mogą zawierać znaków

specjalnych;

• nazwy mają ograniczenie do 32 znaków;

• nazwy kolejnych podsystemów są dołączane do nazwy danej zmiennej i w

przypadku uŜycia wielu podsystemów nazwa zmiennej moŜe mieć długość

przekraczającą moŜliwości uŜytego kompilatora (dla kompilacji S-Funkcji);

• nazwy bloków nie mogą być identyczne z zarezerwowanymi nazwami dla języka

C++ i VHDL (np. blok gateway in nie moŜe się nazywać „if” czy „for”) ;

• do niektórych zmiennych dołączane są przyrostki tj: _hold, _clock i naleŜy uwaŜać

aby nie powstał konflikt z innymi zmiennymi np. zmiennymi wejścia/wyjścia.;

• zaokrąglenia podczas operacji arytmetycznych skierowane są zawsze do minus

nieskończoności (co odpowiada ustawieniom „floor”);

• zalecane jest aby wszystkie stałe były określone w postaci liczbowej. Jeśli z jakiś

powodów zaistnieje potrzeba uŜycia stałych w postaci nazw to program wygeneruje

je poprawnie ale na początku kodu naleŜy dopisać „ręcznie” dyrektywę preprocesora

„define” określającą wartości tych stałych;

• niektóre bloki w Fixed-Point pozwalają na automatyczne dobieranie miejsca kropki

dla podanej stałej, program dopuszcza taką opcję tylko dla stałej w postaci liczby;


- 47 -

dla stałej podanej jako nazwa wymaga sprecyzowania co do typu

stałoprzecinkowego (zawsze naleŜy wybrać opcję „Use Specified Scaling”);

• program rezerwuje pamięć na: 32 zmienne wejściowe, 32 zmienne wyjściowe, 256

linii połączeniowych, 128 bloczków i przekroczenie tego limitu moŜe spowodować

nieprzewidziane zachowanie programu;

• wejścia i wyjścia ustawiane są w kolejności alfabetycznej;

• wszystkie pola w blokach Fixed-Point powinny być wpisane jednoznacznie i

wstawianie „białych” znaków typu spacja lub tabulacja moŜe spowodować błędną

generację kodu C++;

• w procesie generacji kodu standardowe bloki Simulinka (tj. Scope, Step) są

pomijane, natomiast bloki niestandardowe z innych bibliotek (Toolbox’y róŜne od

Fixed-Point Blockset) są generowane i oznaczane jako „undefined”; dla tych

bloków zmienna reprezentująca ten blok posiada wartość stałą równą 0;

• zmienna wejściowa nie moŜe posiadać nasycenia, dlatego Ŝe wejście ma zawsze

stałą ograniczoną wielkość

• dla bloków FIR , LookUp i LookUp2 generowane są dodatkowe pliki w których

naleŜy samemu umieścić wymagany kod;

• wszystkie sygnały muszą być skalarne;

• bloki Fixed-Point nie moŜliwe do zrealizowane: GUI, Matrix-Gain, Conversion

Inherited (oznaczane są w postaci „undefined”);

Przestrzeganie tych ograniczeń pozwala automatycznie generować kod C++ opisany przez

dowolny schemat blokowy.

Wygenerowany w ten sposób kod moŜna bezpośrednio skompilować do postaci s-

funkcji, umoŜliwiając symulację algorytmu, ale juŜ w postaci, jaka będzie zamieniana na

język VHDL. Dodatkowo dołączając na tym etapie inne funkcje opisujące interfejsy

wejścia/wyjścia, istnieje moŜliwość symulacji całości w Simulinku.

W przypadku potrzeby zamiany generowanego kodu na język VHDL, zadanie

projektanta sprowadza się do otwarcia projektu z wygenerowanym plikiem „src.cxx” i

wybrania opcji „Compile” w aplikacji ART.|Builder [1]. Lub poprzez przycisk „Go” w

interfejsie (rys. 5.2-4) opracowanym przez autora. Wówczas w katalogu gdzie znajdował się

plik ze schematem Simulinka, pojawi się plik zawierający pełny opis algorytmu, zapisany w

języku VHDL. Wygenerowany kod VHDL nadaje się do bezpośredniego uŜycia przez

aplikację dedykowaną dla danego układu FPGA.


- 48 -

5.3. Przykład zastosowania opracowanego generatora

Aby zobrazować uŜyteczność opracowanego generatora przedstawiono przykład, w

którym opracowany algorytm sterowania został zaimplementowany w układzie FPGA. Od

fazy sformułowania stałoprzecinkowego algorytmu procedura przebiegała automatycznie

[61].

5.3.1. Obiekt sterowania

Jako obiekt wybrano pryzmatyczny człon robota SCARA (rys. 5.3-1). Suwak

poruszany jest wzdłuŜ sztywnych prętów za pomocą silnika prądu stałego poprzez linkę

stalową. Pozycja suwaka mierzona jest za pomocą optycznego przetwornika obrotowo-

impulsowego o rozdzielczości odpowiadającej przemieszczeniu 0.2µm [39].

a

b

c

d

e

f

g

Rys. 5.3-1 Pryzmatyczny człon robota: a - silnik prądu stałego, b - przekładnie zębate, c – linka stalowa, d –

rolka, e – suwak, f – prowadnice, g – optyczny przetwornik obrotowo-impulsowy

5.3.2. śądany algorytm sterowania

Jako algorytm sterowania przyjęto nieliniowy sterownik PID z nieliniowościami w

członie całkującym. Nieliniowości układu dotyczyły nasycenia wartości w integratorze i

zerowaniu jej przy zmianie znaku sygnału błędu połoŜenia. Wstępne parametry

współczynników PID otrzymane zostały metodą Zieglera-Nicholosa.

Nieliniowy sterownik PID wykorzystuje sygnał błędu ek, który jest róŜnicą pomiędzy

pozycją zadaną xd i rzeczywistą (mierzoną) xk w chwili czasu k


- 49 -

kdk xxe −= (5.4)

W porównaniu z liniowym algorytmem PID, w zastosowanym prawie sterowania:

kikkdkpk rKeeKeKu ⋅+−⋅+⋅= − )( 1 (5.5)

człon całkujący jest zmodyfikowany przez nasycenie wartości integratora sk:

≤+≤+<+>+

=

maxmin

minmin

maxmax

;

;

;

sesses

sess

sess

s

kkkk

kk

kk

k (5.6)

i przez jego zerowanie za kaŜdym razem, gdy sygnał błędu zmienia znak:

≥⋅<⋅

=−

−

0;

0;0

1

1

kkk

kkk ees

eer (5.7)

Sterownik został zbadany eksperymentalnie stosując metodę szybkiego prototypowania. W

procesie tym dostrojono parametry tak, aby uzyskać jak najdokładniejsze śledzenie

trajektorii. Częstotliwość próbkowania dobrano na 100Hz. Wartość ta wynikała z

moŜliwości realizacji zadanych trajektorii przez badany obiekt. Sterownik zamodelowany w

środowisku Simulink w wersji zmiennoprzecinkowej przedstawiono na rys. 5.3-2.

Rys. 5.3-2 Zmiennoprzecinkowy algorytm sterowania


- 50 -

Następnie opracowano wersję stałoprzecinkową, której poprawność sprawdzono stosując

proces szybkiego prototypowania (rys. 5.3-3).

Rys. 5.3-3 Stałoprzecinkowa wersja sterownika, uŜyta do implementacji w FPGA

Stałoprzecinkową wersję sterownika poddano procesowi automatycznej implementacji w

układzie FPGA

5.3.3. Automatyczna implementacja sterownika

W pierwszej kolejności wykorzystano opracowany generator i wygenerowano kod

C++ wykorzystujący bibliotekę stałoprzecinkową. Poprawność wygenerowanego kodu

sprawdzono poprzez jego skompilowanie do postaci S-Funkcji i porównanie go z wersją

stałoprzecinkową. Następnie stosując aplikację A|RT Builder wygenerowano algorytm w

języku VHDL. Stosując aplikację Quartus [24] dedykowaną dla układu FPGA firmy Altera,

dołączono wygenerowany opis w języku VHDL do przygotowanych bloków wejścia

wyjścia i przyporządkowano połączenia z konkretnymi nogami układu FPGA.

Algorytm sterownia zaimplementowano w układzie Altera EP20K100E (rys. 5.3-4).

Cała aplikacja, obliczenie sterowania, protokół komunikacji zajął 34% elementów

logicznych i 1% pamięci. Układ pracując z zegarem 6.25MHz potrzebował na wyznaczenie

sterowania 160ns, co po dodaniu 1.9µs na pozostałe zadania daje maksymalną częstotliwość

próbkowania ponad 480kHz. Częstotliwość jest ograniczona przez obsługę urządzeń

zewnętrznych. Wskazania programu Quartus pokazywały, Ŝe sam algorytm mógłby był

wyliczany z częstotliwością ponad 10MHz, co przy wymaganej 100Hz daje bardzo duŜy

zapas.


- 51 -

Rys. 5.3-4 Altera EP20K100E wraz z przetwornikami cyfrowo/analogowymi

Rezultat działania takiego sterownika przedstawiono na rys. 5.3-5.

Rys. 5.3-5 Wyniki eksperymentu dla trajektorii sinusoidalnej (po lewej) i przebiegu piłokształtnego (po

prawej)

Dla przedstawionego przykładu istotne jest, Ŝe wszystkie obliczenia wykonywane były w

sposób sprzętowy.

Przedstawiony proces implementacji jest poprawny gdyŜ:

• algorytm sterowania został zrealizowany przez układ FPGA

• procedura implementacji przebiegała w sposób automatyczny

• kod wygenerowany przez opracowany generator daje identyczny rezultat co

algorytm zapisany za pomocą bloków stałoprzecinkowych

• uzyskany błąd realizacji trajektorii jest identyczny z błędem uzyskanym podczas

realizacji trajektorii przez wirtualny sterownik zbudowany w procesie szybkiego

prototypowania


- 52 -

6. Projekt robota równoległego

Efektywności zastosowania podejścia mechatronicznego w projektowaniu robotów

równoległych wykonano przeprowadzając procedury projektowania dla wybranego robota

równoległego. Istnieje wiele konstrukcji robotów równoległych jednak najbardziej

wartościowe byłoby przeprowadzenie procedury projektowania dla konstrukcji nowej. Daje

to moŜliwość pokazania korzyści płynących z zastosowania podejścia mechatronicznego.

Zgodnie z przyjętymi zasadami projektowania mechatronicznego, postawiono wytyczne, co

do przeznaczenia robota, przyjęto koncepcję konstrukcji mechanicznej, opracowano

wielomasowy model symulacyjny, wyprowadzono równania opisujące kinematykę

projektowanego robota a następnie zbudowano i zaimplementowano układ sterowania.

6.1. Rozwiązanie konstrukcyjne robota równoległego

W przypadku projektowania konstrukcji mechanicznej robotów, poszukuje się

rozwiązań, które pozwolą na wykorzystanie danego robota w jak najszerszej grupie zadań.

O ile w przypadku robotów o kinematyce otwartej uzyskanie duŜej elastyczności nie

stanowi problemu, to w przypadku robotów równoległych pojawia się bariera w postaci

kosztów projektu i wykonania takiego robota. Wynika to z faktu, Ŝe robot równoległy składa

się z wielu ramion działających bezpośrednio na platformę. Przestrzeń robocza ściśle zaleŜy

od zakresu ruchu napędów i jest częścią wspólną wszystkich moŜliwych połoŜeń kaŜdego z

ramion. Oznacza to, Ŝe zwiększanie przestrzeni roboczej tylko w jednej osi wiąŜe się z

jednoczesnym zwiększaniem zakresu ruchów wszystkich ramion, a utrzymanie

odpowiedniej sztywności przy zwiększaniu dynamiki wymaga zastosowania lepszych

materiałów lub większych przekrojów elementów konstrukcji w miejscach najbardziej

naraŜonych na niekorzystne napręŜenia. Większe przekroje to większe masy, które naleŜy

poruszać podczas pracy. W efekcie kaŜde zwiększanie parametrów robota równoległego

związanych z przestrzenią i dynamiką podnosi koszty wymaganych napędów i to w

znacznie większym stopniu niŜ wynikałoby to z uzyskanych efektów. Korzystne jest, więc

precyzyjne określenie grupy zadań, jakie ma projektowany robot równoległy realizować.

W Katedrze Robotyki i Mechatroniki AGH opracowano i zbudowano dwa roboty

równoległe o zbliŜonej kinematyce, ale o róŜnym zastosowaniu. Pierwszy przeznaczony do

montaŜu i paletyzacji [57], drugi jako konstrukcja wsporcza frezarki [58]. Konstruując


- 53 -

części mechaniczne robota równoległego zgłoszono kilka wniosków patentowych, których

współtwórcą jest autor pracy [34][32][33][71].

6.1.1. Robot równoległy przeznaczony do paletyzacji

Zastosowaniem projektowanego robota było uzyskanie moŜliwości realizacji zadań

związanych z przenoszeniem obiektów. W przypadku robotów o kinematyce otwartej

przeznaczonych do zadań manipulacyjnych największą grupę stanowią roboty kartezjańskie,

które wprost za pomocą trzech napędów odpowiednio równoległych do osi X, Y, Z realizują

ruch chwytaka, za pomocą, którego przenoszone są przedmioty. ZałoŜono, Ŝe projektowany

robot będzie posiadać trzy napędy oraz będzie mieć moŜliwość manipulacji obiektami w

układzie kartezjańskim XYZ. Z definicji robot równoległy powinien posiadać łańcuchy

kinematyczne oddziaływujące bezpośrednio na platformę/chwytak. Oznacza to, Ŝe dla

rozpatrywanego przypadku najlepszym rozwiązaniem byłoby, aby projektowany robot

posiadał trzy łańcuchy kinematyczne, połączone wspólnym przegubem z zamocowanym

chwytakiem. Łańcuchy kinematyczne będą tworzyć ramiona, których głównym elementem

będą napędy wraz prowadnicami. Takie załoŜenie znacznie upraszcza konstrukcję

mechaniczną robota. Pozostaje jedynie do rozwiązania problem połączenia ramion

wspólnym przegubem tak, aby pełnił on rolę platformy, z moŜliwością zamocowania

chwytaka.

Rys. 6.1-1 Przegub pozwalający połączyć trzy ramiona.

Na rys. 6.1-1 przedstawiono rozwiązanie w postaci przegubu, za pomocą którego moŜna

połączyć trzy ramiona tak aby przy odpowiedniej orientacji osi napędów, oś przegubu była

zawsze prostopadła do podstawy robota [32]. Przegub składa się z uchwytu głównego 1 i

dwóch uchwytów pomocniczych 2 obróconych o 180 stopni. Do tych elementów


- 54 -

przymocowano kolejne przeguby obrotowe 3 o jednym stopniu swobody. Do przegubów

mocuje się ramiona robota.

Rys. 6.1-2 Elementy konstrukcyjne przegubu głównego: 1- uchwyt główny 2-uchwyt pomocniczy 3-

mocowanie do ramienia robota 4-wał drąŜony 5- tuleja dystansowa gwintowana 6- tuleja dystansowa główna

7- tuleja dystansowa pomocnicza 8- łoŜysko 9- sworzeń 10- pierścień osadczy spręŜynujący 11- otwory

gwintowane przeznaczone do przymocowania chwytaka

Przedstawiony przegub łączący trzy ramiona pełni istotną rolę w konstrukcji robota.

Jego sposób wykonania oraz dobór elementów składowych w znacznym stopniu wpływa na

ostateczne parametry robota takie jak przestrzeń robocza i dokładność. W zaproponowanym

rozwiązaniu występują pary obrotowe o jednym stopniu swobody. Oznacza to, Ŝe istnieje

moŜliwość zastosowania łoŜysk tocznych, a przy ich odpowiednim zamocowaniu

moŜliwość skasowania luzów. Rozwiązanie to jest przedmiotem zgłoszenia patentowego

współtworzonego przez autora pracy [32]. Zrealizowane rozwiązanie przedstawiono na rys.


- 55 -

6.1-2. Uchwyt główny 1 pełni rolę bazy przegubu. Znajdujący się na nim wał drąŜony 4

wyznacza wspólną oś na uchwytów pomocniczych. W skład uchwytów pomocniczych

wchodzą dwa łoŜyska 8 blokowane przez pierścienie 10. Tuleje dystansowe 7 wraz z

dokręcanymi tulejami gwintowanymi 6 zapewniają wykasowanie luzów. Do kaŜdego z

uchwytów dołączono mocowania ramion 3 poprzez przegub obrotowy oparty na elementach

7 8 9 10.

Rys. 6.1-3 Półprzekrój (po lewej) przez uchwyty przegubu. 1- uchwyt główny, 2-uchwyt pomocniczy, 3 – wał

drąŜony, 4 – tuleja dystansowa gwintowana, 5 – tuleja dystansowa, 6 – łoŜysko, 7 – pierścień osadczy

spręŜynujący. Po prawej kolorem niebieskim i zielonym pokazano uchwyty pomocnicze.

W efekcie uzyskano konstrukcję przegubu zwartą i pozbawioną luzów (rys. 6.1-3). Przegub

taki pełni rolę platformy i mocowanie chwytaka moŜna zrealizować albo poprzez połączenie

chwytaka z uchwytem głównym poprzez nawiercone otwory, albo bezpośrednio do wału

drąŜonego. Dla chwytaka w postaci przyssawki wybrano rozwiązanie drugie wykorzystując

wał drąŜony jako fragment przewodu doprowadzającego podciśnienie. Zbudowany i

zamontowany przegub w robocie równoległym przedstawiono na rys. 6.1-4.


- 56 -

Rys. 6.1-4 Widok rzeczywistego przegubu zamontowanego w robocie przeznaczonym do paletyzacji.

Rys. 6.1-5 Schemat kinematyczny zaprojektowanego robota równoległego.

Schemat kinematyczny zaprojektowanego robota przedstawiono na rys. 6.1-5. W

skład robota wchodzą trzy identyczne ramiona I, II , III . KaŜde z ramion połączone jest do

podstawy robota za pomocą przegubów obrotowych A, B, C. Poziome osie tych przegubów

są równoległe do odpowiednich osi na przegubie głównym D i są to A-A’, B-B’, C-C’. W


- 57 -

kaŜdym ramieniu znajduje się część nieruchoma 2, która zawiera w sobie stojan napędu z

radiatorem i przymocowanym wózkiem prowadnicy 4, oraz cześć wysuwną ruchomą 3.

Dobór miejsc mocowania napędów oraz orientacja osi wszystkich przegubów powodują, Ŝe

chwytak 7 wykonuje ruchy translacyjne w płaszczyznach X, Y, Z. PoniewaŜ uchwyt główny

przegubu połączony jest z ramieniem 1, to orientacja jego wzdłuŜ osi prostopadłej do

podstawy zaleŜy od aktualnej pozycji X,Y. Przedstawione rozwiązanie zawarte jest w

zgłoszeniu patentowym, współtworzonym przez autora pracy [34].

Rys. 6.1-6 Rysunek robota przeznaczonego do paletyzacji w programie AutoCAD (po lewej), wielomasowy

model w programie VisualNastran (po prawej).

Projekt robota wraz z podstawą wykonano w aplikacji AutoCAD. Korzystając z bazy

elementów normowanych, elementów katalogowych oraz własnych, wykonano projekt w

postaci brył. Takie podejście umoŜliwiło po pierwsze wykrycie wszelkich nieprawidłowości

geometrycznych, po drugie sprawdzono poprawność dopasowania elementów katalogowych


- 58 -

wraz z elementami projektowanymi oraz po trzecie uzyskano moŜliwość automatycznego

utworzenia wielomasowego modelu za pomocą programu VisualNastran (rys. 6.1-6). Mając

model wielomasowy moŜna było przeprowadzić symulację pracy robota w tym sprawdzić

zakresy ruchów napędów, moŜliwość występowania kolizji oraz wartości sił koniecznych do

poruszania robotem.

Rys. 6.1-7 Widok opracowanego robota przeznaczonego do montaŜu i paletyzacji.


- 59 -

Satysfakcjonujące rezultaty symulacji na modelu wielomasowym pozwoliły przejść do

kolejnego etapu, jakim było wykonanie brakujących elementów oraz montaŜ prototypu

robota. Widok rzeczywistej konstrukcji robota równoległego przeznaczonego do montaŜu i

paletyzacji przedstawiono na rys. 6.1-7.

6.1.2. Robot równoległy jako konstrukcja wsporcza obrabiarki

W przypadku projektowania robota jako konstrukcji wsporczej obrabiarki naleŜy

postawić odmienne załoŜenia, w stosunku do projektu robota specjalizowanego do

paletyzacji lub montaŜu. Po pierwsze w procesie skrawania występują dodatkowe siły

związane z obróbką. Oznacza to, Ŝe wartości napręŜeń w elementach konstrukcyjnych

robota są duŜo większe niŜ w robocie przeznaczonym do paletyzacji. Jedynym wyjściem

pozwalającym utrzymać odpowiednią wytrzymałość konstrukcji jest zwiększanie gabarytów

elementów składowych robota. Większe gabaryty są przyczyną większych mas, jakie naleŜy

poruszać, oraz większych sił potrzebnych do skompensowania siły grawitacji działającej na

ramiona robota. Po drugie prowadzenie narzędzia wymaga utrzymania tej samej

dokładności pozycji w kaŜdym miejscu przestrzeni roboczej. W przypadku robota

przeznaczonego do montaŜu, przede wszystkim zwraca się uwagę na dokładność

pozycjonowania do zadanego punktu a dokładność pozycji w czasie realizacji trajektorii nie

jest juŜ tak istotna. Utrzymanie odpowiedniej dokładności wiąŜe się ściśle z jakością układu

sterowania i z moŜliwością wyliczenia parametrów związanych z kinematyką i dynamiką

rozpatrywanego robota. W przypadku robota przeznaczonego do paletyzacji, brak

moŜliwości uzyskania rozwiązania zadania kinematyki prostej w postaci algebraicznej

wymusił zastosowanie metod numerycznych oraz przyjęcia uproszczonego modelu

dynamiki. Dla zadań montaŜu i paletyzacji było to w zupełności wystarczające. Okazuje się,

Ŝe w przypadku robotów przeznaczonych do prowadzenia narzędzia (śledzenia trajektorii)

dokładność i szybkość obliczeń związanych z kinematyką i dynamiką znacznie wpływa na

końcową dokładność realizacji trajektorii. Trudność w znalezieniu rozwiązania zadania

kinematyki prostej wymusiła modyfikację budowy robota. Okazuje się, Ŝe w rozpatrywanej

konstrukcji istnieje moŜliwość tak usytuowania jego osi obrotów przegubów, Ŝe po pierwsze

łatwo moŜna znaleźć rozwiązanie zadania kinematyki prostej w postaci algebraicznej, po

drugie przyjęty uproszczony model dla pierwszej wersji robota duŜo lepiej odwzorowuje

wersje robota przeznaczonego do prowadzenia narzędzia skrawającego.


- 60 -

Rys. 6.1-8 Widok ramienia (po lewej), równoległobok zawarty między osiami przegubów (po prawej).

JeŜeli punkty, przez które przechodzą osie obrotów tworzą równoległobok (rys. 6.1-8) to w

kaŜdym dowolnym połoŜeniu przegub łączący trzy ramiona zachowuje się jak przegub

kulowy, a co za tym idzie długości równoległoboków w ramionach wprost określają

odległość chwytaka od podstawy i są bezpośrednio zawarte w równaniach zadania

kinematyki prostej i odwrotnej. Pomysł ten zawarto w zgłoszeniu patentowym, który

współtworzył autor pracy [71].

Rys. 6.1-9 Rysunek robota przeznaczonego do frezowania w aplikacji AutoCAD (po lewej), wielomasowy

model w aplikacji VisualNastran (po prawej).


- 61 -

Rys. 6.1-10 Widok opracowanego robota przeznaczonego do frezowania

Przyjęcie innej koncepcji budowy robota w stosunku do wersji przedstawionej w

poprzednim punkcie wymusiło przeprowadzenie procedury projektowania

mechatronicznego od początku. Wykorzystując narzędzia CAD zaprojektowano konstrukcję

mechaniczną, następnie zbudowano jego model wielomasowy (rys. 6.1-9).

Wykonany prototyp robota równoległego przedstawiono na rys. 6.1-10.


- 62 -

6.2. Model

6.2.1. Zadanie odwrotne kinematyki

Jak pokazują dotychczasowe badanie, w przypadku manipulatorów równoległych,

znalezienie rozwiązania zadania odwrotnego kinematyki jest znaczne prostsze niŜ zadania

prostego [83]. Wynika to z faktu, Ŝe dla zachowania korzystnej cechy manipulatora

równoległego, jaką jest wysoka sztywność, poszczególne ramiona tworzące zamknięty

łańcuch kinematyczne, są zwykle prostymi mechanizmami. W wielu przypadkach, gdy

napędy w ramionach działają wprost na platformę, zaleŜność pomiędzy ruchem końcówki

robota, a wysunięciem napędów jest wprost odległością pomiędzy punktem początku i

końca zaczepienia napędu. Taka sytuacja występuje w kinematyce rozpatrywanego robota

(rys. 6.2-1), gdzie trzy identyczne ramiona I, II , III , poprzez napędy liniowe znajdujące się

na nich działają bezpośrednio na platformę. KaŜde z ramion tworzy łańcuch kinematyczny,

którego początek jest związany z przyjętą podstawą, a połączenie końca ma bezpośredni

wpływ na połoŜenie platformy.

Rys. 6.2-1 Struktura kinematyczna robota

KaŜde z ramion moŜna przedstawić na płaszczyźnie równoległej do tego ramienia

(rys. 6.2-2). Określając początek pojedynczego koniec ramienia, oraz formułując zaleŜności

geometryczne wynikające z konstrukcji mechanicznej, uzyskano charakterystyczne wymiary

pojedynczego ramienia (rys. 6.2-3).


- 63 -

Rys. 6.2-2 Widok jednego z ramion z zaznaczonymi charakterystycznymi wymiarami

Rys. 6.2-3 Charakterystyczne wymiary, występujące przy wyprowadzeniu równań kinematycznych, wraz z

zaznaczonymi punktami początku i końca ramienia

Długość członu pryzmatycznego 3 oznaczona jako la (co odpowiada kolejno l1, l2, l3)

zawarta jest pomiędzy punktem Pp, o współrzędnych (xp, yp, zp=0) wynikającym z

przecięcia osi symetrii złącza 2 a środkiem obrotu złącza 4. W układzie mierzone jest

wysuniecie napędu liniowego l, którego oś jest odsunięta od punkt Pp o odległość A.

l

A

B


- 64 -

ZaleŜność wiąŜącą mierzoną odległość l z długością członu pryzmatycznego la określa

następujące równanie:

22 All a −= (6.1)

Odległość B wynika z przesunięcia środka obrotu złącza 4 od głównej osi platformy

przechodzącej przez koniec ramienia P o współrzędnych (x, y, z). Punkt P jest wspólnym

końcem wszystkich ramion i środkiem ruchomej platformy. Pomocnicza wielkość K będąca

rzutem la na płaszczyznę xy, pozwala powiązać współrzędną z platformy z długością la

poprzez:

222 Kzla += (6.2)

Odległość BK + , będąca rzutem długości ramienia ( PPp ) na płaszczyznę xy wyraŜa się

jako:

( ) ( ) ( )222PP yyxxBK −+−=+ (6.3)

Przekształcając powyŜszą zaleŜność otrzymano wzór na wyznaczenie K:

( ) ( ) ByyxxK PP −−+−= 22 (6.4)

Po wstawieniu (6.4) do (6.2) a następnie (6.2) do (6.1) otrzymano równanie (6.5) opisujące

wysunięcie napędu l w zaleŜności od współrzędnych kartezjańskich środka platformy (x,y,z)

z parametrami xp, yp, będącymi współrzędnymi początku ramienia Pp:

( ) ( )( ) 222

22 AzByyxxl PP −+−−+−= (6.5)

Manipulator posiada trzy ramiona. Początek kaŜdego z ramion znajduje się na okręgu o

promieniu R (rys. 6.2-4)


- 65 -

Rys. 6.2-4 Rozmieszczenie miejsc mocowania ramion na płaszczyźnie xy – widok z góry

Współrzędne (xP,yP) dla ramienia pierwszego (I) wynoszą:

RRy

Rx

=⋅=

=⋅=

)90sin(

0)90cos(0

1

01 (6.6)

Analogicznie, dla ramienia drugiego (II) i trzeciego (III):

RRy

RRx

2

1)330sin(

2

3)330cos(

02

02

−=⋅=

=⋅= (6.7)

RRy

RRx

2

1)210sin(

2

3)210cos(

03

03

−=⋅=

−=⋅= (6.8)

Po podstawieniu współrzędnych początków ramion otrzymano układ równań, będący

rozwiązaniem zadania odwrotnego kinematyki:


- 66 -

( )( )

22

222

3

22

222

2

222

221

2

13

2

1

2

13

2

1

AzBRyRxl

AzBRyRxl

AzBRyxl

−+

−

++

+=

−+

−

++

−=

−+−−+=

(6.9)

PowyŜsze równanie znacznie upraszcza się, gdy odległości A i B przyjmą wartość zero.

Okazuje się, Ŝe istnieje moŜliwość zbudowania takiej konstrukcji [71] i wówczas istnieje

rozwiązanie analityczne zadania prostego kinematyki, oraz duŜo łatwiej jest sformułować

równania opisujące dynamikę tego układu.

Gdy A i B przyjmą wartość 0 to wówczas równanie zadania odwrotnego kinematyki

przedstawia się następująco:

( )

222

2

222

2

2221

21

23

21

23

zRyRxl

zRyRxl

zRyxl

+

++

+=

+

++

−=

+−+=

(6.10)

ZaleŜność (6.10) wykorzystano do planowania trajektorii robota równoległego.

6.2.2. Zadanie proste kinematyki

Poszukiwanie połoŜenia środka P platformy manipulatora równoległego we

współrzędnych kartezjańskich, na podstawie znajomości wysunięć napędów, jest zadaniem

znacznie bardziej skomplikowanym. Przyczyna tkwi w bezpośrednim działaniu wszystkich

równoległych łańcuchów kinematycznych na platformę. W układach otwartych moŜna

niezaleŜnie opisać kaŜdy człon łańcucha kinematycznego osobnym równaniem a następnie

dodając geometrycznie te równania, uzyskać równanie całego łańcucha [10]. W

manipulatorze równoległym kaŜde ramię moŜe przyjmować nieskończenie wiele połoŜeń

przy danym wysunięciu jego napędu. PołoŜenie środka platformy znajduje się w punkcie

przecięcia powłok określających dostępne połoŜenia końców poszczególnych ramion przy

danych wysunięciach napędów [83]. Rozwiązaniem zadania prostej kinematyki jest wiec


- 67 -

rozwiązanie układu równań nieliniowych, w których występują zaleŜności pomiędzy

połoŜeniem końcówki ramienia, a jego wysunięciem w funkcji współrzędnych

kartezjańskich punktu P. W zaleŜności od rozpatrywanej konstrukcji [34] lub [71]

zaleŜności te przedstawiają równania odpowiednio (6.9) lub (6.10). W przypadku

konstrukcji robota [34] próba analitycznego rozwiązania tego układu równań nie powiodła

się, poniewaŜ rozwiązania tego typu problemów sprowadzają się do wyznaczenia

pierwiastków wielomianów wysokiego rzędu [29] [86]. Następnie, spośród wielu

matematycznie wyznaczonych rozwiązań (pierwiastków) naleŜy zidentyfikować

realizowalne fizycznie. JednakŜe skomplikowana postać tych rozwiązań sprawia duŜe

problemy z ich fizyczną interpretacją i co za tym idzie wyborem właściwego rozwiązania.

Wobec braku rozwiązania analitycznego, naleŜy zastosować metody numeryczne,

jednak algorytm powinien dawać wystarczająco dokładne rozwiązanie w minimalnej ilości

iteracji. Do znajdowania rozwiązania zadania prostego kinematyki zastosowano metodę

Newtona-Raphsona [18]. Dla uproszczenia obliczeń, przekształcono równania (6.9) do

postaci:

( )( )

( )( )0

2

13

2

1

2

13

2

1

21

23

21

22

222

222

222

222

=

−−

−

−−+−

−

++

+

−−+−

−

++

−

ll

ll

BRyxBRyRx

BRyxBRyRx

(6.11)

gdzie t=[x y] T. Rozwiązując powyŜsze równanie moŜna numerycznie wyznaczyć wartości

współrzędnych środka platformy x, y. Współrzędną z moŜna następnie wyznaczyć

analitycznie, przekształcając pierwsze równanie układu (6.9) do postaci:

( )( ) 22

2221 ABRyxlz +−−+−= (6.12)

Oznaczając lewą stronę równania (6.11) przez S(t), zgodnie z metodą Newtona-Raphsona

iteracje odbywają się wg zaleŜności:

( ) ( )kkkk tStJtt1

S1 ⋅−= −+ (6.13)


- 68 -

gdzie JS jest jakobianem równania (6.11), a k jest numerem kroku iteracji. Jakobian moŜna

wyznaczyć analitycznie, dzięki czemu unika się błędów numerycznych związanych z

obliczaniem pochodnych. Macierz jakobianu we wzorze (6.13) przyjmuje postać:

=

∂∂

∂∂

∂∂

∂∂

=2221

1211

22

11

SJJJ

JJ

y

s

x

sy

s

x

s

(6.14)

Poszczególne elementy macierzy jakobianu dane są zaleŜnościami:

( )( )( )

( )22

22

22

22

11

2

2

13

2

1

322

13

2

1

Ryx

xBRyx

RyRx

RxBRyRx

J−+

−−+−+

++

−

−

−

++

−

= (6.15)

( )( )( )( )

( )22

22

22

22

12

2

2

13

2

1

22

13

2

1

Ryx

RyBRyx

RyRx

RyBRyRx

J−+

−−−+−

++

−

+

−

++

−

= (6.16)

( )( )( )

( )22

22

22

22

21

2

2

13

2

1

322

13

2

1

Ryx

xBRyx

RyRx

RxBRyRx

J−+

−−+−

++

+

+

−

++

+

= (6.17)

( )( )( )( )

( )22

22

22

22

22

2

2

13

2

1

22

13

2

1

Ryx

RyBRyx

RyRx

RyBRyRx

J−+

−−−+−

++

+

+

−

++

+

= (6.18)

Dla osiągnięcia szybkiej zbieŜności metody Newtona-Raphsona duŜe znaczenie ma

przyjęcie wartości początkowej zmiennej t, jak najbliŜszej rozwiązaniu. MoŜna to osiągnąć


- 69 -

poprzez uproszczenie układu równań (6.9) do postaci (6.10) i rozwiązanie go ze względu na

współrzędne x, y, z. Oczywiście takie rozwiązanie będzie jednoznaczne z zadaniem

kinematyki prostej dla konstrukcji robota [71]. Przyjmując więc załoŜenie upraszczające, Ŝe

0=B oraz stosując dodatkowe podstawienie 222 All ini += , układ równań (6.9) przyjmuje

postać :

( )

222

3

222

2

2221

2

13

2

1

2

13

2

1

zRyRxl

zRyRxl

zRyxl

n

n

n

+

++

+=

+

++

−=

+−+=

(6.19)

Przekształcając pierwsze równanie układu równań z otrzymano wzór na parametr:

( )2221

2 Ryxlz n −−−= (6.20)

Po podstawieniu z do pozostałych dwóch równań układu (6.19) otrzymuje się:

( )

( )2221

22

3

2221

22

2

2

13

2

1

2

13

2

1

RyxlRyRxl

RyxlRyRxl

nn

nn

−−−+

++

+=

−−−+

++

−= (6.21)

Rozwiązanie powyŜszego układu równań ze względu na niewiadome x i y przyjmuje postać:

( )

R

llly

R

llx

nnn

nn

6

2

6

3

22

23

21

23

22

−−−=

−−= (6.22)

Wykorzystując powyŜsze zaleŜności otrzymano zaleŜność pozwalającą na wyznaczenie

zmiennej z:


- 70 -

( ) 222

23

21

2

223

222

1 6

2

12

+−−−−−= R

R

lll

R

lllz nnnnnn (6.23)

Równania (6.22) i (6.23) stosuje się do wyznaczania wartości początkowych w metodzie

Newtona-Raphsona. Są one równieŜ rozwiązaniem problemu kinematyki prostej modelu

robota równoległego [71]. Badania symulacyjne opracowanej metody rozwiązywania

zadania kinematyki prostej wykazały, Ŝe uzyskanie dokładności 1µm wymaga nie więcej niŜ

trzech kroków iteracji.

6.2.3. Pierwsza i druga pochodna równania kinematyki odwrotnej

Projektowanie trajektorii robota moŜe odbywać się zarówno w przestrzeni

współrzędnych złączowych jak i w przestrzeni współrzędnych kartezjańskich związanych z

podstawą. Najczęściej spotykanym rozwiązaniem jest generator trajektorii generujący

przebiegi czasowe w przestrzeni współrzędnych złączowych. Spowodowane jest to tym, iŜ z

reguły układ sterowania steruje wprost napędami znajdującymi się w przegubach robota i

realizacja trajektorii odbywa się poprzez minimalizowanie róŜnic pomiędzy aktualnym

połoŜeniem a pozycją zadaną wybranego przegubu. Istnieją jednak przypadki gdzie

generator trajektorii generuje trajektorie w przestrzeni współrzędnych kartezjańskich.

Występuje to zazwyczaj wtedy, gdy trajektorie są złoŜone lub trajektorie są generowane

przez uniwersalny generator „nieznający” specyfiki konstrukcji robota, gdy istnieje

moŜliwość wystąpienia kolizji oraz gdy trajektorie powstają pod kontrolą operatora, dla

którego przestrzeń kartezjańska jest bardziej czytelna. Bezpośrednie uŜycie takich trajektorii

jest moŜliwe tylko dla układów sterowania dedykowanych do pracy w przestrzeni

współrzędnych kartezjańskich. W przypadku, gdy układ sterowania wymaga połoŜeń w

przestrzeni współrzędnych złączowych, to wówczas naleŜy znaleźć zaleŜności pozwalające

transformować generowane pozycje współrzędnych kartezjańskich na współrzędne

złączowe. Taką zaleŜność przedstawia zadanie odwrotne kinematyki robota. W sytuacji, gdy

generator trajektorii generuje trzy przebiegi zawierające połoŜenia, prędkości oraz

przyspieszenia końcówki robota w przestrzeni współrzędnych kartezjańskich to wówczas

naleŜy określić trzy równania, które pozwolą określić wartości połoŜeń, prędkości i

przyspieszeń w przestrzeni współrzędnych złączowych


- 71 -

W przedstawionym rozwiązaniu robota równoległego zadanie odwrotne kinematyki

wyraŜa się następującymi równaniami:

( )

222

2

222

2

2221

21

23

21

23

zRyRxl

zRyRxl

zRyxl

+

++

+=

+

++

−=

+−+=

(6.24)

RóŜniczkując powyŜsze równania otrzymamy zaleŜności określające prędkości końcówki

robota:

( )( )

2223

2222

2221

21

23

221

223

2

2

1

2

1

2

3

22

12

2

32

21

222

21

zRyRx

vzvRyvRx

v

zRyRx

vzvRyvRx

v

zRyx

vzvRyvxv

zyx

zyx

zyx

+

++

+

⋅⋅+⋅

+⋅+⋅

+⋅

=

+

++

−

⋅⋅+⋅

+⋅+⋅

−⋅

=

+−+

⋅⋅+⋅−⋅+⋅⋅=

(6.25)

RóŜniczkując prędkości otrzymamy przyspieszenia:

( )( )( )( )

( )( ) 222

222

2

3222

2

1

222222

2

1

222

4

1

zRyx

azvaRyvaxv

zRyx

vzvRyvxa

zzyyxx

zyx

+−+

⋅⋅+⋅+⋅−⋅+⋅+⋅⋅+⋅+

+−+

⋅⋅+⋅−⋅+⋅⋅−=

(6.26)


- 72 -

222

222

2

3

222

2

2

2

1

2

3

222

122

2

322

2

1

2

1

2

3

22

12

2

32

4

1

zRyRx

azvaRyvaRxv

zRyRx

vzvRyvRx

a

zzyyxx

zyx

+

++

−

⋅⋅+⋅+⋅

+⋅+⋅+⋅

−⋅+⋅

+

+

++

−

⋅⋅+⋅

+⋅+⋅

−⋅

−=

(6.27)

222

222

2

3

222

2

3

2

1

2

3

222

122

2

322

2

1

2

1

2

3

22

12

2

32

4

1

zRyRx

azvaRyvaRxv

zRyRx

vzvRyvRx

a

zzyyxx

zyx

+

++

+

⋅⋅+⋅+⋅

+⋅+⋅+⋅

+⋅+⋅

+

+

++

+

⋅⋅+⋅

+⋅+⋅

+⋅

−=

(6.28)

PowyŜsze równania zapisano jako funkcję w języku C. Pozwala to na zastosowanie

uzyskanych równań w dowolnych aplikacjach wyposaŜonych w kompilator tego języka.

Funkcję sprawdzono dla generatorów zaimplementowanych w Simulinku, oraz w

algorytmach skompilowanych dla procesora PowerPC (platforma dSpace) i Soft-Procesora

(NIOS 2) zaimplementowanego w układzie FPGA.

Sposób wykorzystania przestawionych równań pokazano przeprowadzając symulację

(rys. 6.2-5). Równania zaimplementowano w S-funkcji o nazwie InverseKJH.


- 73 -

Rys. 6.2-5 Schemat przedstawiający uŜycie generatora pracującego w przestrzeni współrzędnych

kartezjańskich oraz blok zawierający S-funkcję transformującą trajektorię do współrzędnych złączowych.

Jako generator trajektorii w przestrzeni współrzędnych kartezjańskich opracowano

kolejną S-funkcję, która generuje linie prostą równoległą do osi X w przedziale Y podanym

przez uŜytkownika. Przyjęto, Ŝe połoŜenie y zmieniać się będzie w zakresie od -0.2[m] do

0.2[m] na wysokości z=0.4[m] przy prędkości V=0.1[m/s]. Dla tak przyjętych danych

wejściowych trajektorię linii prostej przedstawiono na rys. 6.2-6.

Rys. 6.2-6 Trajektoria linii prostej w przestrzeni współrzędnych kartezjańskich.

Dla powyŜszej trajektorii przebiegi czasowe w przestrzeni współrzędnych kartezjańskich

przedstawiono na rys. 6.2-7, a obliczone przebiegi w przestrzeni współrzędnych złączowych

na rys. 6.2-8. Z rys. 6.2-7 i rys. 6.2-8 moŜna zauwaŜyć, Ŝe realizacja trajektorii linii prostej o


- 74 -

stałej prędkości wymaga, aby układ sterowania kontrolował połoŜenia, prędkości i

przyspieszenia napędów według złoŜonych przebiegów, podczas których mogą zmienić się

znacznie wartości przyspieszeń a co za tym idzie wartości sił jakie powinny generować

napędy.

Rys. 6.2-7 Przebiegi czasowe trajektorii w przestrzeni współrzędnych kartezjańskich

Rys. 6.2-8 Przebiegi czasowe trajektorii w przestrzeni współrzędnych złączowych

Z przedstawionej symulacji moŜna wysunąć następujące wnioski. Posiadanie równań

kinematyki odwrotnej w postaci algebraicznej oraz pierwszej i drugiej pochodnej tych

równań, znacznie podnosi komfort projektowania trajektorii realizujących złoŜone zadania.

Przede wszystkim pozwala uŜywać generatory trajektorii, które nie były domyślnie


- 75 -

projektowane dla posiadanego robota. Mogą to być własne proste generatory trajektorii lub

złoŜone, które jednak bazują na prostych funkcjach. Takimi generatorami są na przykład

aplikacje z rodziny CAM (ang. Computer Aided Manufacturing), które między innymi

pozwalają automatycznie generować kod opisujący ruchy narzędzia w procesie

technologicznym. Trajektoria taka, określona jest w przestrzeni współrzędnych

kartezjańskich. W przypadku niedozwolonych ruchów, uŜytkownik wykorzystujący takie

trajektorie moŜe wizualizować ruchy narzędzia oraz wprowadzać modyfikacje. Następnie w

procesie symulacji, korzystając z powyŜszych równań moŜna sprawdzić czy dana trajektoria

nie przekracza parametrów granicznych robota. Parametrami ograniczającymi trajektorie są

przestrzeń robocza, maksymalne prędkości i przyspieszenia. W przypadku generatorów,

które wprost generują trajektorie w przestrzeni współrzędnych złączowych, równieŜ istniej

moŜliwość stawiania takich ograniczeń, ale wygenerowane trajektorie są po pierwsze

nieczytelne, po drugie zawierają złoŜone funkcje, które nie koniecznie dobrze opisują dany

proces. Spowodowane to jest tym, Ŝe dla realizacji podstawowych ruchów takich jak

poruszanie się po liniach prostych, okręgach, czy łukach funkcje je realizujące są zbyt

złoŜone, aby je obliczyć w czasie rzeczywistym. NaleŜy pamiętać przy tym, Ŝe zadaniem

robota jest współpraca z innymi urządzeniami, a wspólną płaszczyzną wymiany informacji

na danym stanowisku są wymiary geometryczne odnoszące się do przestrzeni kartezjańskiej.

6.2.4. ZaleŜność miedzy siłami w napędach a siłą zredukowaną do środka

przegubu głównego w układzie kartezjańskim

W przypadku robotów równoległych korzystne jest określenie zaleŜności pomiędzy

siłami działającymi na końcówkę robota (chwytak) a siłami w napędach. ZaleŜności takie,

moŜna wykorzystać między innymi przy wyznaczeniu sił w napędach, potrzebnych do

skompensowania siły grawitacji działającej na elementy robota lub do określenia wpływu sił

działających na chwytak robota podczas pracy. Wystarczy wówczas zredukować siły, do

przyjętego punktu końcówki robota a wyznaczone zaleŜności podadzą nam wprost wartości

sił, jakie muszą generować napędy, aby daną siłę skompensować. Dla przedstawianego

rozwiązania robota równoległego połoŜenie końcowe jest zaleŜne od wysunięć ramion i

znajduje się na przegubie łączącym ramiona. Siły działające na ten przegub oznaczono jako

Fx, Fy, Fz. PołoŜenie ich w stosunku do charakterystycznych punktów przedstawianego

robota pokazano na rys. 6.2-9.


- 76 -

β1

β2

β3

α1

α2

α3

F1

F3

F2

Fx

Fz

Fy

xy

z

0

p(x,y,z)

l1

l2l3

s1(x,y,z)

s3(x,y,z)

s2(x,y,z)

Rys. 6.2-9 Rozkład sił w napędach w stosunku do sił zredukowanych dla środka przegubu głównego.

Punkty s1, s2, s3 przyłoŜenia sił napędów Fx, Fy, Fz znajdują się w tej samej płaszczyźnie i

leŜą w odległości R od środka układu współrzędnych. Jako Ŝe punkty te znajdują się na tej

samej wysokości, co środek układu współrzędnych oraz tworzą wierzchołki trójkąta

równobocznego to ich współrzędne x ,y, z, są stałe i wynoszą :

[ ]

−−=

−=

=

022

3

022

3

00

3

2

1

RRs

RRs

Rs

(6.29)

Ramiona połączone są wspólnym przegubem o środku p, którego współrzędne x, y, z,

określają końcówkę robota. Długości ramion l1, l2, l3 wynikają z odległości pomiędzy

punktem p oraz odpowiednio punktami s1, s2, s3 będącymi początkami sił F1, F2, F3. Dla tak

przyjętego układu wartości sinusów kątów α1, α2 oraz α3 wyraŜają się wzorami:


- 77 -

33

22

11

)sin(

)sin(

)sin(

l

z

l

z

l

z

=

=

=

α

α

α

(6.30)

a sinusy kątów β1, β 2 oraz β3:

223

3

222

2

221

1

2)sin(

2)sin(

)sin(

zl

Ry

zl

Ry

zl

x

−

+=

−

+=

−=

β

β

β

(6.31)

wartości cosinusów moŜna określić stosując zaleŜność:

2)sin(1)cos( ϕϕ −= (6.32)

co po podstawieniu do (6.31) daje:

3

223

2

3

233

2

222

2

2

222

1

221

2

1

211

1)sin(1)cos(

1)sin(1)cos(

1)sin(1)cos(

l

zl

l

z

l

zl

l

z

l

zl

l

z

−=

−=−=

−=

−=−=

−=

−=−=

αα

αα

αα

(6.33)


- 78 -

223

222

3

2

223

233

222

222

2

2

222

222

221

2221

2

221

211

221)sin(1)cos(

221)sin(1)cos(

1)sin(1)cos(

zl

Ryzl

zl

Ry

zl

Ryzl

zl

Ry

zl

xzl

zl

x

−

+−−=

−

+−=−=

−

+−−=

−

+−=−=

−−−=

−−=−=

ββ

ββ

ββ

(6.34)

Rzuty sił F1, F2, F3 na płaszczyznę XY określają wzory:

)cos(

)cos(

)cos(

333

222

111

ααα

⋅=⋅=⋅=

FF

FF

FF

XY

XY

XY

(6.35)

Wykorzystując powyŜsze zaleŜność, rzuty sił F1, F2, F3 na oś X wyznacza się z równania:

)cos()cos()cos(

)cos()cos()cos(

)sin()cos()sin(

333333

222222

111111

βαββαβ

βαβ

⋅⋅=⋅=⋅⋅−=⋅−=

⋅⋅=⋅=

FFF

FFF

FFF

XYX

XYX

XYX

(6.36)

Analogicznie rzuty sił F1, F2, F3 na oś Y dane są zaleŜnościami:

)sin()cos()sin(

)sin()cos()sin(

)cos()cos()cos(

333333

222222

111111

βαββαβ

βαβ

⋅⋅=⋅=⋅⋅=⋅=

⋅⋅−=⋅−=

FFF

FFF

FFF

XYY

XYY

XYY

(6.37)


- 79 -

Rzuty wszystkich sił na oś X Y Z moŜna zapisać jako:

=+⋅+⋅+⋅=+⋅+⋅+⋅=+⋅+⋅+⋅

0

0

0

332211

332211

332211

zZZZ

yYYY

xXXX

FFwFwFw

FFwFwFw

FFwFwFw

(6.38)

Przekształcając układ równań (6.38) do postaci macierzowej otrzymano:

−=

⋅

z

y

x

ZZZ

YYY

XXX

F

F

F

F

F

F

www

www

www

3

2

1

321

321

321

(6.39)

Co odpowiada zapisowi:

),,( zyxi FFW −=⋅ (6.40)

Przyjmując, Ŝe:

( )

222

23

222

22

22221

22

3

22

3

zR

yxRl

zR

yxRl

zyRxl

+

++

+=

+

++

−=

+−+=

(6.41)

Elementy macierzy W wyraŜone są za pomocą wzorów:

322

3

222

3

3

223

333

222

2

222

2

2

222

222

122

11

221

111

2

32

)cos()cos(

2

32

)cos()cos(

)sin()cos(

l

xR

zl

Ryzl

l

zlw

l

xR

zl

Ryzl

l

zlw

l

x

zl

x

l

zlw

X

X

X

+=

−

+−−⋅

−=⋅=

−−=

−

+−−⋅

−−=⋅−=

=−

⋅−

=⋅=

βα

βα

βα

(6.42)


- 80 -

322

33

223

333

222

22

222

222

122

1

2221

1

221

111

22)sin()cos(

22)sin()cos(

)cos()cos(

l

Ry

zl

Ry

l

zlw

l

Ry

zl

Ry

l

zlw

l

yR

zl

xzl

l

zlw

Y

Y

Y

+=

−

+⋅

−=⋅=

+=

−

+⋅

−=⋅=

−−=−

−−⋅−

=⋅−=

βα

βα

βα

(6.43)

333

222

111

)sin(

)sin(

)sin(

l

zw

l

zw

l

zw

Z

Z

Z

==

==

==

α

α

α

(6.44)

Podstawiając elementy W wyznaczone z zaleŜności (6.42),(6.43), (6.44) do macierzy (6.39)

otrzymano:

−=

⋅

++−

+−

z

y

x

F

F

F

F

F

F

l

z

l

z

l

zl

Ry

l

Ry

l

Ry

l

Rx

l

Rx

l

x

3

2

1

321

221

321

22

2

3

2

3

(6.45)


- 81 -

Przekształcając powyŜsze równanie otrzymamy:

⋅

++−

+−

−=

−

z

y

x

F

F

F

l

z

l

z

l

zl

Ry

l

Ry

l

Ry

l

Rx

l

Rx

l

x

F

F

F

1

321

221

321

3

2

1

22

2

3

2

3

(6.46)

Co odpowiada poszukiwanej zaleŜności:

( )

⋅

⋅⋅⋅−⋅+⋅⋅

⋅−

⋅−

⋅⋅⋅+⋅−⋅⋅−

⋅−

⋅

⋅⋅+⋅−

⋅

=

z

y

x

F

F

F

lzR

Ryxl

Rl

R

lzR

Ryxl

Rl

R

lzR

Ryl

R

F

F

F

333

222

11

3

2

1

9

)333(3

3

1

3

39

)333(3

3

1

3

33

2

3

20

(6.47)

Zapis (6.47) pozwala na określenie sił F1, F2, F3 napędów w zaleŜności od sił Fx, Fy, Fz

działających na środek przegubu łączącego trzy ramiona. ZaleŜność tą zaimplementowano

w układzie sterowania robota równoległego.

6.2.5. Wpływ siły grawitacji na siły generowane w napędach

Analiza budowy robota wskazuje, iŜ w zaleŜności od zmian połoŜenia końcówki

chwytaka, zmieniają się wartości siły, z jakimi muszą działać napędy, Ŝeby skompensować

siłę grawitacji (rys. 6.1-10). ZaleŜność ta jest nieliniowa i wszelkie próby budowania

układów sterowania przeznaczonych dla obiektów liniowych nie dadzą poŜądanych

dokładności. Przykładowo dla algorytmu PID nie będzie moŜliwe znalezienie takich

współczynników wzmocnień, aby zapewnić daną dokładność w całej przestrzeni roboczej

robota. Z kolei znajomość tych zaleŜności znacznie podnosi jakość projektowanego układu

sterowania. Dla ruchów wolnych gdzie dynamika robota nie ma istotnego wpływu na

wartości sił napędów, wpływ sił grawitacji moŜe być na tyle decydujący, Ŝe jego znajomość

wystarczy w pełni do zlinearyzowania układu sterowania.


- 82 -

Rys. 6.2-10 Zmiana wielkości sił w napędach kompensujących wpływ siły grawitacji w zaleŜności od zmiany

pozycji x na wysokości z=0.204m i współrzędnej y=0m.

W przypadku robota równoległego przeznaczonego do podtrzymywania wrzeciona

frezującego, projekt przewidywał rozmieszczenie przeciwwag na ramionach, których

zadaniem było, aby w pobliŜu pozycji, w której robot najczęściej się znajduje wartość sił w

napędach kompensujących siłę grawitacji była bliska zeru. Jeśli załoŜymy, Ŝe prędkości

ruchów podczas frezowania są niewielkie w stosunku do zadań manipulacyjnych, to moŜna

przyjąć, Ŝe w modelu linearyzującym układ sterowania, naleŜałoby jak najdokładniej

określić wpływ siły grawitacji.

W rozdziale 6.2.4 przedstawiono jak zmieniają się wartości sił w napędach w

zaleŜności od siły przyłoŜonej do końcówki chwytaka. Okazuje się, Ŝe jeśli zredukujemy

wpływ działania siły grawitacji działającej na ruchome elementy robota do pozycji środka

przegubu głównego, to określenie wartości sił w napędach będzie stosunkowo łatwe.

Dodatkowo na uwagę zasługuje fakt, Ŝe siła grawitacji działa w kierunku pionowym, co

znacznie upraszcza wyprowadzenie równań.

Rozkład charakterystycznych elementów na ramieniu robota przedstawiono na rys.

6.2-11. Elementy w ramionach moŜna podzielić na dwie grupy. Pierwszą grupę tworzą

elementy wykonujące obrót względem przegubu obrotowego. Są nimi: stojan napędu,


- 83 -

radiator i przeciwwaga. Drugą grupę tworzą elementy ruchome napędu wraz z prowadnicą i

łącznikami na końcach.

Rys. 6.2-11 Rozkład charakterystycznych elementów robota na pojedynczym ramieniu.

Znalezienie wpływu siły grawitacji na ruch manipulatora wiąŜe się z określeniem

środków cięŜkości poszczególnych grup. Oczywiste jest, Ŝe dokładne określenie tych

wartości nie jest moŜliwe ze względu na nieregularny kształt ich brył.

Rys. 6.2-12 Odległości środków cięŜkości elementów ramienia.

Ramię z rys. 6.2-11 moŜna przedstawić schematycznie jak na rys. 6.2-12. Oś części

ruchomej jest oddalona od osi biegnącej przez środek obrotu ramienia o stałą odległość Lo.

Ze względu na nieregularny kształt stojana, radiatora oraz wprowadzenie przeciwwag,


- 84 -

środek masy c grupy elementów wykonujących obrót wokół punkt O przesunięty jest o

odległość rr i ro. Kąt α jest kątem obrotu ramienia wokół punktu O i odpowiada kątom z rys.

6.2-9.

MoŜna, więc wyprowadzić zaleŜności na siłę jaka działa na grupę elementów wykonujących

obrót wokół O. Wynosi ona:

gmF rr ⋅= (6.48)

gdzie:

mr – masa wszystkich elementów tworzących jedną bryłę obracającą się wokół O

g – przyspieszenie ziemskie

Analogicznie siła działająca na cześć ruchomą ramienia wynosi:

gmF nn ⋅= (6.49)

gdzie:

mn – masa wszystkich elementów znajdujących się w grupie wykonującej ruch

wzdłuŜ osi napędu

Dodatkowo naleŜy określić siłę działającą na przegub główny łączący w sobie trzy ramiona

robota:

gmF pp ⋅= (6.50)

Rozkład ramion został przedstawiony na rys. 6.2-9 w rozdziale 6.2.4. Oznacza on, Ŝe

wartości sinusów i cosinusów kątów są określone równaniami (6.30) - (6.34).

ZaleŜności opisujące siły grawitacji moŜna zapisać w postaci:

)sin(

)sin(

)sin(

33

22

11

ααα

⋅=⋅=⋅=

nn

nn

nn

FF

FF

FF

(6.51)

Rozkładając powyŜsze siły na składowe w kierunkach X Y Z otrzymano:


- 85 -

)cos()cos(

)cos()cos(

)sin()cos(

3333

2222

1111

βαβαβα

⋅⋅−=⋅⋅=⋅⋅−=

nXn

nXn

nXn

FF

FF

FF

(6.52)

)sin()cos(

)sin()cos(

)cos()cos(

3333

2222

1111

βαβα

βα

⋅⋅−=⋅⋅−=

⋅⋅=

nYn

nYn

nYn

FF

FF

FF

(6.53)

)sin(

)sin(

)sin(

333

222

111

ααα

⋅−=⋅−=⋅−=

nZn

nZn

nZn

FF

FF

FF

(6.54)

Wysuwająca się ruchoma część napędu (rys. 6.2-11) powoduje, Ŝe w zaleŜności od

połoŜenia ramion siła grawitacji zredukowana do środka przegubu głównego zmienia się

znacznie. Konsekwencją tego jest fakt, Ŝe na przegub robota działają dodatkowe siły

równoległe do osi X Y Z.

Przyjmując, Ŝe:

3

333

3

2

222

2

1

111

1

)sin()cos()2(2

1

)sin()cos()2(2

1

)sin()cos()2(2

1

l

LlLF

F

l

LlLF

F

l

LlLF

F

oPn

l

oPn

l

oPn

l

⋅+⋅⋅−⋅=

⋅+⋅⋅−⋅=

⋅+⋅⋅−⋅=

αα

αα

αα

(6.55)

gdzie:

Lp – długość części ruchomej,

Lo – odległość pomiędzy osiami, jednej przebiegającej przez środek cięŜkości

elementów ruchomych, a osią przechodzącej przez punkt obrotu ramienia.

Rzutując powyŜsze siły na osie X Y Z otrzymano:

)cos()sin(

)cos()sin(

)sin()sin(

3333

2222

1111

βαβαβα

⋅⋅−=⋅⋅=⋅⋅−=

lXl

lXl

lXl

FF

FF

FF

(6.56)


- 86 -

)sin()sin(

)sin()sin(

)cos()sin(

3333

2222

1111

βαβα

βα

⋅⋅−=⋅⋅−=

⋅⋅=

lYl

lYl

lYl

FF

FF

FF

(6.57)

)cos(

)cos(

)cos(

333

222

111

ααα

⋅=⋅=⋅=

lZl

lZl

lZl

FF

FF

FF

(6.58)

W przypadku grupy elementów wykonujących obrót wokół punku O odsunięcie ich środka

cięŜkości od punktu obrotu powoduje zmienne wartości sił zredukowanych do środka

przegubu głównego.

Wartości tych sił wynoszą:

( )

( )

( )3

333

2

222

1

111

)sin()cos(

)sin()cos(

)sin()cos(

l

rrFF

l

rrFF

l

rrFF

orrr

orrr

orrr

αα

αα

αα

⋅+⋅=

⋅+⋅=

⋅+⋅=

(6.59)

Rzutując siły (6.59) kolejno na kierunki X Y Z otrzymano:

)cos()sin(

)cos()sin(

)sin()sin(

3333

2222

1111

βαβαβα

⋅⋅−=⋅⋅=⋅⋅−=

rXr

rXr

rXr

FF

FF

FF

(6.60)

)sin()sin(

)sin()sin(

)cos()sin(

3333

2222

1111

βαβα

βα

⋅⋅−=⋅⋅−=

⋅⋅=

rYr

rYr

rYr

FF

FF

FF

(6.61)

)cos(

)cos(

)cos(

333

222

111

ααα

⋅=⋅=⋅=

rZr

rZr

rZr

FF

FF

FF

(6.62)

Mając rzuty wszystkich sił na osie X Y Z moŜna zapisać układ równań:


- 87 -

+++++++++=++++++++=

++++++++=

PZlZlZlZnZnZnZrZrZrZ

YlYlYlYnYnYnYrYrYrY

XlXlXlXnXnXnXrXrXrX

FFFFFFFFFFF

FFFFFFFFFF

FFFFFFFFFF

321321321

321321321

321321321

( 6.63)

Podstawiając wartości sił oraz sinusy i cosinusy kątów ramion robota otrzymano przy

wykorzystaniu metod algebry komputerowej zaimplementowanej w Maple [41]:

(6.64)

(6.65)


- 88 -

(6.66)

Mając wyznaczone siły zredukowane Fx Fy Fz oraz stosując zaleŜność (6.47) moŜna

wyznaczyć wartości sił w napędach F1 F2 F3 kompensujący wpływ siły grawitacji.

Wyznaczone wartości będą określały, jaką siłą musi zadziałać kaŜdy z napędów, aby

skompensować wpływ siły grawitacji na ruch manipulatora w kaŜdym jego połoŜeniu.

W celu sprawdzenia poprawności powyŜszych równań przeprowadzono

eksperyment, w którym porównano siły obliczone za pomocą wyprowadzonych równań z

siłami określonymi dla wielomasowego modelu w programie VisualNastran (rys. 6.2-13).

Rys. 6.2-13 Wielomasowy model w programie VisualNastran


- 89 -

Na podstawie równań w postaci algebraicznej zbudowano funkcję w języku C, następnie

dołączono ją do interfejsu Simulink w formie S-Funkcji. Blok reprezentujący S-Funkcję

został umieszczony na wspólnym schemacie wraz z blokiem odwołującym się do

wielomasowego modelu zbudowanego w programie VisualNastran (rys. 6.2-14).

Rys. 6.2-14 Schemat w Simulinku porównujący wyniki pomiędzy wielomasowym modelem a modelem w

postaci algebraicznej zapisanym w postaci S-Funkcji.

W całej przestrzeni roboczej robota wybrano 7 punktów, które odpowiadały kolejnym

połoŜeniom w ruchu pionowym i 7 punktów odpowiadających ruchowi poziomemu. W

kaŜdym punkcie zanotowano wartość sił napędów z programu VisualNastran oraz z modelu

algebraicznego, jakie były wymagane, aby skompensować siłę grawitacji działającą na

wszystkie elementy robota. Dane zebrano w tabeli poniŜej (tabela 3):

Pozycja [m]

Model wielomasowy [N]

Model algebraiczny [N]

RóŜnica [N]

l1 l2 l3 F1n F2n F3n F1n F2n F3n F1n-F1a F2n-F2a F3n-F3a

0.38 0.38 0.38 -41.19 -41.19 -41.20 -42.39 -42.39 -42.39 1.2 1.2 1.2 0.40 0.40 0.40 -6.10 -6.10 -6.10 -6.26 -6.26 -6.26 0.2 0.2 0.2 0.42 0.42 0.42 14.95 14.95 14.95 15.26 15.26 15.26 -0.3 -0.3 -0.3 0.44 0.44 0.44 29.42 29.42 29.42 29.97 29.97 29.97 -0.5 -0.5 -0.5 0.46 0.46 0.46 40.06 40.06 40.06 40.73 40.73 40.73 -0.7 -0.7 -0.7 0.48 0.48 0.48 48.21 48.21 48.21 48.94 48.94 48.94 -0.7 -0.7 -0.7 0.50 0.50 0.50 54.63 54.63 54.63 55.39 55.39 55.39 -0.8 -0.8 -0.8

0.50 0.66 0.27 -41.72 73.20 45.82 -40.05 72.37 45.76 -1.7 0.8 0.1 0.45 0.57 0.29 -26.19 29.02 34.38 -25.17 29.18 34.17 -1.0 -0.2 0.2 0.42 0.48 0.34 -5.54 3.68 14.24 -5.27 3.95 14.09 -0.3 -0.3 0.2 0.41 0.41 0.41 5.53 5.53 5.53 5.65 5.65 5.65 -0.1 -0.1 -0.1 0.42 0.34 0.48 -5.54 14.23 3.68 -5.27 14.09 3.95 -0.3 0.1 -0.3 0.45 0.29 0.57 -26.18 34.39 29.02 -25.17 34.17 29.18 -1.0 0.2 -0.2 0.50 0.27 0.66 -41.72 45.82 73.20 -40.05 45.76 72.37 -1.7 0.1 0.8

Tabela 3 Porównanie wartości sił napędów wymaganych do skompensowania siły grawitacji pomiędzy

modelem wielomasowym w aplikacji VisualNastarn a modelem algebraicznym zapisanym w języku C.


- 90 -

Z przeprowadzonej symulacji wynika, Ŝe róŜnica pomiędzy przedstawionymi modelami jest

mniejsza niŜ 2N w połoŜeniach skrajnych i mniej niŜ 1N w połoŜeniach bliskich środka

przyjętego układu współrzędnych. Przeprowadzony dodatkowo eksperyment na

rzeczywistym ramieniu robota określił wartość siły tarcia na poziomie nie większym niŜ 5N.

Znając tą wartość moŜna uznać, Ŝe przedstawiona dokładność wydaje się zadawalająca i jest

uzasadnione wykorzystania powyŜszego modelu w układzie sterowania.

6.2.6. Model dynamiki odwrotnej robota

Znalezienie równań opisujących dynamikę projektowanego robota jest zadaniem

znacznie bardziej skomplikowanym. Robot posiada trzy ramiona, z których kaŜde wykonuje

ruch obrotowy wokół dwóch osi oraz ruch postępowy wynikający z pracy napędów.

Ramiona są połączone wspólnym przegubem, którego konstrukcja determinuje połoŜenie

chwytaka robota. Próba sformułowania równań opisujących dynamikę takiej konstrukcji jest

zadaniem trudnym i często zbytecznym, aby mogła być zastosowane w projekcie. W robocie

o strukturze równoległej występuje wiele elementów, które mają wpływ na jego dynamikę.

Dla kaŜdego elementu naleŜałoby wyznaczyć jego chwilowe połoŜenie i przyspieszenia

związane z jego dynamiką. Poszukiwanie przyspieszeń wiąŜe się z wyznaczeniem

pochodnych funkcji wynikających z przekształceń równań zawartych w zadaniu kinematyki

prostej i odwrotnej. Naturalne jest, więc Ŝe postać takich równań będzie znacznie

rozbudowana. Nawet, jeśli uda się znaleźć wszystkie zaleŜności to pojawi się problem z ich

weryfikacją wynikającą z błędów numerycznych. Celowość w poszukiwaniu dokładnych

równań opisujących dynamikę mogą równieŜ podwaŜyć ograniczenia wynikające z mocy

obliczeniowej systemu realizującego sterowanie, który takie równania będzie

wykorzystywał. Dodatkowo naleŜy zwrócić uwagę na fakt, Ŝe często przy realizowaniu

ruchu robota zakres zmian pozycji wielu elementów jest niewielki. Oznacza to, Ŝe ich

wpływ na dynamikę moŜe być pomijalnie mały. Przykładowo w rozpatrywanym robocie,

symulacje przy pomocy programu VisualNastran pokazywały, Ŝe zakres zmian kątów w

wielu przegubach obrotowych nie przekraczał kilkunastu stopni, a wartości sił potrzebnych

do realizacji ruchu był o rząd mniejsze w stosunku do sił potrzebnych na kompensację siły

grawitacji.

PoniewaŜ równania ruchu konieczne są do syntezy układu sterowania. Autor

zdecydował się na sformułowanie uproszczonych równań, w których pomija się pewne

zjawiska mające mniejszy wpływ na ruch robota. W rozwaŜnym przypadku zaniedbano siły

Coriolisa, siły tarcia i inne nieliniowości


- 91 -

Równanie modelu odwrotnego dynamiki robota wyraŜa się wzorem [10]:

FxGlxM =+ )()( && (6.67)

gdzie:

M – macierz mas zredukowanych

G – wektor zredukowanych sił grawitacji

F – wektor sił wywieranych przez napędy

l=[l 1, l2, l3]T – wektor współrzędnych złączowych

x=[x, y, z]T – wektor współrzędnych w układzie kartezjańskim (x,y,z)

co odpowiada równaniu:

=

+

⋅

3

2

1

3

2

1

3

2

1

3

2

1

)(

)(

)(

)(00

0)(0

00)(

F

F

F

xG

xG

xG

l

l

l

lM

lM

lM

&&

&&

&&

(6.68)

Do wyznaczenia zredukowanych sił grawitacji wykorzystano równania (6.64) -

(6.66) z rozdziału 6.2.5, które podstawione do równań (6.47) pozwolą na wyznaczenie

skłądowych G1, G2 G3.

Pozostaje tylko określić M1(l), M2(l), M3(l) tak, aby funkcje te zawierały masy

elementów robota oraz aby ich postać była na tyle prosta Ŝeby istniała moŜliwość

zastosowania w układzie sterowania pracującego w czasie rzeczywistym. Wśród wszystkich

elementów robota tylko kilka ma decydujący wpływ na masę zredukowaną i dlatego moŜna

napisać Ŝe:

)cos()sin()cos()sin(23

23


23


23

212

131

222

221

3

313

121

223

221

2

313

212

223

222

1

αααα

αααα

αααα

⋅⋅+⋅⋅+⋅−

+⋅−

++=

⋅⋅+⋅⋅+⋅−

+⋅−

++=

⋅⋅+⋅⋅+⋅−

+⋅−

++=

l

J

l

J

zl

J

zl

JmmM

l

J

l

J

zl

J

zl

JmmM

l

J

l

J

zl

J

zl

JmmM

rrwwpn

rrwwpn

rrwwpn

(6.69)

gdzie:

mn – masa przesuwnych części ramienia

mp – masa przegubu głównego wraz elementami dołączonymi

Jw, Jr – momenty bezwładności ramion odczytane za pomocą aplikacji CAD


- 92 -

PowyŜsze funkcje zaimplementowano w języku C i wykorzystano je w modelu dynamiki

odwrotnej robota w wirtualnym prototypie układu sterowania dla modelu wielomasowego

jak i w układzie sterowania pracującego na obiekcie rzeczywistym.

6.3. Układ sterowania projektowanego robota równoległego

Projektowanie układu sterowania dla badanego robota równoległego zrealizowano w

kilku etapach:

• wybór prawa sterowania

• wirtualne prototypowanie sterowania

• symulację sterownika w czasie rzeczywistym

• szybkie prototypowanie

• implementacja na układzie docelowym

Zrealizowanie tych etapów pozwoliło na syntezę sterownika i jego implementację w

układzie FPGA

6.3.1. Algorytm sterowania

Do sterowania połoŜeniem manipulatora przyjęto następujące prawo sterowania:

( ) ( ) ( )xGdeeellMFt

0

IVVPd +

+++= ∫ ττKKK&& (6.70)

gdzie:

Ivp KKK ,, - stałe

..

lle

lle

dv

d

−=

−= (6.71)

dl - pozycja zadana

Strukturę projektowanego sterownika pokazano na rys. 6.3-1.


- 93 -

Rys. 6.3-1 Struktura sterownika

Część linearyzująca M(l), G(x) zawiera równania (6.47), (6.64), (6.65), (6.66), (6.69).

PowyŜszą architekturę przyjęto w procesie wirtualnego prototypowania.

6.3.2. Wirtualne prototypowanie i szybkie prototypowanie

Wstępne współczynniki Kp, KV, KI, dobrano eksperymentalnie w procesie

wirtualnego prototypowania. W tym celu wielomasowy model stworzony w programie

VisualNastran włączono do modelu układu sterowania zamodelowanego w środowisku

Simulink (rys. 6.3-2). Współczynniki dobrano tak, aby uzyskać minimalizację błędu pozycji

realizowanej trajektorii przy uniknięciu efektu przeregulwoania.

Rys. 6.3-2 Schemat uŜyty podczas wirtualnego prototypowania

Znalezienie poprawnych wartości współczynników pozwoliło przejść do fazy

szybkiego prototypowania. W zasadzie istnieje moŜliwość dobierania współczynników

algorytmu sterowania od razu w procesie szybkiego prototypowanie, jednak dla złoŜonych

konstrukcji bezpieczniej jest przeprowadzić eksperyment na wirtualnym obiekcie. Jak

wynika z doświadczeń autora do procesu szybkiego prototypowania naleŜy mieć juŜ gotowy

system zabezpieczeń oraz system kalibracji robota. Brak tych elementów moŜe spowodować

uszkodzenie rzeczywistego obiektu. W badanym przypadku, wirtualne prototypowanie


- 94 -

wykazało nie tylko poprawność uŜytego algorytmu, ale i dało wytyczne, co do systemu

zabezpieczeń.

System zabezpieczeń został napisany w C i zawarty w S-Funkcji (funkcja „Guard”

rys. 6.3-3).

Rys. 6.3-3 Schemat przedstawiający system zabezpieczeń (kolor Ŝółty) połączony z interfejsami

wejścia/wyjścia

Dodatkowo system zabezpieczeń nadzorował pracę algorytmu sterowania i w przypadku

zagroŜenia mógł przejąć nad nim kontrolę (rys. 6.3-4).

Rys. 6.3-4 Schemat przedstawiający układ sterowania połączony do systemu zabezpieczeń (kolor Ŝóty)


- 95 -

W skład systemu zabezpieczeń wchodzą procedury:

• procedura kalibracji – uŜyto sterowanie prędkościowe. Procedura kalibracji

aktywowana w momencie przyciśnięcia przycisku „Kalibracja” na szafie

sterowniczej. System zabezpieczeń poprzez prosty algorytm zawarty w S-Funkcji

wysuwał ze stałą prędkością ramiona aŜ do osiągnięcia pozycji krańcowych.

• procedura płynnego startu napędów – wymagana dla uŜytych napędów liniowych

firmy Linmot. Zastosowane napędy w robocie wymagały, aby w momencie

włączenia, amplituda sił nie przekraczała wartości określonych przez producenta.

Zadaniem systemu zabezpieczeń było redukowanie wartości sił podczas startu,

wyliczonych przez algorytm sterownia

• procedura dojazdu do pozycji bezpiecznej (zerowy wpływ siły grawitacji)– uŜyto

prosty sterownik proporcjonalny. W momencie pojawienia się zbyt duŜych wartości

sił z algorytmu sterowania, lub parametrów pozycji wykraczających poza przestrzeń

roboczą robota, system zabezpieczeń przejmował kontrolę nad robotem.

Wykorzystano dodatkowy prosty algorytm sterowania proporcjonalnego, którego

zadaniem było ustawić ramiona robota w pozycji gdzie wartość siły grawitacji

równowaŜyła się z siłą pochodzącą od przeciwwag umieszczonych na ramionach

robota

• procedura dojazdu do trajektorii – dla uniknięcia skokowej wartości błędu pomiędzy

zakończoną kalibracją a zadawaną trajektorią. UŜycie trzech algorytmów sterowania:

prędkościowy podczas kalibracji, prosty proporcjonalny podczas dojazdów, oraz

główny realizujący trajektorię wymagało zbudowania procedury zapewniającej

płynne przejścia pomiędzy tymi fazami.

• kontrola przestrzeni roboczej – w przypadku przekroczenia zdefiniowanej

przestrzeni roboczej uruchamiana jest procedura dojazdu do pozycji bezpiecznej

• procedura parkowania – procedura pozwala na zatrzymanie robota, bez względu na

zadaną trajektorię

• procedura nadzoru pracy napędów Linmot – procedura obejmowała kontrolę

sygnałów pochodzących od napędów tj. informacje o błędach i ostrzeŜenia

(przekroczenie dopuszczalnej temperatury napędów)

• procedura kontroli zadań operatora – zadaniem tej procedury było odczytanie

poleceń płynących od operatora i aktywowanie kolejno wszystkich procedur


- 96 -

związanych z wykonywanym zadaniem. W skład poleceń wchodziło: start, stop,

kalibracja, potwierdzenie ostatniego błędu.

Pozytywne zweryfikowanie systemu nadzoru w Simulinku pozwoliło zastosować go

w całości wraz z projektowanym sterownikiem. Jako platformę szybkiego prototypowania

wybrano płytę DS1102 firmy dSpace z procesorem sygnałowym PowerPC.

Rys. 6.3-5 Schemat przedstawiający połączenie robota wraz ze sprzętem do szybkiego prototypowania

Schemat połączenia stanowiska ze sprzętem do szybkiego prototypowania przedstawiono na

rys. 6.3-5. Obsługa pracy robota oraz akwizycja danych odbywała się poprzez wirtualny

pulpit zbudowany w programie ControlDesk (rys. 6.3-6).

Rys. 6.3-6 Pulpit programu ControlDesk, pozwalający na kontrolę pracy robota


- 97 -

Rezultat działania prototypowego układu sterowania przedstawiono na rys. 6.3-7.

Rys. 6.3-7 Realizacja trajektorii (po lewej) w procesie wirtualnego prototypowania (Simulation) i podczas

szybkiego prototypowania (Experiment) oraz błąd na kierunku osi Y.

Jak wynika z przedstawionych przebiegów uzyskano zgodność pomiędzy sterownikiem

opracowanym w fazie wirtualnego prototypowania a sterownikiem pracującym z

rzeczywistym obiektem. Oznacza to, Ŝe fazy wirtualnego prototypowania i szybkiego

prototypowania przeprowadzono poprawnie i uzasadnione jest przejście do dalszego etapu

projektowania, jakim jest implementacji algorytmu sterowania w układzie FPGA.

6.3.3. Sprzętowo-programowa implementacja algorytmów sterowania

Podział na części realizowane programowo i sprzętowo ściśle zaleŜy od rodzaju

algorytmu i musi być rozpatrywane indywidualnie dla kaŜdego przypadku. Powszechnym

podejściem jest identyfikacja tych fragmentów algorytmu, które są krytyczne dla

wydajności (tj. wymagają najdłuŜszego czasu przetwarzania) i przyspieszanie ich realizacji

przez implementację sprzętową (akceleratory sprzętowe). Przed kaŜdą implementacją

sprzętową zaleca się określenie moŜliwego do osiągnięcia wzrostu wydajności, czy jest ona

opłacalna i wykonalna. Mogą wystąpić sytuacje, gdy nieznaczna poprawa wydajności

odbywa się za cenę znacznego wzrostu kosztu spowodowanego przeniesieniem funkcji z

taniego oprogramowania na drogą realizację sprzętową [40].

Dobrymi kandydatami do implementacji sprzętowej są te fragmenty algorytmu, które

mogą być realizowane przy pomocy operacji stałoprzecinkowych, szybszych i

wymagających mniejszej ilości zasobów FPGA. Gdy krytyczne części algorytmu, muszą

być realizowane za pomocą arytmetyki zmiennoprzecinkowej, moŜna je przyspieszyć

uŜywając własnych specjalizowanych zmiennoprzecinkowych instrukcji mikroprocesora.


- 98 -

Rys. 6.3-8 Procedura implementacji sprzętowo-programowej algorytmów sterowania w systemie w

programowalnym układzie scalonym (SoPC)

Do implementacji sprzętowej nadają się szczególnie algorytmy o prostej i stałej

strukturze, operujące na duŜych ilościach danych. I przeciwnie, nieregularne algorytmy z


- 99 -

wieloma warunkowymi rozgałęzieniami, których wybór zaleŜy od wartości obliczanych

podczas wykonywania, wymagają z reguły bardzo duŜej ilości zasobów do implementacji

sprzętowej i dlatego nadają się bardziej do realizacji programowej na procesorach ogólnego

przeznaczenia.

W celu usystematyzowania podejścia do problemu i zautomatyzowania części zadań

zaproponowano procedurę implementacji sprzętowo-programowej algorytmów sterowania

w układach FPGA (rys. 6.3-8). Procedurę wykorzystano do implementacji algorytmu

sterowania robota równoległego.

Początkowa specyfikacja algorytmu w postaci schematu Simulinka wymagała kilku

przekształceń. Pierwszą z nich jest dyskretyzacja w czasie, a drugą dyskretyzacja amplitudy

(kwantyzacja). Obie te fazy dokonano na etapie wirtualnego prototypowania, chociaŜ faza

dyskretyzacji amplitudy moŜliwa jest w późniejszym etapie podczas szybkiego

prototypowania. Wydawać by się mogło, Ŝe zastosowanie wyłącznie arytmetyki

stałoprzecinkowej pozwoli na redukcję kosztu realizacji algorytmu i czasu jego

wykonywania. Tym niemniej naleŜy zachować ostroŜność, poniewaŜ reprezentacja

stałoprzecinkowa z trudem radzi sobie z sygnałami o duŜej dynamice, których amplitudy

zmieniają się w zakresie kilku lub więcej rzędów wielkości. NaleŜy ponadto zwrócić uwagę

na wpływ efektów charakterystycznych dla obliczeń stałoprzecinkowych, takich jak

przepełnienie i nasycenie. Dlatego przy dokonywaniu dyskretyzacji amplitudy działanie

algorytmu powinno zostać sprawdzone poprzez symulację. W implementowanym

algorytmie sterowania część odpowiedzialna za model dynamiczny oraz część związana z

kompensacją siły grawitacji nie nadaje się na realizację stałoprzecinkową. Powodem tego

jest znaczna złoŜoność tych obliczeń, dla których trudno jest jednoznacznie stwierdzić ile

naleŜy przydzielić zasobów układu FPGA, aby je zrealizować. Na podstawie

przeprowadzonej analizy dokonano podziału pomiędzy dwoma częściami: stało- i

zmiennoprzecinkową. Część zmiennoprzecinkowa zaimplementowano programowo w

postaci kodu C/C++. Kod ten w fazie wirtualnego prototypowania i szybkiego

prototypowania zawarty był w s-funkcjach. W skład tej części zawarto algorytm obliczeń

związany z linearyzacją modelu oraz systemem zabezpieczeń. Część stałoprzecinkową

moŜna zaimplementować sprzętowo (krytyczne fragmenty) albo programowo (pozostałe

fragmenty). Uznano, Ŝe tylko dwa fragmenty z algorytmu sterowania nadają się do

implementacji sprzętowej: fragment sterowania zawierający algorytm PID oraz zadanie

kinematyki prostej. Na uwagę zasługuje fakt, Ŝe implementację obu tych fragmentów moŜna


- 100 -

zautomatyzować poprzez wykorzystanie opracowanego generatora przedstawionego w

rozdziale 5.

Rezultatem podziału części stałoprzecinkowej i zmiennoprzecinkowej jest opis

algorytmu sterowania w języku C/C++. Kod w języku C/C++ posiada trzy zalety. Po

pierwsze moŜe, poprzez dodanie odpowiednich interfejsów, zostać jądrem s-funkcji

Simulinka, co pozwala powtórnie symulować fragmenty algorytmu tak, jak inne bloki

(transformację do C/C++ moŜna przeprowadzać etapami i łatwo weryfikować jej

poprawność). Po drugie, pewne operacje, takie jak manipulacje na bitach, interfejsy

urządzeń zewnętrznych i skalowanie danych duŜo prościej jest zapisać od razu w języku

C/C++ niŜ składać ze standardowych bloków Simulinka. Po trzecie wreszcie, symulacja

kodu pozwala na określenie krytycznych fragmentów algorytmu, czyli tych, które wymagają

najdłuŜszego czasu obliczeń i powinny być zrealizowane sprzętowo. JeŜeli krytyczne

fragmenty występują w kodzie zmiennoprzecinkowym, to naleŜy przeprowadzić analizę

najczęściej występujących w nich operacji i przeznaczyć je do realizacji sprzętowej za

pomocą własnych instrukcji mikroprocesora Nios II. W rozwaŜanym algorytmie sterowania

cztery operacją arytmetyczne są najczęściej powtarzane. Operacje te to: dodawanie,

mnoŜenie, dzielenie, pierwiastkowanie. Dla szybszej realizacji tych operacji zaprojektowano

cztery niezaleŜne instrukcje procesora.

Kod C/C++ zawierający zarówno operacje stałoprzecinkowe jak i

zmiennoprzecinkowe przenoszony jest automatycznie. W przypadku operacji

stałoprzecinkowych tworzony jest automatycznie, przy uŜyciu biblioteki AR|T Library, ze

schematu Simulinka poprzez opracowany generator przedstawiony w rozdziale 5. Kod

C/C++ części zmiennoprzecinkowej został stworzony ręcznie, ale juŜ od etapu wirtualnego

prototypowania jest niezmienny. Oznacz to, Ŝe nadaję się on do bezpośredniego uŜycia

przez kompilator dla mikroprocesora Nios II. Jedynie moŜe się okazać konieczne

uzupełnienie kodu C++ o operację realizowane przez własne instrukcji mikroprocesora Nios

II, przyspieszające wykonywanie kodu zmiennoprzecinkowego.

Następnym etapem było skompilowanie kod przeznaczonego do implementacji

sprzętowej do języka VHDL za pomocą programu AR|T Builder. W końcowym etapie

połączono własne elementy sprzętowe, w języku VHDL z pozostałą częścią systemu (m.in.

procesorem) w programie SOPC Builder, w wyniku czego otrzymano kompletny opis

warstwy sprzętowej sterownika w języku VHDL oraz pliki konfiguracyjne dla kompilatora

dla Niosa II. Na podstawie tego kodu VHDL, program Quartus wygenerował pliki do

zaprogramowania układu FPGA.


- 101 -

6.3.4. Realizacja obliczeń stałoprzecinkowych

W przedstawionym algorytmie sterowania jak wynika z analiz wykonanych przez

autora, tylko dwa fragmenty nadają się do realizacji stałoprzecinkowej. Pierwszym

fragmentem jest rozwiązanie zadania kinematyki odwrotnej, wymagane przy obliczeniach

na modelu, drugim fragmentem jest algorytm sterowania PID. Schemat stałoprzecinkowy

dla rozwiązania zadania odwrotnego (6.2.1) przedstawia rys. 6.3-9.

Rys. 6.3-9 Stałoprzecinkowa wersja zadania kinematyki odwrotnej

Do obliczeń stałoprzecinkowego pierwiastka opracowano własną funkcję w C++ z

wykorzystaniem biblioteki stałoprzecinkowej bazując na algorytmie całkowitoliczbowym

przedstawionym w pracy [70]. PoniewaŜ opracowane przez autora równanie moŜna było

zaimplementować na róŜnych modelach układu FPGA Altery, to procedurę implementacji

wykonano dla trzech wersji (rys. 6.3-10).


- 102 -

Rys. 6.3-10 Układy FPGA, na których testowano poprawność implementacji (Stratix/Apex/Flex10)

Ilość wymaganych zasobów do rozwiązania zadania odwrotnego kinematyki przedstawia

tabela 4.

Family Stratix APEX20KE FLEX10K Device EP1S30F780C7 EP20K100EQC240-2X EPF10K20RC240-4

Maksymalna liczba elementów logicznych

32470 4160 1152

Maksymalna liczba bitów pamięci

2137536 53248 0

Maksymalna liczba bloków DSP

96 0 0

Pierwiastek z 40-bitowj liczby (wynik 20bit)

Stratix APEX20KE FLEX10K

Liczba elem. logicznych 750 749 1997 Liczba elem. pamięci 0 0 0 Liczba bloków DSP 0 0 0

Czas [ns] 114.9 205.9 ---

Zadanie kinematyki prostej bez pierwiasta



Czas [ns] 77.9 125.4 ---

Zadanie kinematyki prostej



Czas [ns] 181.6 --- ---

Tabela 4 Porównanie zasobów implementacji zadani kinematyki odwrotnej w róŜnych wersjach układu FPGA

(Altera). Zapis (---) oznacza, Ŝe nie moŜliwe było określenie czasu ze względu na przekroczenie dopuszczalnej

liczby elementów logicznych.

Procedurę implementacji dla algorytmu PID przeprowadzono analogicznie jak w

rozdziale 5.3. Stałoprzecinkową wersję w Simulinku przedstawiono na rys. 6.3-11.


- 103 -

Rys. 6.3-11 Stałoprzecinkowy algorytm PID

Przeprowadzono równieŜ weryfikację poprawności zamiany zmiennoprzecinkowego

algorytmu PID na wersję stałoprzecinkową oraz sprawdzono poprawność działania

algorytmu zapisanego w C++ według rys. 6.3-12.

Rys. 6.3-12 Schemat porównania algorytmu PID podczas kolejnych etapów przekształcania

Zmiennoprzecinkowy algorytm sterowania z ciągłym czasem próbkowania

oznaczono kolorem Ŝółtym, kolorem róŜowym oznaczono zmiennoprzecinkowy algorytm

po dyskretyzacji czasu. Kolor niebieski reprezentuje realizację algorytmu uŜywającego

operacji stałoprzecinkowych a kolor czerwony reprezentuje algorytm zapisany w języku

C++ wygenerowany automatycznie za pomocą generatora opracowanego w punkcie 5 (rys.

6.3-13).


- 104 -

Rys. 6.3-13 Rezultat działania algorytmu PID w wersji zmiennoprzecinkowej i stałoprzecinkowej. Po prawej

stronie przedstawiono powiększony fragment wykresu.

Przedstawiona weryfikacja potwierdziła poprawność przeprowadzonej procedury

dyskretyzacja czasu i amplitudy. Jedyne róŜnice, jakie są widoczne wynikają z przyjętej

dokładności typu stałoprzecinkowego.

W docelowym rozwiązaniu wybrano układ FPGA firmy Altera z rodziny Stratix

(EP1S30F780C7), jako spełniający wymagania sformułowane przy definiowaniu zadań dla

robota równoległego.

6.3.5. Realizacja obliczeń zmiennoprzecinkowych

W części obliczeń zmiennoprzecinkowych zawarto model linearyzujący oparty na

wzorach (6.47), (6.64), (6.65), (6.66), (6.69). Ich postać jednoznacznie wskazuje, Ŝe do ich

obliczeń wymagane są operacje takie jak: dodawanie, odejmowanie, mnoŜenie, dzielenie,

pierwiastkowanie. Okazuje się, Ŝe nie wszystkie z operacji matematycznych naleŜy

zaimplementować. Operację odejmowania moŜna dokonać poprzez dodawanie liczby

przeciwnej do danej, a operację dzielenia moŜna wykonać poprzez mnoŜenie liczby

odwrotnej:

)/1(*/

*)1(

baba

baba

=−+=−

(6.72)

Przy odpowiednim dobraniu kolejności obliczeń, czas potrzebny na ich realizację będzie

taki sam, jak w przypadku wykonywania osobnej operacji odejmowania i pełnego dzielenia.

Ze względu na przyjętą realizację obliczeń zmiennoprzecinkowych zaprojektowano

cztery własne instrukcje:

• MnoŜenie


- 105 -

• Dodawanie

• Odwrotność

• Pierwiastek

Projektowanie takich operacji wymaga znajomości zapisu liczb zmiennoprzecinkowych. W

przyjętym standardzie [17] liczba zmiennoprzecinkowa reprezentowana jest w postać ba 2⋅

gdzie a jest liczbą z przedziału <1,2), a wartość b zapisana jest liczbą całkowitą zapisaną w

kodzie U2.

Oznacz to Ŝe:

222

21

2

1

2)(22

bb

b

b

dbdb

aa

aa

caca

⋅=⋅

⋅=⋅

⋅×=⋅×⋅

−

+

(6.73)

W przypadku pierwiastkowania wystarczy dodatkowo sprawdzić czy wykładnik b jest liczbą

nieparzystą. JeŜeli tak to naleŜy dokonać korekcji w podstawie a poprzez mnoŜenie jej przez

2 , gdyŜ nie moŜliwe jest przedstawienie b w postaci ułamkowej. Proces pierwiastkowania

podstawy a jest identyczny z pierwiastkowaniem stałoprzecinkowym przedstawionym w

poprzednim rozdziale.

Najbardziej czasochłonną operacją jest operacja dodawania. Wynika to z faktu, Ŝe

przy dodawaniu liczb o róŜnych wykładnikach naleŜy odpowiednio zinterpretować bity w

podstawach dodawanych liczb. Dlatego uzasadnione jest wyeliminowanie odejmowania na

rzecz dodawania liczby przeciwnej. W efekcie czego zaoszczędzono miejsce w układzie

FPGA.

Własne instrukcje dołącza się poprzez moduł SOPC Builder (rys. 6.3-14). Przy

tworzeniu instrukcji waŜne jest, aby te instrukcje posiadały odpowiedni interfejs. Interfejs

oparty jest na zmiennych wejściowych i wyjściowych z funkcji instrukcji. Dla instrukcji

jedno-cyklowych (mnoŜenie) wymagane jest tylko pole dataa i datab oraz result. Operacje

wielo-cyklowe (dodawanie, odwrotność, pierwiastkowanie) wymagają dodatkowych

sygnałów kontrolnych tj clk,clk_en reset, start, done (rys. 6.3-15).


- 106 -

Rys. 6.3-14 Opracowane instrukcje procesora dołączone w module SOPC Builder

Rys. 6.3-15 Interfejs instrukcji jedno-cyklowej (po lewej) i wielo-cyklowej (po prawej)

W kolejnym etapie zapisano wszystkie zmiennoprzecinkowe operacje za pomocą

makroinstrukcji. PoniŜej przedstawiono zaproponowane instrukcje dla procesora NIOS.

Mają one następującą postać:

#define Mul(A,B) __builtin_custom_fnff(0x00000000,(A),(B))

#define Add(A,B) __builtin_custom_fnff(0x00000001,(A),(B))

#define Sqrt(A) __builtin_custom_fnf(0x000000002,(A))

#define Inv(A) __builtin_custom_fnf(0x00000003,(A))


- 107 -

oraz operacje pochodne:

inline FP Neg(FP a) {return (Mul(-1.0,a));};

inline FP Pow2(FP a) {return (Mul(a,a));};

inline FP Pow3(FP a) {return (Mul(Mul(a,a),a));};

Rozwiązanie zadania kinematyki prostej zapisane za pomocą zdefiniowanych powyŜej

funkcji ma postać: L1_2=Pow2(L1);

L2_2=Pow2(L2);

L3_2=Pow2(L3);

L23=Add(L2_2,Neg(L3_2));

L123=Add(Mul(2.0,L1_2),Neg(Add(L2_2,L3_2)));

x=Mul(L23,(-sqrt_3/(6.0*R)));

y=Mul(L123,(-1.0/(6.0*R)));

x_2=Pow2(x);

yR_2=Pow2(Add(y,-R));

z_2=Add(L1_2,Neg(Add(x_2,yR_2)));

z=Sqrt(z_2);

Wykorzystując powyŜszy zapis utworzono bibliotekę zawierającą wszystkie

równania robota, które bazują na arytmetyce zmiennoprzecinkowej. W skład niej wchodzą

funkcje określające:

• Rozwiązanie zadania kinematyki prostej

• Rozwiązanie zadania kinematyki odwrotnej

• Wpływ siły grawitacji na napędy

• ZaleŜność pomiędzy siłami w napędach a siłą zredukowaną

• Uproszczony model dynamiczny robota równoległego

Bibliotekę tą wykorzystano do implementacji układu sterowania robotem równoległym w

układzie FPGA. Poszczególne funkcje biblioteki realizowane były na procesorze NIOS II.

Projekt skompilowano w środowisku NIOS IDE (rys. 6.3-16). Tak przygotowany program

załadowano do pamięci, z której soft-procesor NIOS czytał program.


- 108 -

Rys. 6.3-16 Okno programu NIOS IDE

6.3.6. Struktura zaprojektowanaego układu FPGA

Na rys. 6.3-17 przedstawiono strukturę zaprojektowanego układu FPGA. Kolorem

białym oznaczone są bloki standardowe dostarczone przez firmę Altera. Bloki oznaczone na

szaro są blokami zaprojektowanymi przez autora i zawierają w sobie operacje realizowane

w postaci sprzętowej.

Rys. 6.3-17 Struktura zaprojektowanego układu FPGA


- 109 -

Soft-procesor NIOS II skonfigurowany w aplikacji Quartus przedstawiono na rys.

6.3-18. Bloki oznaczone kolorem szarym zawierają w sobie sprzętową realizację

rozwiązania zadania kinematyki odwrotnej. Komponenty standardowe dołączone do

magistrali Avalon przedstawiono na rys. 6.3-19.

Rys. 6.3-18 Okno programu Quartus z softprocesorem NIOS II. Bloki kolorem szarym zawierają strukturę

sprzętową realizującą rozwiązanie zadania kinematyki odwrotnej

Rys. 6.3-19 Okno modułu SOPC Builder, zawierającego dołączone komponenty standardowe.


- 110 -

Sprzęt odpowiedzialny za wymianę danych z urządzeniami zewnętrznymi opracowano

niezaleŜnie w sposób uniwersalny tak, aby moŜna było go uŜywać w róŜnych

konfiguracjach. W skład systemu wymiany danych wchodził interfejs do:

• wymiana danych logicznych w postaci 1-bitowej. NajwaŜniejsze z nich to: przycisk

start, przycisk stop, przycisk kalibracja, status napędów, start/stop trajektorii,

zwolnienie hamulców.

• Trzy bloki enkoderów

• Blok obsługujący przetworniki cyfrowo-analogowe

6.3.7. System wymiany danych pomiędzy PC a FPGA

W przedstawianym rozwiązaniu układ Altera Stratix znajdował się na płycie Tsunami

PC-104 (rys. 6.3-20) [84].

Rys. 6.3-20 Altera Stratix Tsunami PC-104

Oprócz samego układu FPGA płyta ta zawiera w sobie przede wszystkim pamięci

dynamiczne, oraz pamięć statyczną, interfejs JTAG [30], mostek do magistrali PCI oraz

elementy dodatkowe wymagane do poprawnej pracy płyty [76]. Jednak najbardziej

interesującym elementem płyty Tsunami PCI-104 jest magistrala PCI, która pozwala

wykorzystać całość jako część większego system w skład którego wchodzi projektowany

układ. Magistralę PCI moŜna wykorzystać do zasilenia płyty Tsunami, do programowania


- 111 -

układu FPGA (alternatywa w stosunku do programowania JTAG), oraz co najwaŜniejsze do

wymiany danych pomiędzy układem FPGA i systemem nadrzędnym. Jako system

nadrzędny wykorzystano komputer PC (rys. 6.3-21).

Rys. 6.3-21 Widok przedstawiający komputer PC (u góry) połączony z płytą Tsunami PC-104 (na dole)

Następnym etapem realizacji sterowania było stworzenie odpowiedniego oprogramowania

ułatwiającego kontrolę układu FPGA z poziomu PC. Dostępne na rynku programy

pozwalają na zdalną pracę na dowolnym komputerze PC poprzez transmisję stanu pulpitu

systemu operacyjnego. Pozostało opracować własny program, który pod kontrolą danego

systemu operacyjnego, będzie mógł kontrolować układ FPGA. W połączeniu ze zdalnym

pulpitem uzyskano zdalną kontrolę układu FPGA. Jako system operacyjny wybrano

Windows XP ze względu na dostępność sterowników mostka PCI dla tego systemu.

Rys. 6.3-22 Okno programu MemWave przeznaczonego do kontroli układu FPGA.

PowyŜej (rys. 6.3-22) przedstawiono okno programu opracowanego przez autora (o

nazwie MemWave), przeznaczonego do kontroli układu FPGA z poziomu PC znajdującego

się na płycie Tsunami PC-104 poprzez mostek PCI. Program wyposaŜono w interfejs


- 112 -

graficzny w skład, którego wchodzą cztery pola. Pole pierwsze „FPGA RBF” pozwala na

załadowanie struktury połączeń do układu FPGA na podstawie informacji zawartych w

pliku z rozszerzeniem RBF. Plik z rozszerzeniem RBF jest rezultatem kompilacji danej

struktury i jest jednym z trzech plików wynikowych wygenerowanych przez program

Quartus. Pozostałe to pliki z rozszerzeniem SOF i POF przeznaczone do programowania

FPGA za pomocą interfejsu JTAG lub jako obraz struktury FPGA dla pamięci FLASH [24].

Opracowany program wyposaŜono w moŜliwość załadowania domyślnego pliku RBF

podanego przez uŜytkownika podczas jego uruchomienia. Oznacza to, Ŝe umieszczenie

opracowanego programu w autostarcie systemu, spowoduje, Ŝe po włączeniu komputera PC

zostanie załadowany automatycznie domyślny plik RBF. OdciąŜa to uŜytkownika od

kaŜdorazowego ręcznego ładowania pliku RBF przy starcie komputera.

Drugim polem kontrolnym programu jest „Command”. Znajdujące się w tej sekcji

przyciski pozwalają na przesłanie podstawowych komend kontrolnych. „GetID” pozwala

odczytać nadany numer identyfikacyjny przypisany dla danego pliku RBF. Zadaniem tego

przycisku jest ułatwienie identyfikacji programu zawartego w FPGA. Opcja ta wykorzystuje

moŜliwość nadania unikatowych numerów dla kaŜdego projektowanego algorytmu. Opcje

„Get” i „Put” słuŜą do odczytu i wpisania wartości 8-bitowej pod podany adres. Opcja

„Reset” wysyła sygnał restartu dla wszystkich urządzeń znajdujących się na płycie Tsunami.

Kolejnym i najwaŜniejszym polem jest pole o nazwie „Memory Dump”. Znajdujące

się w tej sekcji opcje pozwalają na dwukierunkowe przesyłanie pakietów danych pomiędzy

pamięciami na płycie Tsunami a PC nawet podczas pracy. Wykorzystano tutaj cechę

architektury płyty, w której pamięci: dynamiczne i statyczne oraz pamięci wewnątrz FPGA

znajdują się na wspólnej magistrali z mostkiem PCI. Oznacza to, Ŝe w dowolnym momencie

moŜemy odczytać lub zapisać zawartość pamięci widzianych przez wirtualny procesor

zawarty w FPGA. Przy zapisie dane zostaną zapisane na dysk twardy komputera PC. Opcja

w drugą stronę pozwala na zapis danych w przestrzeni określonej przez adresy uŜytkownika

na podstawie wskazanego pliku. Dodatkowo zastosowanie kanałów DMA rozszerza

znacznie wydajność i elastyczność w zakresie transmisji. Całość moŜna wykorzystać

miedzy innymi do ładowania zadanych trajektorii oraz do akwizycji rezultatów podczas

sterowania.

Ostatnim polem jest informacja o autorze oraz wersja kompilacji. Dodatkowo na dole

okna umieszczono pasek statusu, w którym to wyświetlane są informacje o stanie

wykonywanych poleceń.


- 113 -

6.3.8. Wyniki implementacji sterownika w układzie FPGA

Tabela 5 przedstawia jak malał czas obliczeń podczas dodawania kolejnych

instrukcji wspomagających operacje zmiennoprzecinkowe.

MnoŜenie Dodawanie Odwrotność Pierwiastek

kwadratowy

Czas

wykonania

(µs)

programowe programowe programowa programowy 2500

programowe programowe programowa sprzętowy 1800

programowe programowe sprzętowa sprzętowy 350

programowe sprzętowe sprzętowa sprzętowy 110

sprzętowe sprzętowe sprzętowa sprzętowy 27

Tabela 5 Czas obliczeń sterowania linearyzującego w FPGA dla róŜnych realizacji operacji

zmiennoprzecinkowych..

Ilość zasobów oraz czas potrzebny do wykonania danej instrukcji przedstawiono w

tabeli poniŜej. Zaznaczyć naleŜy, Ŝe zadanie kinematyki odwrotnej i PID było liczone

równolegle i niezaleŜnie w stosunku do softprocesora NIOS wyposaŜonego w dodatkowe

instrukcje. Uzyskano w ten sposób zrównoleglenie procesów realizowanych przez

sterownik.

LE DSP

Czas

wykonania

(sprzętowo)

Czas

wykonania

(programowo)

MnoŜenie 41 8 18 ns 2.3 µs

Dodawanie 818 0 55 ns 2.7 µs

Odwrotność 1697 0 200 ns 2.8 µs

Pierwiastek

kwadratowy 1149 0 182 ns 12 µs

Kinematyka 1292 78 182 ns 0.18 ms

PID 2150 0 75 ns 0.5 ms

Tabela 6 Wykorzystane zasoby i przyspieszenie obliczeń dla sprzętowej implementacji operacji

zmiennoprzecinkowych, równań kinematyki i PID; LE – liczba elementów logicznych, DSP – liczba bloków

DSP.


- 114 -

Wzrost wydajności obliczeń całego układu sterowania prezentuje tabela 7. NaleŜy

zwrócić uwagę na wymagany czas do realizacji całego układu sterowania. W

projektowanym układzie czas próbkowania był 1kHz. Oznacz to, Ŝe wszystkie obliczenia w

danym kroku sterowania powinny zakończyć się w czasie krótszym niŜ 1ms.

Przeprowadzone doświadczenie pokazuje, Ŝe bez sprzętowego wspomagania obliczeń

uzyskanie takiego czasu jest niemoŜliwe.

Moduł

linearyzujący sterownik

Zasoby

Czas

wykonania LE DSP

Realizacja programowa 2.5 ms 9023 10

Ze sprzętowymi

instrukcjami

zmiennoprzecinkowymi,

kinematyką i PID

27 µs 15170 96

Tabela 7 Czas obliczeń algorytmu realizowanej programowo i za pomocą sprzętowej akceleracji

Tabela 8 przedstawia czas potrzebny do wykonania wszystkich obliczeń związanych

ze sterowaniem na róŜnych procesorach dla jednego kroku sterowania. System bazujący na

procesorze PowerPC wykorzystywany był podczas szybkiego prototypowania i na bazie

tego eksperymentu wykonano obliczenia. Dodatkowo pokazano czas obliczeń na

klasycznym procesorze PC. Tutaj naleŜy dodać jeszcze jedną istotną informację. Do tego

czasu naleŜy dodać czas obsługi interfejsów wejścia/wyjścia. O ile dla układu FPGA

obsługę realizuje się sprzętowo (czas rzędu nano sekund) to dla systemu z procesorem

PowerPC jest juŜ to kilkanaście mikrosekund. Podanie czasu dla procesorów PC jest bardzo

trudne i silnie zaleŜy od uŜytego systemu operacyjnego (dla Windows nie więcej niŜ 1ms).

FPGA Nios II,

55 MHz

PowerPC,

400MHz

Celeron,

400MHz

Czas obliczeń 27 µs 34 µs 34 µs

Tabela 8 Czas obliczeń sterowników w ostatecznej postaci w FPGA i na mikroprocesorach ogólnego

przeznaczenia.


- 115 -

6.3.9. Rezultat działania sterownika

W celu pokazania poprawności działania układu sterowania, przeprowadzono

eksperyment na rzeczywistym obiekcie. Dla uzyskania szerszej informacji o jakości

sterowania zbadano dwie róŜne trajektorie.

Pierwsza z trajektorii zawierała krzywą zamkniętą (rys. 6.3-23).

Rys. 6.3-23 Zadana trajektoria w postaci zamkniętej krzywej w przestrzeni kartezjańskiej

Dla tak zadanej trajektorii wyznaczono zmianę połoŜenia we współrzędnych kartezjańskich,

a następnie określono charakterystyki zmiany długości ramion robota równoległego opisane

poprzez trajektorie we współrzędnych złączowych (rys. 6.3-24).

Rys. 6.3-24 Trajektoria krzywej we współrzędnych kartezjańskich (po lewej) oraz złączowych (po prawej)

Zarejestrowany błąd połoŜenia we współrzędnych złączowych przedstawiono kolorem

zielonym na rys. 6.3-25. Dodatkowo, kolorem niebieskim przedstawiono błąd sterowania

uzyskany podczas wirtualnego prototypowania dla zadanej trajektorii.


- 116 -

Rys. 6.3-25 Błąd połoŜenia we współrzędnych złączowych. Kolorem zielonym pokazano rezultat działania

rzeczywistego sterownika, kolor niebieski przedstawia rezultat działania sterownika podczas wirtualnego

prototypowania.

Stosując zadanie kinematyki prostej moŜna było wykreślić błąd połoŜenia określony we

współrzędnych kartezjańskich (rys. 6.3-26).

Rys. 6.3-26 Błąd połoŜenia we współrzędnych kartezjańskich. Kolorem zielonym pokazano rezultat działania


prototypowania.


- 117 -

Uzyskany rezultat potwierdza zgodność zaimplementowanego algorytmu sterowania z

algorytmem przygotowanym podczas wirtualnego prototypowania. Oscylacje wartości błędu

wskazują wpływ na dokładność zjawisk niewystępujących w modelu wielomasowym.

Zjawiskami tymi mogą być tarcie lub dodatkowe nieliniowości w modelu dynamicznym

wynikające ze specyfiki konstrukcji.

W celu wskazania dodatkowych przyczyn uzyskanej wartości błędu,

przeprowadzono drugi pomiar, w którym zadano podobną trajektorię, ale przy znacznie

mniejszej prędkości. Trajektorię przedstawiono na rys. 6.3-27.

Rys. 6.3-27 Trajektoria zadana dla wolnego ruchu

Trajektorię we współrzędnych kartezjańskich i odpowiadającą jej trajektorię we

współrzędnych złączowych przedstawiono na rys. 6.3-28. NaleŜy zwrócić uwagę na

znacznie dłuŜszy czas realizacji trajektorii wynikający z mniejszej prędkości ruchu.

Rys. 6.3-28 Trajektoria krzywej we współrzędnych kartezjańskich (po lewej) oraz złączowych (po prawej)


- 118 -

Wykres zmierzonego błędu we współrzędnych złączowych przedstawiono na rysunku

poniŜej.

Rys. 6.3-29 Błąd połoŜenia we współrzędnych złączowych. Kolorem zielonym pokazano rezultat działania


prototypowania.

Wartość błędu określona we współrzędnych kartezjańskich przedstawiono na rys. 6.3-30.

Rys. 6.3-30 Błąd połoŜenia we współrzędnych kartezjańskich. Kolorem zielonym pokazano rezultat działania

rzeczywistego sterownika, kolorem niebieski przedstawia rezultat działania sterownika podczas wirtualnego

prototypowania.


- 119 -

Dla obu trajektorii zarejestrowano przebiegi zawierające zmianę wartości sił generowanych

przez napędy (rys. 6.3-31).

Rys. 6.3-31 Zmiana wartości sił generowanych przez napędy podczas realizacji trajektorii szybkiej (po lewej)

oraz wolnej (po prawej). Kolorem zielonym pokazano wartości zmierzone na rzeczywistym obiekcie, kolor

niebieski pokazuje wartości zarejestrowane podczas wirtualnego prototypowania.

Z przedstawionych pomiarów moŜna wysunąć następujące wnioski. Procedurę

implementacji sterownika wykonano poprawnie, gdyŜ uzyskano identyczne rezultaty

pomiędzy algorytmem sterowania zbudowanym podczas wirtualnego prototypowania a

algorytmem zaimplementowanym. Jedyne róŜnice, jakie zaobserwowano występują w

przebiegach wartości błędu. Jest to dopuszczalne gdyŜ wiadomo, Ŝe model wielomasowy,

dla którego wstępnie projektowano sterownik, nie zawiera wszystkich cech obiektu

rzeczywistego. Z kolei róŜnica pomiędzy maksymalnym błędem w ruchu wolnym i szybkim

wskazuje na konieczność wykonania dokładniejszego opisu modelu dynamicznego.

JednakŜe taki dokładniejszy opis wymagany jest tylko w przypadku zastosowania robota

równoległego do realizacji ruchów szybkich gdzie dokładność realizacji trajektorii jest

sprawą istotną. Z kolei, w przypadku ruchu wolnego wykres błędu pokazuje, Ŝe uzyskanie

dokładniejszych rezultatów moŜe być niemoŜliwe, ze wglądu na zastosowany tor

pomiarowy. Wartość występujących w nim zakłóceń wskazuje na potrzebę jego zastąpienia

innym bardziej precyzyjnym. Niewielka róŜnica pomiędzy maksymalnymi wartościami sił

generowanymi przez napędy dla ruchu wolnego i szybkiego oznacza, iŜ znaczna część

wartości tych sił skierowana jest na kompensację siły grawitacji, która to nie zaleŜy od

szybkości realizacji trajektorii. Potwierdza to poprawność załoŜenia autora, które zakłada,

Ŝe większe znaczenie ma dokładne określenie wpływu siły grawitacji na elementy robota

równoległego, niŜ znajomość sił wynikających z jego dynamiki.


- 120 -

7. Wnioski

Cel pracy zrealizowano dokonując implementacji układu sterowania dla robota

równoległego typu tripod oraz weryfikując poprawność jego działania. W trakcie badań

zostały stworzone narzędzia ułatwiające proces implementacji i weryfikacji układu

sterowania. Opracowano algorytm pozwalający budować architekturę sprzętowo-

programową realizowaną przez układ FPGA. Wyprowadzono równania opisujące zadanie

proste i odwrotne kinematyki przedstawianego robota równoległego oraz wyprowadzono

równania ułatwiające zbudowanie modelu dynamicznego. Zbudowano system nadzoru i

kalibracji ruchu robota, opisany w języku C i sprawdzony na rzeczywistym obiekcie.

Wszystkie równania zapisano w języku C, tworząc dwie biblioteki, dla robota

przeznaczonego do paletyzacji oraz dla robota przeznaczonego do frezowania. KaŜda z faz

realizacji celu została w pełni osiągnięta pokazując skuteczność obranej metody oraz przede

wszystkim dając gotowe rozwiązania moŜliwe do bezpośredniego wykorzystania przez inne

zespoły pracujące nad podobnymi zagadnieniami. Praca pokazuje zalety zastosowania

podejścia mechatronicznego w projektowaniu robotów równoległych, do których naleŜą:

znaczne skrócenie czasu projektu, uproszczenie i przyspieszenie implementacji, moŜliwość

elastycznej realizacji poszczególnych funkcji systemu.

W procesie wirtualnego prototypowania pokazano jak wirtualny obiekt zachowuje się

z projektowanym sterownikiem. Następnie przeprowadzono proces szybkiego

prototypowania na rzeczywistym robocie, przedstawiono wytyczne, co do systemu

sterowania pracy robota równoległego. Przedstawiono równieŜ rezultaty implementacji

algorytmu sterowania do układu FPGA.

Platforma sprzętowa do implementacji jest oparta na nowoczesnym układzie FPGA.

Algorytm sterowania został zaimplementowany w sposób sprzętowo-programowy, tzn.

pewna część obliczeń została zrealizowana sprzętowo, a pozostała część za pomocą

programu wykonywanego przez mikroprocesor RISC Nios II wprogramowany w układ

FPGA. Opracowany sterownik jest przykładem systemu w układzie programowalnym, co

pozwala na wysoki stopień integracji i elastyczności. System moŜe być łatwo

modyfikowany np. w celu optymalizacji dla innego algorytmu sterowania albo rozszerzenia

funkcjonalności, poprzez przeprogramowanie układu FPGA, bez zmian płyty drukowanej.

Zastosowana metodologia pozwoliła na obniŜenie kosztów i skrócenie czasu implementacji.

Opracowane narzędzie informatyczne znacznie usprawnia proces projektowania

sterownika. Pozwala zarówno na symulację części sprzętowej jak i na automatyczną


- 121 -

implementację algorytmu sterowania w układach FPGA. Przedstawiono jego uŜyteczność

dla prostego obiektu sterowania jak i dla złoŜonej struktury, jaką jest robot równoległy.

Elastyczność przedstawionego rozwiązania pozwala na uŜycie go dla innych

projektowanych układów.

Uzyskane rezultaty sterowania pokazują skuteczność obranej metody oraz mogą

posłuŜyć jako materiał dla prac związanych z tematyką robotów równoległych. Wyniki

kaŜdego z przedstawionych etapów projektowania mogą słuŜyć jako gotowe rozwiązania dla

innych obiektów mechanicznych projektowanych z wykorzystaniem podejścia

mechatronicznego, których złoŜoność wymaga zaawansowanych systemów obliczeniowych.


- 122 -

8. Bibliografia

[1] A|RT Builder User’s and Reference Documentation. Danville, Frontier Design Inc. 2000

[2] A|RT Library User’s and Reference Manual. Danville, Frontier Design Inc. 2000

[3] Altera Corporation - http://www.altera.com

[4] ANSI C- ISO 9899:1990

[5] Auslander D.M, Kempf C.J.: Mechatronics: Mechanical System Interfacing. Prentice Hall, 1996, ISBN

013120338X

[6] Autodesk Mechanical Desktop User’s Guide, Autodesk, 2001

[7] Bishop R.S., The mechatronics handbook, CRC press, 2002

[8] Bojko T., Uhl T., Prototypowanie układów sterowania robotów z wykorzystaniem procesorów

sygnałowych, V Krajowa Konferencja Robotyki, Wrocław, 1996

[9] Cohen, D.: On Holy Wars and a Plea for Peace. IEEE Computer Magazine, Oct. 1981, 14, s. 48-54

[10] Craig J.J.: Introduction to Robotics: Mechanics and Control, Addison-Wesley 1989

[11] Dieterle W.: Mechatronic System: Industral Applications and Modern Design Methodologies. Preprints of

the 3rd IFAC Symposium on Mechatronic Systems, Sydney 2003, pp. 1-5

[12] DSP Builder – http://www.altera.com

[13] Dyson G., Darwin among the machines: the evolution of global intelligence, 1997, ISBN 0-201-40649-7

[14] Eclipse - http://www.eclipse.org/

[15] Felis J., Jaworowski H., Cieślik J., Teoria maszyn i mechanizmów. Część I. Analiza mechanizmów. Wyd.

KRiDM AGH, Kraków 2001

[16] Fixed-Point Blockset User’s Guide. Natic, The MathWorks, Inc., 1999

[17] Floating Point – IEEE 754

[18] Fortuna Z., Macukow B., Wąsowski J.: Metody numeryczne. WNT, 1982.

[19] Grębosz J,: Symfonia C++, tom 1, ISBN 83-901689-1-X

[20] Harshama F., Tomizuka M., Fukada T.: Mechatronics – What is it, why, and how? IEEE/ASME

Transactions on Mechatronics, vol. 1, No. 1, 1996, pp. 1-4

[21] Hewit J.R.: Mechatronic design – The key to performance enhancement. Robotics and Autonomous

Systems, vol. 19, 1996, pp. 135-142

[22] http://babbage.cs.qc.edu/courses/cs341/IEEE-754.html

[23] http://en.wikipedia.org/wiki/Byte

[24] Introduction to Quartus, Altera Corporation, http://www.altera.com

[25] ISO/IEC 14882-1998 Information Technology - Programming Languages - C++

[26] Iurascu C.C, Park F.C.: Geometric Algorithms for Kinematic Calibration of Robots Containing Closed

Loops. Journal of Mechanical Design, vol. 125, No. 1, 2003, pp. 23-32

[27] Japan Trade Mark Kohhoku, Class 9, Shou 46-32713, 46-32714, 1971

[28] Japan Trade Registration No. 946594, 1972

[29] Joshi S.A.,Tsai L.-W.: The Kinematics of a Class of 3-DOF, 4-Legged Parallel Manipulators, ASME J.

Mech. Des. Vol. 125, pp.52=60


- 123 -

[30] JTAG - IEEE 1149

[31] Kaczorek T., Teoria sterowania i systemów, PWN, Warszawa, 1996

[32] Karpiel G., Petko M., Przegub obrotowy potrójny, Zgłoszenie patentowe nr P-363254

[33] Karpiel G., Petko M., Trzyramienny manipulator równoległy, Zgłoszenie patentowe nr P-371508

[34] Karpiel G., Petko M., Uhl T., Manipulator równoległy trzyramienny, Zgłoszenie patentowe nr P-363255

[35] Karpiel G., Petko M.; Mechatronika w projektowaniu robotów równoległych, Projektowanie

mechatroniczne: Zagadnienia wybrane, Wydawnictwo Katedry Robotyki i Dynamiki Maszyn, AGH w

Krakowie, Kraków 2003, str. 135-143

[36] Kaynak O.: Recent advances in mechatronics. Robotics and Autonomous Systems, vol. 19, 1996, pp.

113-116

[37] Kod uzupełnienia do dwóch U2 – http://pl.wikipedia.org/wiki/ Kod_uzupe%C5%82nie%C5%84_do_dw

%C3%B3ch

[38] Kohut P., Prototypowanie układów sterowania wizyjnego z wykorzystaniem procesorów sygnałowych,

Praca doktorska, Kraków, 2002

[39] Korendo Z., Zastosowanie sieci neuronowych i regulatorów rozmytych do sterowania układami

mechanicznymi, podejście mechatroniczne, Praca doktorska, Kraków, AGH 1999

[40] M.D. Edwards, J. Forrest and A.E. Whelan, Acceleration of software algorithms using hardware/software

co-design techniques, J. of Systems and Architecture, Vol. 42, 1996/97, pp. 697-707

[41] Maplesoft - http://www.maplesoft.com

[42] Martowicz A., Petko M., Karpiel G., Prusak D.; Generator trajektorii dla manipulatora równoległego

typu tripod w Wojnarowski J, Uhl T (red) Teoria Maszyn i Mechanizmów t.2, str.227-232, Wyd. Inst..

Techol. Eksploatacji, Radom, 2004 ISBN:83-7204-405-8

[43] MATLAB/Simulink - http://www.mathworks.com/products/simulink/

[44] Mentor Graphics - http://www.mentor.com/

[45] Merlet J., Parallel Robots. Kluwer Academic Publishers, Dordrecht 2000.

[46] Meyer-Bease U., Digital Signal Procesing with Field Programmable Gate Arrays, Springer, ISBN 3-540-

41341-3

[47] Mrozek B., Mrozek Z., MATLAB uniwersalne środowisko do obliczeń naukowo-technicznych CCATIE,

Kraków 1995

[48] Nios II Custom Instruction – User Guide, San Jose 2004, http://www.altera.com

[49] Nios II Processor Reference Handbook, San Jose, 2004, http://www.altera.com

[50] Nios II Software Developer’s – Handbook, San Jose, 2005, http://www.altera.com

[51] Omondi A.R.: Computer arithmetic system: algorithms architecture and implementation. Prentice Hall,

1994, ISBN 0-13-334301-4

[52] Oppenheim A., Schafer R.: Cyfrowe przetwarzanie sygnałów. WKiŁ, 1979

[53] Pelz G., Mechatronic Systems, Modelling and Simulation with HDLs, Wiley 2003, ISBN 0-470-84979-7

[54] Petko M., Implementacja algorytmów sterowania w układach ASIC/FPGA. W: Uhl T. (red.),

Projektowanie mechatroniczne, zagadnienia wybrane, Kraków, Wyd. KRiDM AGH 2002, ISDN 83-

916598-1-X


- 124 -

[55] Petko M., Karpiel G., Prusak D., Martowicz A.; A new 3-DOF parallel manipulator Proceedings of the

fourth international workshop on robot motion and control RoMoCo 2004, Puszczykowo 17-20 Styczeń

2004, str. 127-131

[56] Petko M., Karpiel G., Prusak D., Martowicz A.; Kinematyka nowego typu manipulatora równoległego o

trzech stopniach swobody PAK 5'2004, str. 25-28

[57] Petko M., Karpiel G., Prusak D., Martowicz A.; Nowy manipulator równoległy typu tripod w

Wojnarowski J, Uhl T (red) Teoria Maszyn i Mechanizmów t.2, str.139-144, Wyd. Inst.. Techol.

Eksploatacji, Radom, 2004 ISBN:83-7204-405-8

[58] Petko M., Karpiel G., Prusak D., Uhl T.; Virtual Prototyping of a New Parallel Robot for Milling, 2006

[59] Petko M., Karpiel G., Uhl algorytmów.; Implementacja algorytmów sterowania w układach FPGA na

przykładzie robota równoległego, PAK 5'2006

[60] Petko M., Karpiel G., Uhl T., Prusak D.; Virtual prototyping of parallel manipulators The 9th

Mechatronics Forum International Conference, Ankara Turkey 31 Sierpień - 1 Wrzesień 2004

[61] Petko M., Karpiel G.: Controller for a prismatic robot link with friction – design and implementation. W:

Kaszyński R. (red), Proceedings of the 9th IEEE International Conference on Methods and Models in

Automation and Robotics, vol. 2, Wyd. Uczelniane Pol. Szcz., 2003, pp. 1027-1032, ISBN 83-88764-77-2

[62] Petko M., Karpiel G.; Implementation of Control Algorithms in a System-on-a-Programmable-Chip, 2006

[63] Petko M., Karpiel G.; Mechatronic design of a parallel manipulator for milling, IEEE/ASME

International Conference on Advanced Intelligent Mechatronics, California USA, str. 759-764, 2005

ISBN 0-7803-9047-4

[64] Petko M., Karpiel G.; Mechatronic design of parallel manipulators preprints of the 3rd IFAC Symposium

on Mechatronic Systems, str.433-438, Casual productions, Sydney 2004

[65] Petko M., Karpiel G.; Nowy przegub obrotowy potrójny dla mechanizmów przestrzennych w

Wojnarowski J, Uhl T (red) Teoria Maszyn i Mechanizmów t.2, str.145-150, Wyd. Inst.. Techol.

Eksploatacji, Radom, 2004 ISBN:83-7204-405-8

[66] Petko M., Karpiel G.; Semi-Automatic Implementation of Control Algorithms in ASIC/FPGA, Materiały

konferencyjne ETFA2003, vol.1 str. 427-433

[67] Petko M., Wybrane zagadnienia projektowania mechatronicznego, Rozprawy Monografie, nr 153, Wyd.

AGH, Kraków 2005. ISSN: 0867-6631

[68] Petko M.: Realizacja produktów mechatronicznych W: Uhl T. (red), Projektowanie mechatroniczne,

Kraków, Wyd. KRiDM AGH 1999, ISDN 83-913104-1-8

[69] Pierrot F., Reynaud C., Fournier A., DELTA: A Simple and Efficient Parallel Robot. Robotica, 8, pp.

105-109

[70] Piromsopa K., Aporntewan C., Chongsatitvatana P., An FPGA implementation of a fixed-point square

root operation.

[71] Prusak D., Petko M., Karpiel G., Manipulator trzyramienny o prostym modelu geometrycznym,

Zgłoszenie patentowe nr P-371508

[72] Prusak D., Petko M., Karpiel G., Martowicz A.; Kalibracja robota równoległego typu tripiod w Tchoń K

(red) Postępy Robotyki str. 323-332. Wydawnictwo Komunikacji i Łączności, 2005 ISBN 83-206-1553-4

[73] Quartus II Version 5.1 Handbook – Volume 4: SOPC Builder, San Jose 2005, http://www.altera.com


- 125 -

[74] R. Iserman, Mechatronic Systems: Fundamentals, Springer, 2003

[75] Real-Time Workshop - http://www.mathworks.com

[76] SBS Technologies - http://www.sbs.com

[77] Shetty D., Kolk R.A.: Mechatronic System Design. PWS Publishing Company, 1997, ISBN 0534952852

[78] Smith S., The Scientist and Engineer's Guide to Digital Signal Processing, California Technical

Publishing, San Diego, 1999

[79] Stewart D. , A Platform with Six Degrees of Freedom. Proc. Inst. Mech. Eng., 180 (15), pp. 371-386

[80] System Generator for DSP - http://www.xilinx.com

[81] Tan K.K., Lee T.H., Dou H.F., Lim S.Y.: Various developments in mechatronics in Asia. Mechatronics,

vol. 8, pp. 777-791, 1998

[82] Tomizuka M.: Mechatronics: from the 20th to 21st century. Control Engineering Practice, vol. 10, 2002,

pp. 877-886

[83] Tsai L.-W.: Robot Analysis: the Mechatronics of Serial and Parallel manipulators, John Wiley & Sons

1999

[84] Tsunami PCI-104 FPGA Processor Technical Data Sheet, SBS Technologies 2004, http://www.sbs.com

[85] Tzafestas S.G., Schmidt G. (red): Progress in system and robot analysis and control design. Springer,

1999, ISBN 1852331232

[86] Tzafestas S.G., Schmidt G. [red.]: Progress in system and robot analysis and control design, Springer

1999

[87] Uhl T., Bojko T., Podstawy projektowania mechatronicznego w Wybrane problemy projektowania

mechatronicznego, Wydawnictwo Katedry Robotyki i Dynamiki Maszyn, Kraków, 1999

[88] Uhl T., Komputerowe wspomaganie w projektowaniu mechatronicznym w Projektowanie

Mechatroniczne, Wydawnictwo Katedry Robotyki i Dynamiki Maszyn, Kraków, 2002

[89] Uhl T., Mechatronic system controller design-interdisciplinary approach, International conference,

Mechatronics 2000, September, Warsaw, 2000

[90] Uhl T., Projektowanie mechatroniczne, Wydawnictwo Katedry Robotyki i Dynamiki Maszyn AGH,

Kraków 1999.

[91] Węgrzyn M., Adamski, M.A., Monteiro, J,L,: The application of reconfigurable logic to controller

design. Control Engineering Practice, vol. 6, 1998, 879-887

[92] Xilinx Inc. - http://www.xilinx.com

Grzegorz Karpiel - Praca Doktorska - Biblioteka...

Documents

Transcript of Grzegorz Karpiel - Praca Doktorska - Biblioteka...