Praca dyplomowa magisterska - robotyka.ia.pw.edu.pl · Politechnika Warszawska Rok akademicki 2005/2006 Wydział Elektroniki i Technik Informacyjnych Instytut Automatyki i Informatyki

Politechnika Warszawska Rok akademicki 2005/2006

Wydział Elektroniki i Technik Informacyjnych

Instytut Automatyki i Informatyki Stosowanej

Praca dyplomowa magisterska

Krzysztof Jan Dziubek

Nr albumu 180672

Temat : Sterowanie pozycyjno-siłowe robotami

Subject : Position-force Control of Robots

Kierownik pracy

prof. nzw. dr hab. Cezary Zielinski

Opiekun naukowy pracy

prof. nzw. dr hab. Cezary Zielinski

Abstract

The object of the Master’s thesis is to design a controller for a dual robot system capable

of performing a task of recording and replicating a drawing by two IRp-6 robots located in

the Robotics Laboratory in the Institute of Control and Computation Engineering (ICCE) of

Warsaw University of Technology, and to investigate the operation of this controller.

The original IRp-6 robot is a typical industrial manipulator designed to perform position

control tasks. As a result of employing new axis microcontrollers and a robot programming

frameworkMRROC++(Multi-Robot Research-Oriented Controller design framework in C++),

which was applied to produce the software of the robot controllers, it became possible to realize

the position-force control for the robots. The enhanced control capabilities have allowed the

IRp-6 robot to be used as a service robot. Due to the fact, that service robots operate usually in

unstructured environments, one of the basic factors that provides safe and effective operation

of the robots is the precise control of the force applied to the surrounding objects by the end-

effector of the robot.

The realization of the multi-robot application, including the position-force control, required

equipping the two robots with force-torque sensors in their wrists. It was necessary to design an

electronic interface and to write the software for the for,ce-torque sensor Gamma No FT6284

from ATI, as well as to modify the interface of the force-torque sensor No FT3084 from ATI.

The following main goals of the Master’s thesis have been accomplished:

• implementation of a trajectory generator for a two robot system using two force-torque

sensors, based onMRROC++,

• investigation of position-force control while performing the drawing task,

• implementation of a new force-torque sensor driver inMRROC++,

• implementation of direct and inverse kinematics problem for the IRp-6 robot with 5 de-

grees of freedom and an axially symmetric tool,

• design of a printed circuit board (PCB) to establish the communication between the Gam-

ma force-torque sensor and the PCI Data Acquisition DAQ Board located in the control

computer.

The mounting of the new force-torque sensors in the IRp-6 robots and the implementation of

the position-force control have enhanced the capabilities of the system. The results of the tests

have confirmed the proper operation of the controller and the correctness of the modifications

introduced into theMRROC++system.

The implemented multi-robot application is a significant step towards the design of a con-

troller of a dual robot system solving the Rubik’s cube – another benchmark task on the way to

a full blown service robot.

2

Streszczenie

Celem pracy jest opracowanie sterownika systemu dwurobotowego realizujacego zadanie

uczenia oraz powielania nauczonego rysunku przez dwa roboty IRp-6, znajdujace sie w labora-

torium Robotyki, Instytutu Automatyki i Informatyki Stosowanej, oraz przeprowadzenie badan

działania tego sterownika.

Oryginalny robot IRp-6 jest typowym manipulatorem przemysłowym przystosowanym do

realizacji zadania sterowania pozycyjnego. W wyniku wbudowania nowych mikroprocesoro-

wych sterowników osi oraz wprowadzenia otwartego systemuMRROC++, słuzacego do two-

rzenia programowych sterowników robotów, umozliwiono uruchomienie sterowania pozycyjno–

siłowego robota. Poszerzone mozliwosci pozwalaja na uzycie robota IRp-6 jako robota usługo-

wego. Ze wzgledu na czeste działanie robotów usługowych wsrodowisku nieuporzadkowanym,

jednym z podstawowych czynników zapewnienia bezpieczenstwa i ich efektywnego działania,

jest precyzyjne sterowanie siła wywierana przez koncówke robota.

Realizacja aplikacji wielorobotowej wykorzystujacej sterowanie pozycyjno–siłowe wyma-

gała wyposazenia obu robotów w czujniki sił i momentów sił, które zamontowano w nadgarst-

kach. Konieczne było zaprojektowanie elektronicznego połaczenia nowego czujnika sił Gamma

FT6284 firmy ATI z karta PCI-6034E firmy NI umieszczona w komputerze, opracowanie ste-

rownika tego czujnika oraz modyfikacja połaczenia czujnika FT3084 firmy ATI znajdujacego

sie w laboratorium.

W ramach pracy zrealizowano nastepujace zadania:

• zaimplementowano generator trajektorii dla aplikacji dwurobotowej wykorzystujacej dwa

czujniki siły w srodowiskuMRROC++,

• zbadano mozliwosci działania nowego czujnika,

• zaimplementowano sterownik nowego czujnika siły wMRROC++,

• zaimplementowano proste i odwrotne zagadnienia kinematyki dla robota IRp-6 o 5 stop-

niach swobody, wyposazonego w narzedzie osiowosymetryczne,

• zaprojektowana płytke umozliwiajaca transfer informacji pomiedzy czujnikiem zamon-

towanym na koncówce robota, a karta PCI umieszczona w komputerze.

Zamontowanie nowego czujnika siły na robocie IRp-6 oraz zrealizowanie powyzszych za-

dan znacznie rozszerzyło mozliwosci wykorzystania robota. Przeprowadzone badania potwier-

dziły sprawne działanie zaimplementowanego sterownika czujnika siły FT6284 oraz popraw-

nosc zmian i poprawek wprowadzonych w systemieMRROC++.

Aplikacja wielorobotowa opracowana w ramach pracy, realizujaca zadanie sterowania pozycyjno-

siłowego dwoma robotami, jest istotnym etapem prac prowadzonych w laboratorium Robotyki

IAiIS, zmierzajacych do implementacji zadania układania kostki Rubika przez dwa manipula-

tory.

3

Podziekowania

Składam podziekowania wszystkim pracownikom Instytutu Automatyki i In-formatyki Stosowanej Politechniki Warszawskiej, którzy słuzyli mi rada i po-moca w realizacji niniejszej pracy.

Przede wszystkim słowa podziekowania kieruje do opiekuna i kierownikamojej pracy prof. nzw. Cezarego Zielinskiego, który nadawał własciwy kieru-nek moim działaniom. Bez jego zaangazowania i wsparcia nie dotarłbym dowielu przydatnych informacji i trudno byłoby mi osiagnac wyznaczony cel.

Duza pomoc otrzymałem takze ze strony dr Wojciecha Szynkiewicza, któryudostepnił mi wiele przydatnych publikacji i słuzył swoja rozległa wiedza wdziedzinie kinematyki robotów, a takze ze strony mgr Andrzeja Rydzewskiego,który przekazał swoja wiedze dotyczaca zagadnien zwiazanych z projektowa-niem obwodów drukowanych.

Chciałbym równiez bardzo podziekowac mgr Tomaszowi Winiarskiemu, zktórym mogłem na biezaco konsultowac wszystkie watpliwosci pojawiajace siew trakcie pracy i którego wiedza oraz pomoc były nieocenione przy poznawa-niu, a nastepnie modyfikowaniu systemuMRROC++.

Warszawa, maj 2006 rok

4

SPIS TRESCI

Spis tresci

1 Wstep 8

2 Podstawy sterowania siłowego w robotach 9

2.1 Wprowadzenie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .9

2.2 Zasada działania czujnika siły . . . . . . . . . . . . . . . . . . . . . . . . . .9

2.2.1 Opis mechaniczny . . . . . . . . . . . . . . . . . . . . . . . . . . . .9

2.2.2 Schemat przepływu pomiarów z czujnika siły . . . . . . . . . . . . . .11

2.3 Czujniki siły w manipulatorach . . . . . . . . . . . . . . . . . . . . . . . . . .11

2.4 Pozycyjne i siłowe sterowanie robotami . . . . . . . . . . . . . . . . . . . . .12

2.5 Klasyfikacja sposobów sterowania siła . . . . . . . . . . . . . . . . . . . . . .12

2.5.1 Regulacja pozycyjno-siłowa . . . . . . . . . . . . . . . . . . . . . . .13

3 Opis stanowiska badawczego 16

3.1 Budowa robota IRp6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .16

3.2 Stanowisko z dotychczasowym czujnikiem siły . . . . . . . . . . . . . . . . .19

3.3 Stanowisko do podłaczenia nowego czujnika siły . . . . . . . . . . . . . . . .20

4 System MRROC++ 22

4.1 System robotyczny . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .22

4.2 Struktura sterownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .23

5 Projektowanie płytek drukowanych 30

5.1 Opis narzedzi programowych. Program Protel DXP . . . . . . . . . . . . . . .30

5.2 Nowa płytka dla czujnika siły FT3084 (Schunk F/T 65/5) . . . . . . . . . . . .30

5.3 Płytka umozliwiajaca komunikacje z czujnikiem Gamma FT6284 . . . . . . .33

6 Implementacja oprogramowania czujnika siły w systemie MRROC++ 37

6.1 Czujnik siły FT3084 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .37

6.2 Implementacja sterownika nowego czujnika siły Gamma FT6284 firmy ATI . .37

6.2.1 Parametry karty National Instruments PCI-6034E oraz czujnika siły

Gamma firmy ATI . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

6.2.2 Metody implementacji sterownika czujnika siły w systemie QNX . . .41

6.2.3 Wstepna implementacja sterownika czujnika siły poza systemem

MRROC++ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .42

6.2.4 Implementacja czujnika siły w systemie MRROC++ . . . . . . . . . .46

5

SPIS TRESCI

7 Opis funkcji i kod sterownika nowego czujnika siły Gamma FT6284 firmy ATI 50

7.1 Watekforce_threadrealizujacy odczyt danych z czujnika . . . . . . . . . . . .52

7.2 Klasaedp_ATI6284_force_sensor : public edp_force_sensorrealizujaca ste-

rownik czujnika siły FT6284 . . . . . . . . . . . . . . . . . . . . . . . . . . .53

7.3 Funkcje i metody sterownika czujnika siły Gamma FT6284 . . . . . . . . . . .54

7.4 Stałe modyfikujace działanie programu . . . . . . . . . . . . . . . . . . . . . .74

8 Przygotowania do realizacji aplikacji wielorobotowej 75

8.1 Zmiany konstrukcji robotów . . . . . . . . . . . . . . . . . . . . . . . . . . .75

8.2 Schemat stanowiska badawczego . . . . . . . . . . . . . . . . . . . . . . . . .75

8.3 Zmiany zwiazane z modelem kinematyki robotów . . . . . . . . . . . . . . . .77

8.3.1 Kinematyka robotów . . . . . . . . . . . . . . . . . . . . . . . . . . .78

8.3.2 Kinematyka piecioosiowa . . . . . . . . . . . . . . . . . . . . . . . .78

8.3.3 Kinematyka szescioosiowa . . . . . . . . . . . . . . . . . . . . . . . .80

8.3.4 Narzedzie osiowosymetryczne . . . . . . . . . . . . . . . . . . . . . .82

8.3.5 Kod funkcji realizujacych przeliczanie narzedzia w notacji

TOOL_XYZ_ANGLE_AXISorazTOOL_AS_XYZ_EULER_ZY. . . . . 86

9 Implementacja aplikacji wielorobotowej 91

9.1 Generator trajektorii w systemie MRROC++ . . . . . . . . . . . . . . . . . . .91

9.2 Implementacja prostego generatora trajektorii wykonujacego ruch pozycyjno-

siłowy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .94

9.3 Aplikacja wielorobotowa . . . . . . . . . . . . . . . . . . . . . . . . . . . . .95

9.3.1 Wykorzystywane generatory . . . . . . . . . . . . . . . . . . . . . . .95

9.3.2 Generatory wykorzystywane w aplikacji wielorobotowej . . . . . . . .102

9.3.3 Interfejs uzytkownika . . . . . . . . . . . . . . . . . . . . . . . . . . .104

9.3.4 Działanie aplikacji . . . . . . . . . . . . . . . . . . . . . . . . . . . .105

10 Badania przeprowadzone w oparciu o nowa aplikacje wielorobotowa 107

10.1 Kalibracja robotów . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .107

10.2 Badania poprawnosci działania kinematyk . . . . . . . . . . . . . . . . . . . .110

10.2.1 Kwadrat . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .111

10.2.2 Okregi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .113

10.2.3 Gwiazda . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .115

10.2.4 Dom . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .117

10.2.5 Wnioski . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .118

10.3 Wykresy siły dla trajektorii nauczonej przez operatora . . . . . . . . . . . . . .119

10.3.1 Wnioski . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .122

6

SPIS TRESCI

10.4 Wykresy siły dla trajektorii idealnych, wygenerowanych w programie Matlab .122

10.4.1 Idealna przerywana linia prosta . . . . . . . . . . . . . . . . . . . . .123

10.4.2 Idealne koło . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .126

10.4.3 Wnioski . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .128

10.5 Podsumownie wyników badan . . . . . . . . . . . . . . . . . . . . . . . . . .129

11 Podsumowanie 130

Literatura 132

Spis rysunków 136

Spis tabel 137

Spis zawartego kodu 138

Dodatki 139

7

1 WSTEP

1 Wstep

Celem pracy jest opracowanie sterownika systemu dwurobotowego realizujacego zadanie

uczenia oraz powielania nauczonego rysunku przez dwa roboty IRp-6, znajdujace sie w labora-

torium Robotyki, Instytutu Automatyki i Informatyki Stosowanej, oraz przeprowadzenie badan

jego działania.

Oryginalny robot IRp-6 jest typowym manipulatorem przemysłowym i, ze wzgledu na swo-

je tradycyjne zastosowania, przystosowany jest do realizacji zadania sterowania pozycyjne-

go. W wyniku wbudowania nowych mikroprocesorowych sterowników osi oraz wprowadzenia

otwartego systemu MRROC++, słuzacego do tworzenia programowych sterowników robotów,

mozliwym stało sie uruchomienie sterowania pozycyjno–siłowego robota. Poszerzone mozli-

wosci pozwalaja na próbe uzycia robota IRp-6 jako robota usługowego.

Roboty usługowe, w przeciwienstwie do robotów przemysłowych, działaja wsrodowisku

nieuporzadkowanym, dynamicznie sie zmieniajacym. W przyszłosci, roboty usługowe beda

pracowały w bezposrednim otoczeniu czlowieka. W zwiazku z tym potrzebuja wielu róznorod-

nych czujnków by efektywnie wykonywac swoje zadania. Jednym z podstawowych czynników

zapewnienia bezpieczenstwa i efektywnego działania robota w takimsrodowisku, jest precy-

zyjne sterowanie siła wywierana przez koncówke robota.

Realizacja aplikacji wielorobotowej wykorzystujacej sterowanie pozycyjno–siłowe wyma-

gała wyposazenia obu robotów w czujniki sił i momentów sił, które zamontowano w nadgarst-

kach. Konieczne było zaprojektowanie elektronicznego połaczenia nowego czujnika sił Gamma

FT6284 firmy ATI z karta PCI-6034E firmy NI umieszczona w komputerze, opracowanie ste-

rownika tego czujnika oraz modyfikacja połaczenia czujnika FT3084 firmy ATI znajdujacego

sie w laboratorium.

W ramach pracy zaplanowano realizacje nastepujacych zadan:

• zaimplementowanie generatora trajektorii dla aplikacji dwurobotowej wykorzystujacej

dwa czujniki siły wsrodowisku MRROC++,

• zbadanie mozliwosci działania nowego czujnika,

• zaimplementowanie sterownika nowego czujnika siły w MRROC++,

• zaimplementowanie prostego i odwrotnego zagadnien kinematyki dla robota IRp-6 o 5

stopniach swobody, wyposazonego w narzedzie osiowosymetryczne,

• zaprojektowanie płytki umozliwiajacej transfer informacji pomiedzy czujnikiem zamon-

towanym na koncówce robota, a karta PCI umieszczona w komputerze.

Zamontowanie nowego czujnika siły na robocie IRp-6, znajdujacym sie w laboratorium

Robotyki IAiIS, oraz zrealizowanie powyzszych zadan znacznie rozszerzyło mozliwosci wyko-

rzystania robota.

8

2 PODSTAWY STEROWANIA SIŁOWEGO W ROBOTACH

2 Podstawy sterowania siłowego w robotach

W tym rozdziale zostana opisane zagadnienia sterowania siłowego w zakresie niezbednym

do zrozumienia jego istoty oraz wykorzystania w robotyce.

2.1 Wprowadzenie

Wraz z rozwojem panstw uprzemysłowionych oraz zmianami demograficznymi w nich za-

chodzacymi pojawia sie coraz wieksza potrzeba automatyzacji produkcji oraz wzrasta zapo-

trzebownie na roboty usługowe. Wykorzystanie robotów pozwoli na utrzymanie produkcji na

dotychczasowym poziomie, a nawet jej wzrost, niezaleznie od wystepujacej tendencji do sta-

rzenia sie społeczenstw.

Dotychczas w przemysle wykorzystywano głównie roboty przemysłowe, które pracowały

w srodowisku uporzadkowanym i w zwiazku z tym nie wymagały duzej liczby czujników ani

nadmiernie duzej inteligencji, by efektywnie realizowac swoje zadania. W przeciwienstwie do

nich nowa generacja robotów, tak zwanych robotów usługowych, musi doskonale działac w

otoczeniu człowieka, wsrodowisku, w którym ludziezyja na codzien. Jest to otocznie mało

uporzadkowane, zmieniajace sie dynamicznie. Aby sprostac wymaganiom stawianym robotom

usługowym, musza sie one efektywnie w nim poruszac, co wymaga wykorzystania wielu róz-

norakich czujników. Roboty usługowe powinny równiez posiadac zdolnosc do szybkiego prze-

twarzania informacji.

Głównymi zmysłami wykorzystywanymi przez ludzi do wykonywania zadan manualnych

sa wzrok i dotyk. Jesli roboty usługowe maja efektywnie działac w srodowisku ludzkim, powin-

ny posiadac podobne zmysły. Z tego powodu obecne badania koncentruja sie na wykorzystaniu

czujników wizyjnych – kamer, oraz róznego rodzaju sensorów dotyku, czyli czujników siły.

Ponizsza praca dotyczy sterowania siłowego robotami. W kolejnych podrozdziałach zostana

opisane podstawowe zagadnienia sterowania siłowego robotami.

2.2 Zasada działania czujnika siły

2.2.1 Opis mechaniczny

Zasady oddziaływania sił sformułował Izaak Newton. Trzecia zasada dynamiki mówi o wza-

jemnosci oddziaływan i jest czesto nazywana zasada akcji i reakcji.

Sformułowanie III zasady dynamiki:

Jezeli ciało A działa na ciało B siłaFAB, to ciało B działa na ciało A siłaFBA, o takim

samym kierunku i wartosci jakFAB, ale przeciwnym zwrocie.

Czujnik (inaczej przetwornik) reaguje na przyłozone siły i momenty sił zgodnie z trzecia

zasada dynamiki Newtona. Na rysunku 2.1 pokazane sa kierunki sił – F oraz momentów sił – T

9


w osiach X,Y oraz Z.

Rysunek 2.1: Wektory sił i momentów przyłozonych do czujnika ([25]).

Siła przyłozona do przetwornika napreza trzy symetrycznie umieszczone belki (prety wy-

konane z metalu) zgodnie z prawem Hooke’a:

σ = Eε

gdzie:

σ – naprezenie belki (σ jest proporcjonalne do siły),

E – wartosc bezwzgledna sprezystosci belki,

ε – siła przyłozona do belki.

Przetwornik jest monolityczna struktura. Belki sa wykonane z metalu. To zmniejsza hi-

stereze i zwieksza siłe oraz odpornosc struktury na odkształcenia. Do belek dołaczone sa pół-

przewodniki mierzace naprezenia w belkach. Nazywane sa one tensometrami. Opornosc tenso-

metrów zmienia sie wraz z przykładanym naprezeniem. Wyraza to ponizszy wzór:

∆ R = SaRoε

gdzie:

∆ R – zmiana rezystancji miernika siły,

Sa – współczynnik naprezenia miernika siły,

Ro – rezystancja w stanie bez przyłozonej siły,

ε – siła przyłozona do miernika naprezenia.

10


Urzadzenie elektroniczne dołaczone do czujnika siły mierzy zmiane rezystancji, natomiast

odpowiednie oprogramowanie przetwarza sygnał z czujnika do postaci sił i momentów sił.

2.2.2 Schemat przepływu pomiarów z czujnika siły

Na rysunku 2.2 pokazano schemat połaczenia realizujacego odczyt sygnałów z czujnika i

przekazujacego je do karty, w celu przetworzenia przez odpowiednie oprogramowanie zainsta-

lowane na komputerze.

dane

Karta akwizycji danych odbiera Informację z przetwornika.

Odpowiednie oprogramowanie przekształca otrzymany sygnał do postaci sił i momentów sił

dane

Dane w postaci cyfrowej

zasilanie

komputer

zasilanie

Urządzenie przeprowadzające akwizycję danych

InterfejselektronicznyCzujnik siły

(przetwornik)

Rysunek 2.2: Schemat elektronicznego połaczenia czujnika siły z komputerem.

2.3 Czujniki siły w manipulatorach

Czujniki siły moga byc montowane w manipulatorach w róznych miejscach. Z punktu wi-

dzenia sterowania siłowego najprzydatniejsze sa pomiary sił wywieranych na koncówke mani-

pulatora. Siły oddziaływania moga byc mierzone pomiedzy kolejnymi członami manipulatora,

jednak w takim przypadku zamiast normalnie wykorzystywanych tensometrów, czesto doko-

nuje sie pomiaru posrednio, badajac prad płynacy przez silnik. Tego typu pomiar jest czesto

wykorzystywany do wykrycia przeciazenia silnika.

Czujniki siły montowane w manipulatorach wykorzystywane sa głównie do pomiaru siły

chwytu oraz siły z jaka koncówka manipulatora oddziaływuje na otoczenie. Dzieki temu mozli-

we jest chwytanie delikatnych przedmiotów bez ryzyka ich uszkodzenia. Czujniki siły znajduja

zastosowanie na przykład przy myciu szyb lub pracach polerskich.

11


2.4 Pozycyjne i siłowe sterowanie robotami

Sterowanie pozycyjne wystarcza w przypadku, gdy manipulator słuzy do przemieszcza-

nia obiektów o precyzyjnie znanym połozeniu. Polega ono na minimalizacji róznicy pomiedzy

zadawanym, a aktualnym połozeniem wałów silników, czyli tak zwanym uchybem regulacji.

Połozenie zadane jest okreslane w wyniku rozwiazania odwrotnego zagadnienia kinematyki

(dokładniej opisane w 8.3.1) dla kolejnych połozen koncówki manipulatora na zdyskretyzowa-

nej czasowo trajektorii zadanej. Zadaniem regulatora jest minimalizacja uchybu pojawiajacego

sie przy nadazaniu rzeczywistego połozenia koncówki manipulatora za trajektoria zadana.

W przypadku, gdy na torze ruchu koncówki manipulatora pojawia sie przeszkoda, uchyb

regulacji narasta. Powoduje to zwiekszenie siły z jaka koncówka oddziaływuje na przeszkode.

Wraz ze wzrostem siły przeszkoda lub sam manipulator moze ulec zniszczeniu. Dlatego tez w

manipulatorach montuje sie układy zabezpieczajace, wykrywajace przeciazenia, które wyłacza-

ja silniki w celu unikniecia przeciazenia i spalenia. Tego typu rozwiazanie chroni manipulator,

ale uniemozliwia wykonanie zadania. Sterowanie siłowe umozliwia zrealizowanie zadania na-

wet po natrafieniu na przeszkode. W oparciu o pomiar, ruch manipulatora moze zostac zmody-

fikowany lub siła wywierana na przeszkode moze byc ustawiona na taka, która nie spowoduje

uszkodzenia zarówno obiektu bedacego przeszkoda, jak i manipulatora.

Sterowanie pozycyjne jest czesto wykorzystywane w przypadku, gdy robot porusza sie w

srodowisku bez przeszkód lub gdy połozenie przeszkód jest znane. Sterowanie siłowe umozli-

wia pchanie lub ciagniecie przedmiotów z zadana siła. Połaczenie tych dwóch sposobów stero-

wania umozliwia realizacje wielu zadan, na przykład realizacje zadan rysowania.

2.5 Klasyfikacja sposobów sterowania siła

Sterowanie siłowe moze byc realizowane róznymi metodami. W pracy [15] przedstawiono

nastepujaca klasyfikacje metod i algorytmów sterowania siłowego rozróznianych na podstawie

trzech kryteriów:

1. Ze wzgledu na charakter sterowania w poszczególnych kierunkach:

– metody jednolite, w których sterowanie we wszystkich kierunkach operacyjnych ma jed-

nolity charakter.

– hybrydowe, w których rozróznia sie kierunkisterowania pozycyjnegoi siłowego,

2. Ze wzgledu na sposób sterowania siła:

– impedancyjne, w których utrzymywana jest relacja dynamiczna miedzy uchybem poło-

zenia, a siła,

12


– admitancyjne, w których regulowana jest bezposrednio siła.

3. Ze wzgledu na rodzaj modelu wykorzystywanego do sterowania:

– kinematyczne, w których jest wykorzystywany tylko opis kinematyczny,

– dynamiczne, w których model dynamiki jest wykorzystywany do kompensacji nieliniowej

dynamiki, a prawo sterowania wyrazone jest w przestrzeni zadaniowej.

Niezaleznie od przyjetej klasyfikacji mozna stosowac dodatkowe kryteria podziału metod

np. ze wzgledu naukład współrzednych, w którym jest wyznaczane sterowanie. Regulator siły

i połozenia moze byc realizowany wukładzie współrzednych kartezjanskichlub w układzie

współrzednych wewnetrznych (przegubowych).

Istnieja równiez tzw. zaawansowane metody sterowania siłowego obejmujace:

1. techniki adaptacyjne:

a) posrednie metody adaptacyjne:

– posrednie adaptacyjne sterowanie impedancyjne,

– adaptacyjne jawne sterowanie siłowe,

b) bezposrednie metody adaptacyjne:

– bezposrednie adaptacyjne sterowanie admitancyjne,

– bezposrednie adaptacyjne sterowanie pozycyjno-siłowe.

2. metody odporne (krzepkie) sterowania siłowego,

3. algorytmy uczace sie.

2.5.1 Regulacja pozycyjno-siłowa

W badaniach przeprowadzonych w ramach niniejszej pracy wykorzystano regulacje siłowo–

pozycyjna, zaimplementowana w systemieMRROC++. Ponizej zostana podane informacje na

temat tego sposobu regulacji. Obszerniejsze informacje na temat róznych algorytmów sterowa-

nia siłowego mozna znalezc w pracy [15].

Regulacja pozycyjno–siłowa jest przykładem hybrydowej metody sterowania siłowego. Re-

gulator pozycyjno–siłowy składa sie z dwóch niezaleznych petli sterowania:

– pozycyjnej,

– siłowej.

Kazda z tych petli posiada indywidualne prawo sterowania. Na rysunku 2.3 pokazano schemat

hybrydowego regulatora pozycyjno-siłowego wykorzystywanego w systemieMRROC++.

13


dF

dX

+

pX

+

-+

dθR

obot

FFX

praw

ost

erow

ania

siło

weg

o

S I-S

regu

lato

rypołoże

niaτ

odw

rotn

eza

gadn

ieni

eki

nem

atyk

i

cX

-

pros

teza

gadn

ieni

eki

nem

atyk

i

gene

racj

atra

jekt

orii

isiły

zada

nej

war

iant

1w

aria

nt 2

mak

X

ECP

EDP_

TRA

NS

EDP_

SERV

O

EDP

robo

t icz

ujni

ksił

y

gene

racj

akr

oku

EDP_

FORC

Eko

nwer

sjaod

czyt

usiły

+θ

inte

rpre

tacj

apo

leceń

zEC

P,w

ysył

anie

odpo

wie

dzi d

oEC

P

EDP_

MA

STER

Rys

unek

2.3:

Hyb

rydo

wy

regu

lato

rpo

zycy

jno–

siło

wy

([7]

).

14


W regulatorze pozycyjno–siłowym istnieje podział pomiedzy kierunkami, w których wy-

wierana jest siła, a w których relizowany jest ruch. Podział dokonywany jest we współrzednych

kartezjanskich, a realizowany jest poprzez zdefiniowanie diagonalnej macierzyS, nazywanej

macierza selekcji. MacierzS jest macierza kwadratowa o wymiarzen równym liczbie stopni

swobody manipulatora. Elementy macierzy na diagonalii przyjmuja wartosci 0 lub 1, odpowia-

dajace przynaleznosci do odpowiedniej petli regulacji, siłowej lub pozycyjnej.

Kazda petla sterowania posiada własne prawo sterowania, czesto bazujace na regulatorze

PID. Pozwala to na wyznaczenie przesuniec (kroków):

XP – w petli pozycyjnej,

XF – w petli siłowej (na rys. 2.3 odpowiadageneracji kroku),

XC – suma przesuniec w petli pozycyjnej i siłowej.

Wyznaczone przesuniecieXC jest przekazywane do modułu obliczajacego odwrotne zagadnie-

nie kinematyki. Dane otrzymane w wyniku obliczenia odwrotnego zadania kinematyki przeka-

zywane sa do regulatorów połozenia, które przekształcaja informacje otrzymane z kinematyki

na wymuszenie, które przekazane odpowiednim silnikom powoduje ruch robota (manipulato-

ra).

Regulator pozycyjno–siłowy pozwala na wykonanie wielu zadan, w których kierunki regu-

lacji siłowej i pozycyjnej sa od siebie odseparowane. Przykłady takich zadan podano w rozdzia-

le 2.4.

15

3 OPIS STANOWISKA BADAWCZEGO

3 Opis stanowiska badawczego

W tym rozdziale zostana opisane wyposazenie stanowisk badawczych oraz budowa wyko-

rzystywanych robotów.

3.1 Budowa robota IRp6

Robot IRp-6 jest robotem przemysłowym, stosowanym do automatyzacji prac wykonywa-

nych przez maszyny lub mogacym samodzielnie wykonywac pewne prace przy uzyciu narzedzi

takich jak np.: spawarki łukowe, szlifierki.

W laboratorium Instytutu Automatyki i Informatyki Stosowanej znajduja sie dwa roboty

IRp-6, z których jeden jest robotem umiejscowionym na postumencie, natomiast drugi, zamiast

postumentu (podstawy), posiada dodatkowo tor jezdny. W ponizszej pracy, oba roboty beda po-

siadac te sama konfiguracje mechaniczna, a wiec tor jezdny nie bedzie wykorzystywany.

Robot IRp-6 składa sie z czesci manipulacyjnej i szafy sterowniczej. W skład czesci mani-

pulacyjnej, bez toru, wchodza nastepujace podzespoły (3.1):

1. przegub (kisc),

2. ramie górne (przedramie),

3. ramie dolne (ramie),

4. korpus obrotowy,

5. podstawa,

6. przekładniasrubowa tocznaθ,

7. przekładniasrubowa tocznaα,

8. naped ruchuv,

9. naped ruchut,

10. naped ruchuθ,

11. naped ruchuα,

12. naped ruchuφ (wewnatrz korpusu).

16


Rysunek 3.1: Manipulator IRp-6 w wersji na postumencie.([26])

Manipulator pokazany na rys.3.1 ma piec obrotowych stopni swobody:

• obrót korpusu wzgledem podstawy (φ),

• obrót ramienia (ramienia dolnego) (θ),

• obrót przedramienia (ramienia górnego) (α),

• pochylanie kisci (przegubu) (t),

• skrecanie kisci (przegubu) (v).

Os kazdego stopnia swobody jest napedzana silnikiem elektrycznym pradu stałego z prze-

kładnia i układem przeniesienia napedu. Kazda jednostka napedowa jest wyposazona w ele-

menty pomiarowe predkosci obrotowej (pradnica tachometryczna) i połozenia katowego (rezol-

wer). W szafie sterowniczej znajduja sie układy sterowników mocy, wyposazonych w regula-

tory predkosci typu PID z tachometrycznym sprzezeniem zwrotnym oraz sterowniki połozenia

osi, wykorzystujace sygnały z rezolwerów. Wartosci zadane dla sterowników połozenia sa do-

starczane przez mikroprocesorowy sterownik obsługujacy równiez pozostałe podzespoły szafy

sterowniczej.

Roboty IRp-6 znajdujace sie w laboratorium Instytutu Automatyki i Informatyki Stosowanej

zostały znaczaco zmodyfikowane. Sterowanie oboma manipulatorami odbywa sie przy uzyciu

systemu MRROC++, umozliwiajacego miedzy innymi implementowanie aplikacji wielorobo-

towych oraz sterowanie siłowe robotami.

17


Roboty zostały ostatnio rozbudowane o dodatkowe elementy zwiekszajace ilosc stopni swo-

body z szesciu do siedmiu plus stopien rozwarcia chwytaka w przypadku robota na torze oraz z

pieciu do szesciu plus stopien rozwarcia chwytaka w przypadku robota na postumencie. Ponizej

na rysunku 3.2 przedstawiono schemat robota IRp-6postumentpo modyfikacji, z dołaczonym

nowym członem manipulatora.

V’

h

Rysunek 3.2: Schemat robota IRp-6 z nowym członem.

Manipulator pokazany na rys.3.2 ma szesc obrotowych stopni swobody i dodatkowo mozli-

wosc zwierania i rozwierania szczek:

• obrót korpusu wzgledem podstawy (φ),

• obrót ramienia (ramienia dolnego) (θ),

• obrót przedramienia (ramienia górnego) (α),

• pochylanie kisci (przegubu) (t),

18


• skrecanie kisci (przegubu) (v),

• skrecanie nowego 6 stopnia swobody (v′),

• zaciskanie i rozwieranie szczek (chwytak) (h).

Dokładne schematy pokazujace wpływ zmian mechanicznych na wykorzystywane w niniej-

szej pracy modele kinematyki, wraz z ich opisem, znajduja sie w rozdziale 8.3.

Obecne prace i modyfikacje robotów prowadzone w laboratorium maja w przyszłosci umoz-

liwi c implementacje zadania układania kostki Rubika przez dwa manipulatory IRp-6. Ponizsza

praca dotyczaca miedzy innymi implementacji nowego czujnika siły, niezbednego przy układa-

niu kostki Rubika, jest waznym elementem tych prac.

3.2 Stanowisko z dotychczasowym czujnikiem siły

Stanowisko badawcze w laboratorium Instytutu Automatyki i Informatyki Stosowanej wy-

posazone jest w nastepujace elementy:

sieć ethernetmanipulator

komputer operatora systemu

mikrokomputerowysterownik

manipulatora

czujnik siły

końcówka manipulatora

(kiść)


(kiść)

czujnik siły

komputer sterujący robotem



manipulatora


czujnika siły


sieć ethernet

sprzęg robota

sprzęg czujnika

sprzęg robota

karta wejść - wyjść

przetwornik


Rysunek 3.3: Schemat stanowiska badawczego z poprzednim czujnikiem siły.

• komputer operatora systemu – tutaj za posrednictwem interfejsu uzytkownika (UI i SRP)

oraz posrednio procesu MP operator sprawuje kontrole nad pracujacym systemem

MRROC++ i w konsekwencji manipulatorem. Komputer operatora systemu wysyła, za

posrednictwem sieci ethernet, polecenia do komputera sterujacego robotem,

• komputer sterujacy robotem – uruchomione w nim procesy: EDP, VSP i ECP sprawuja

bezposrednia kontrole nad robotem i czujnikiem. Do komunikacji słuza dwie karty: I_R,

oraz karta 48 we-wy cyfrowych Advantech PCI-1751,

19


• karta I_R (sprzeg robota) – komunikuje sie ze sprzetowym interfejsem manipulatora.

• karta we-wy (sprzeg czujnika) – odbiera dane od sprzetowego interfejsu czujnika.

• port szeregowy – słuzy w pierwszej fazie do wysyłania i odbioru danych z mikrokompu-

terowego sterownika czujnika, a nastepnie jedynie do wysyłania tych danych,

• mikrokomputerowy sterownik manipulatora – z jednej strony ustawia wypełnienie sy-

gnału PWM zasilajacego silniki manipulatora i zbiera odczyty połozenia poszczególnych

stawów, z drugiej komunikuje sie z karta I_R zainstalowana w komputerze PC,

• mikrokomputerowy sterownik czujnika – zbiera dane z czujnika siły i wstepnie je prze-

twarza (m.in. filtruje). Ponadto komunikuje sie z karta we-wy oraz portem szeregowym

komputera sterujacego robotem,

• czujnik siły – urzadzenie słuzace do pomiaru trzech składowych siły i trzech składowych

momentu sił umieszczone w kisci manipulatora,

• manipulator - robot IRp - 6 na torze jezdnym.

3.3 Stanowisko do podłaczenia nowego czujnika siły

Stanowisko badawcze w laboratorium Instytutu Automatyki i Informatyki Stosowanej wy-

posazone zostało w nastepujace elementy:




manipulatora

czujnik siły


(kiść)


(kiść)

czujnik siły




manipulatora


czujnika siły


sieć ethernet

sprzęg robota

sprzęg czujnika

sprzęg robota

karta wejść - wyjść

przetwornik


Rysunek 3.4: Schemat stanowiska badawczego z nowym czujnikiem siły.

• komputer operatora systemu – za posrednictwem interfejsu uzytkownika (UI i SRP) oraz

posrednio procesu MP operator sprawuje kontrole nad pracujacym systemem MRROC++

20


oraz manipulatorem. Komputer operatora systemu wysyła, za posrednictwem sieci ether-

net, polecenia do komputera sterujacego robotem,

• komputer sterujacy robotem – uruchomione w nim procesy: EDP, VSP i ECP sprawuja

bezposrednia kontrole nad robotem i czujnikiem. Do komunikacji słuza karty: I_R oraz

NI-6034E firmy National Instruments,

• karta I_R (sprzeg robota)– komunikuje sie ze sprzetowym interfejsem manipulatora,

• karta we-wy – karta firmy National Instruments, typ NI-6034E, odbierajaca dane od

sprzetowego interfejsu czujnika,

• mikrokomputerowy sterownik manipulatora,

– ustawia wypełnienie sygnału PWM zasilajacego silniki manipulatora i zbiera odczyty

połozenia poszczególnych stawów,

– komunikuje sie z karta I_R zainstalowana w komputerze PC.

• przetwornik – przesyła dane z czujnika siły do karty we-wy komputera sterujacego robo-

tem, zmienia napiecie z 0,+5V (karta) na -15,+15V (czujnik),

• czujnik siły – czujnik Gamma F/T firmy ATI, urzadzenie do pomiaru trzech składowych

siły i trzech składowych momentu sił, umieszczone w kisci manipulatora,

• manipulator – robot IRp-6.

21

4 SYSTEM MRROC++

4 System MRROC++

SystemMRROC++ jest obiektowa biblioteka modułów, zapisana w jezyku C++, wspoma-

gajaca tworzenie sterowników dla systemów wielorobotowych.MRROC++ słuzy do konstru-

owania sterowników dedykowanych konkretnym zadaniom, które maja byc realizowane przez

system wielorobotowy, wyposazony w róznorodne czujniki i urzadzenia współpracujace. Pra-

cuje pod nadzorem systemu operacyjnego czasu rzeczywistegoQNXw wersji 6.3.0.

Struktura i działanie systemuMRROC++zostały szczegółowo opisane w [2] oraz [3]. Poni-

zej przedstawiono informacje uznane za najwazniejsze, niezbedne do zrozumienia opisu prze-

prowadzonych prac oraz odnosniki do bardziej szczegółowych opisów poszczególnych zagad-

nien. Rozdział ten zawiera opis struktury sterownika jakim jest systemMRROC++ oraz po-

szczególnych jego procesów.

4.1 System robotyczny

W systemie robotycznym ([2]) wyrózniamy trzy czesci:

• efektorye – elementy systemu, które oddziałuja na otoczenie, zmieniaja jego stan (np.:

przemieszczaja jakies obiekty); moga to byc roboty, ale równiez np.: podajniki lub obra-

biarki,

• receptoryr – czujniki rzeczywiste (sprzetowe) - zbieraja informacje o stanie otoczenia

(np.: kamery, czujniki zblizeniowe, dalmierze, czujniki sił i momentów sił),

• podsystem sterujacyc – składa sie zarówno ze sprzetu obliczeniowego (komputera lub

sieci komputerów) jaki i oprogramowania.

Stan systemu robotycznego wyraza wzór:

s =< e; r; c > s ∈ S; e ∈ E; r ∈ R; c ∈ C

gdzie:

s – stan systemu S – przestrzen stanów systemu

e – stan efektorów E – przestrzen stanów efektorów

r – stan receptorów R – przestrzen stanów receptorów

c – stan podsystemu sterowaniaC – przestrzen stanów podsystemu sterowania

Dane z czujników rzeczywistych musza zostac przeksztalcone w odczyty czujników wirtu-

alnychv = fv(r, c, e), v ∈ V , gdzieV oznacza przestrzen odczytów czujników wirtualnych. W

22

4 SYSTEM MRROC++

sterowniku MRROC++ podsystem sterujacy odpowiada za obliczenie trajektorii ruchu efekto-

rów (ramion robotów).

Stan całego systemu mozna przedstawic jako:

s =< e1, . . . , ene , v1, . . . , vnv , c >

gdzie:

ne – liczba efektorów (robotów) w systemie,

nv – liczba czujników wirtualnych,

Podsystem sterujacy podzielony został na(ne + 1) czesci:

c =< c0, c1, . . . , cne >

Kazdy z podsystemówcj, j = 1, . . . , ne jest odpowiedzialny za sterowanie pojedynczym

efektorem, zas c0 koordynuje prace wszystkich efektorów (robotów). Kazdemu efektorowi (ro-

botowi) ej, j = 1, . . . , ne przyporzadkowany jest procesECPj (Effector Control Process),

którego stan jest wyrazany przezcj, j = 1, . . . , ne. Procesem koordynujacym jest procesMP

(Master Process), którego stan wyraza sie przezc0.

Kazdy z procesówECP dzieli sie na dwa podprocesy: pierwszy to własciwy ECP, który

odpowiada za realizacje zadania przez efektor, drugi toEDP (Effector Driver Process), który

odpowiada za rozwiazanie prostego i odwrotnego zadania kinematyki.

4.2 Struktura sterownika

SterownikMRROC++ma hierarchiczna strukture funkcjonalna. Poszczególne jego funkcje

realizowane sa przez moduły – odrebne procesy działajace w wezłach sieci lokalnej. Sterownik

MRROC++ jest zaimplementowany w jezyku C++ i uzywa systemu operacyjnegoQNX 6.3.0.

Na rysunku 4.1 przedstawiono strukture sterownikaMRROC++.

23

4 SYSTEM MRROC++

Rysunek 4.1: Schemat blokowy systemu MRROC++, z uwzglednieniem sposobu komunikacji

miedzywatkowej ([7]).

Poszczególne elementy systemuMRROC++pełnia nastepujace role:

watki UI i SRP – zapewniaja komunikacje sterownika z operatorem,

proces MP – koordynuje prace całego systemu,

procesy ECP – realizuja algorytmy sterowania poszczególnymi efektorami,

procesy EDP – sa własciwymi sterownikami konkretnego typu efektora,

watki EDP_Servo – sa odpowiedzialne za kontakt z warstwa sprzetowa napedów efektora,

procesy VSP – odczytuja i przetwarzaja dane z czujników rzeczywistych.

24

4 SYSTEM MRROC++

Na najnizsza z warstw, które wyrózniamy w układzie sterowania, składaja sie procesy od-

wołujace sie bezposrednio do urzadzen rzeczywistych (efektorów, receptorów). Nazywamy je

sterownikami. W przypadku robotów, procesyEDP rozwiazuja proste i odwrotne zadanie ki-

nematyki, watkiEDP_Servorealizuja natomiast serwomechanizmy poszczególnych osi. Proces

EDPzostał szczegółowo opisany w [2].

ProcesyVSPzajmuja sie odczytem i przetwarzaniem danych z czujników rzeczywistych.

ProcesyVSPi EDP sa niezalezne od zadania, natomiast zalezne od sprzetu. Aby dołaczyc no-

wy efektor lub czujnik do systemu, konieczna jest modyfikacja odpowiedniego procesu.

W warstwie wyzszej, procesyECP(Effector Control Process) zajmuja sie sterowaniem po-

szczególnymi efektorami, a w warstwie nadrzednej procesMP (Master Process) zajmuje sie

koordynacja procesów, które steruja poszczególnymi efektorami. ProcesyMP orazECPsa za-

lezne od zadania, natomiast niezalezne od sprzetu. W celu zdefiniowania nowego zadania nalezy

zmienic fragmenty procesówECP lub EDP.

Generatory trajektorii, okreslajace sposób realizacji zadania, wystepuja zarówno na pozio-

mie procesuMP jaki i procesówECP, sterujacych poszczególnymi efektorami (robotami).

W warstwie komunikacji uzytkownika ze sterownikiem, która jest niezalezna od zadania i

sprzetu, wyrózniamy watkiUI i SRP. WatekUI (User Interface) obsługuje zlecenia operato-

ra systemu.SRP(System Response Process) odpowiada za odbieranie komunikatów od innych

procesów.

W trakcie komunikacji pomiedzy poszczególnymi procesami systemuMRROC++ uzywa-

ne sa bufory komunikacyjne. Kazdy z procesów posiada bufory, w których przechowywane sa

pakiety komunikacyjne – struktury danych odbierane z lub wysyłane do innych procesów.

Podstawowe zadania procesuMP

• koordynacja pracy systemu,

• kontakt z operatorem systemu,

• kontakt z procesami VSP,

• przesyłanie do procesu SRP informacji o swoim stanie i ewentualnych błedach,

• generacja trajektorii dlascisle współpracujacych robotów.

Podstawowe zadania procesuECP

• realizacja programu uzytkownika dla pojedynczego robota,

• kontakt z MP i EDP oraz VSP i UI,

• przesyłanie do SRP informacji o swoim stanie i ewentualnych błedach,

25

4 SYSTEM MRROC++

• generacja trajektorii, jesli robot działa niezaleznie lub luzno współpracuje z innymi robo-

tami.

Kazdy z czterech procesów:MP,ECP, VSPorazEDPmozna podzielic na dwie czesci (rysunek

4.2).

Powłoka procesu (stała)

Zawiera:kod do komunikacji z pozostałymi procesamikod do obsługi błędów

Jądro użytkowe procesu (wymienne)

Zawiera:program właściwy

Rysunek 4.2: Struktura ogólna procesów MP, ECP, EDP oraz VSP ([2]).

Powłoka kazdego z tych procesów wykonuje rejestracje procesu w systemie operacyjnym.

Do jej zadan nalezy równiez komunikacja z innymi procesami oraz obsługa błedów.

Zadania powłoki procesuMP

• powoływanie (po zakonczeniu likwidacja) procesów ECP i VSP,

• tworzenie buforów komunikacyjnych z innymi procesami,

• obsługa zgłoszonych wyjatków,

• sygnalizowanie sytuacji awaryjnych (przekazanie informacji o awarii do watku SRP),

• realizacja programu uzytkowego zawartego w jadrze dostarczonym przez programiste,

gdzie zawarte sa instrukcje ruchowe.

Zadania powłoki procesuECP

• nasłuch polecen procesu MP,

• powoływanie (oraz likwidacja) współdziałajacych procesów VSP,

26

4 SYSTEM MRROC++

• utworzenie buforów komunikacyjnych z innymi procesami,

• sygnalizowanie do procesu MP sytuacji awaryjnych (przekazanie do SRP informacji o

awarii),

• realizacja programu uzytkowego zawartego w jadrze dostarczonym przez programiste,

gdzie zawarte sa instrukcje ruchowe.

Szczegółowe informacje na temat procesów mozna znalezc w pracy [3].

Zadania jadra sa rózne dla kazdego procesu. Rejestracja w systemie, powoływanie proce-

sów potomnych oraz inicjalizacja kanałów komunikacyjnych pomiedzy jadrem, a procesami

współpracujacymi odbywa sie w czesci stałej procesu. Struktura oraz funkcje jadra sa zalezne

od rodzaju i złozonosci zadania.

Struktury jadra procesu MP i jadra procesu ECP przedstawiaja rysunki 4.3 i 4.4.

27

4 SYSTEM MRROC++

Rysunek 4.3: Wewnetrzna struktura jadra procesu MP ([2]).

28

4 SYSTEM MRROC++

Rysunek 4.4: Wewnetrzna struktura jadra procesu ECP ([2]).

29

5 PROJEKTOWANIE PŁYTEK DRUKOWANYCH

5 Projektowanie płytek drukowanych

Podłaczenie czujnika FT6284 do karty akwizycji danych PCI-6034E wymagało zaprojek-

towania płytki, która połaczyłaby dwa odmienne interfejsy. Nalezało równiez usprawnic poła-

czenie pomiedzy karta Advantech PCI-1751 i mikrokomputerowym sterownikiem czujnika siły

FT3084.

W tym rozdziale opisano dwie zaprojektowane płytki drukowane, z których:

pierwsza – uporzadkowała dotychczasowe połaczenie znajdujacego sie w laboratorium mikro-

komputerowego sterownika czujnika siły FT3084 (Schunk F/T 65/5) z karta Advantech

PCI-1751,

druga – umozliwiła komunikacje pomiedzy nowym czujnikem siły ATI Gamma F/T, a karta

PCI National Instruments NI-6034E.

5.1 Opis narzedzi programowych. Program Protel DXP

Płytki drukowane zaprojektowano przy uzyciu programuProtel. ProgramProtel firmy Al-

tium jest systemem do projektowania obwodów drukowanych, pozwalajacym uzytkownikowi

opracowywac i integrowac układy FPGA. Poza kompletnym zestawem narzedzi do projektowa-

nia na poziomie PCB,Protelposiada zestaw komponentów dla projektów FPGA i instrumentów

wirtualnych, wspomagajacych uruchamianie w sprzecie.

Informacje niezbedne do poznania sposobu działania programu oraz projektowania płytek

zaczerpnieto ze strony producenta oprogramowania [19] oraz z dokumentacji dołaczonej do

programu.

5.2 Nowa płytka dla czujnika siły FT3084 (Schunk F/T 65/5)

Dotychczasowe połaczenie łaczace karte Advantech PCI-1751 z mikrokomputerowym ste-

rownikiem czujnika siły Schunk F/T 65/5 wymagało wymiany ze wzgledu na zmiane połaczen

sciezek. Poprzednia wersja płytki była zmodyfikowana poprzez połaczenie niektórychsciezek

przewodami. Takie rozwiazanie było zawodne, gdyz mogłoby dojsc do łatwego rozłaczenia sie

przewodów i braku komunikacji z czujnikiem. W zwiazku z tym zdecydowano sie na wymiane

płytki.

W tabeli 5.1 pokazano sposób połaczenia oraz funkcje sygnałowe karty Advantech PCI-

1751 i mikrokomputerowego sterownika czujnika siły Schunk F/T 65/5. Połaczenia na płytce

zostały poprowadzone pomiedzy:

– złaczem K77-68S firmy KYCON (ULTRA SCSI) o 68 wejsciach–wyjsciach, po stronie karty

we-wy Advantech-1751,

30


– złaczem DB-50 (SCSI) o 50 wejsciach–wyjsciach, po stronie skrzynki sterownika czujnika

siły firmy Schunk.

31


Sygnał Kierunek przepływu danych ULTRA SCSI SCSI50

wyjscie karty we-wy 1 38








GND masa 9 1

wejscie karty we-wy 10 46








ACK wejscie karty we-wy 19 20

OBF wyjscie karty we-wy 23 19

IBF wyjscie karty we-wy 24 49

















STB wejscie karty we-wy 53 50

Tabela 5.1: Sposób połaczenia oraz funkcje linii sygnałowych karty Advantech PCI-1751 i mi-

krokomputerowego sterownika czujnika siły Schunk F/T 65/5.

32


Na podstawie połaczen zdefiniowanych w tabeli 5.1 zaprojektowano, przy uzyciu programu

Protel DXP, płytke pokazana na rysunku 5.1. Płytka ta ma wymiary 9.4 na 8.3 centymetra i

wpasowuje sie w zakupiona skrzynke o wymiarach 11 na 9 na 4 centymetrów.

Rysunek 5.1: Schemat zaprojektowanej płytki dwustronnej, łaczacej karte Advantech PCI-1751

z mikrokomputerowym sterownikiem czujnika siły Schunk F/T 65/5.

Na płytce (rysunek 5.1) zostały zamontowane dwa złacza ULTRA–SCSI o 68 wejsciach–

wyjsciach oraz SCSI–50 o 50 wejsciach–wyjsciach. Z powodu braku odpowiednich bibliotek

zawierajacych potrzebne złacza w programieProtel DXP, zaprojektowano je od nowa i umiesz-

czono w oddzielnych bibliotekach.

5.3 Płytka umozliwiajaca komunikacje z czujnikiem Gamma FT6284

Po zaprojektowaniu pierwszej płytki, zaprojektowano druga płytke drukowana, która jest

wykorzystywana do komunikacji z nowym czujnikiem siły Gamma F/T firmy ATI.

W tabeli 5.2 pokazany został schemat połaczen na płytce pomiedzy:

– przetwornica TEN 3–0523 firmy TRACO POWER, przetwarzajacej napiecie wejsciowe z

karty PCI NI-6034E (0V,+5V), na wymagane w czujniku napiecie (±15V),

33


– złaczem K77-68S firmy KYCON (ULTRA SCSI) o 68 wejsciach–wyjsciach (w kierunku

karty PCI NI-6034E),

– złaczem D-25 o 25 wejsciach–wyjsciach (w kierunku czujnika siły).

D-25 TEN-3 ULTRA SCSI Sygnał

(w kierunku czujnika) (przetwornica) (w kierunku karty PCI NI-6034E)

22, 23 8 +5V zasilanie

2, 3 13 0V zasilanie

6 14 +15V (+VANA) zailanie

10 9, 16 56 AGnd/AlGnd

11 11 −15V (−VANA) zasilanie

1 68 SG0 wyjscie

3 34 SG0 odniesienie

7 33 SG1 wyjscie


17 65 SG2 wyjscie


8 30 SG3 wyjscie


13 28 SG4 wyjscie


9 60 SG5 wyjscie


14 25 zarezerwowane

19 58 zarezerwowane

2 57 zarezerwowane

16 23 zarezerwowane

15 52 zarezerwowane

Tabela 5.2: Sposób połaczenia oraz funkcje linii sygnałowych łaczacych karte PCI NI-6034E z

nowym czujnikiem siły Gamma F/T firmy ATI.

Na rysunku 5.2 znajduje sie schemat połaczenia płytki PCI NI-6034E z czujnikiem siły ATI

Gamma F/T. Posrodku znajduje sie skrzynkaprzetwornik, w której zamontowano przetwornice

zamieniajaca wchodzace do płytki zasilanie 0,+5V na wymagane do działania czujnika±15V.

Na schemacie zaznaczono zmiane koncówek w stosunku do przedstawionego schematu poła-

czen producenta.

34


Rysunek 5.2: Schemat połaczenia płytki PCI NI-6034E z czujnikiem siły ATI Gamma F/T.

W oparciu o połaczenia zdefiniowane w tabeli 5.2 zaprojektowano, przy uzyciu programu

Protel DXP, płytke pokazana na rysunku 5.3. Płytka ta ma wymiary 9.4 na 8.3 centymetra i

wpasowuje sie w zakupiona skrzynke o wymiarach 11 na 9 na 4 centymetrów.

35


Rysunek 5.3: Schemat zaprojektowanej płytki dwustronnej, umozliwiajacej połaczenie karty

PCI NI-6034E z nowym czujnikiem siły ATI Gamma F/T.

Na płytce (rysunek 5.3) zostały zamontowane dwa złacza ULTRA–SCSI o 68 wejsciach–

wyjsciach i DB-25 o 25 wejsciach–wyjsciach oraz przetwornica TEN 3-0523. Z powodu braku

odpowiednich bibliotek zawierajacych potrzebne złacza oraz przetwornicy w programiePro-

tel DXP, zaprojektowano je od nowa i umieszczono w oddzielnych bibliotekach.

36

6 IMPLEMENTACJA OPROGRAMOWANIA CZUJNIKA SIŁY W SYSTEMIE MRROC++

6 Implementacja oprogramowania czujnika siły w systemie

MRROC++

W tym rozdziale opisano sposób podłaczenia sterownika poprzedniego czujnika siły FT3084

oraz podłaczenie nowego czujnika siły Gamma FT6284.

6.1 Czujnik siły FT3084

Sposób podłaczenia czujnika siły FT3084 został juz wczesniej przedstawiony w rozdziale

3.2. Zmodyfikowane i uporzadkowane przez nowozaprojektowana płytke połaczenia z czujni-

kiem siły zostały opisane w tabeli 5.1 w rozdziale 5.

W ponizszym podrozdziale zostana podane podstawowe informacje o implementacji ste-

rownika i działaniu zastanego czujnika. Bardziej szczegółowe informacje na temat sterownika

istniejacego juz czujnika siły mozna znalezc w [8].

Komunikacja z mikrokomputerowym sterownikiem czujnika Schunk moze odbywac sie w

dwóch trybach:

• transmisja szeregowa przez port RS-232C,

• transmisja równoległa poprzez rejestry we–wy

W systemie do transmisji równoległej wykorzystywana jest karta 48 wejsc–wyjsc cyfrowych

typu Advantech PCI-1751 na magistrali PCI. Słuzy ona do odbierania danych od mikrokom-

puterowego sterownika czujnika. Wysyłanie rozkazów do czujnika odbywa sie poprzez port

szeregowy, gdyz transmisja z komputera PC do sterownika czujnika poprzez złacze rownoległe

jest zawodna.

Karta Advantech PCI-1751 posiada pojedyncze, 68 nózkowe złacze typu ULTRA SCSI. Z

kolei mikrokomputerowy sterownik czujnika, 50 nózkowe złacze do tasmy SCSI. Tasmy wy-

chodzace z obu złacz skrzyzowano przy wykorzystaniu nowozaprojektowanej płytki, opisanej

w rozdziale 5. Sposób połaczenia nózek obu złacz, oraz funkcje linii sygnałowych przedstawia

tabela 5.1 na stronie 32.

Teoretycznie wykonanie pełnego odczytu siły (3 składowe siły i 3 momenty sił) jest mozli-

we w czasie krótszym niz 2,4 ms. Badania praktyczne wykazały,zesredni czas odczytu wynosi

2,65 ms.

6.2 Implementacja sterownika nowego czujnika siły Gamma FT6284 fir-

my ATI

Sposób podłaczenia czujnika siły FT6284 został juz wczesniej przedstawiony w rozdziałach

3.3 oraz 5 (rysunek 5.2). W tabeli 5.2 w rozdziale 5 pokazano sposób połaczenia sygnałów po-

37


miedzy czujnikiem, a karta PCI.

W ponizszym podrozdziale zostanie opisany sposób zaimplementowania nowego sterowni-

ka czujnika siły.

6.2.1 Parametry karty National Instruments PCI-6034E oraz czujnika siły Gamma fir-

my ATI

W tym podrozdziale podane zostana parametry nowego czujnika siły Gamma FT6284 firmy

ATI oraz karty akwizycji danych Multi-DAQ PCI-6034E firmy National Instruments.

Parametry czujnika Gamma FT6284 firmy ATI

W tabelach 6.1 oraz 6.2 podano podstawowe dane dotyczace czujnika Gamma FT6284 firmy

ATI, dla którego został napisany sterownik. Na rysunku 6.1 na stronie 40 znajduje sie schemat

wspomnianego czujnika.

Dopuszczalne przeciazenie

Fxy ± 1200 N

Fz ± 4100 N

Txy ± 79 N-m

Tz ± 82 N-m

Sztywnosc

siła w osiach X i Y 9.1 ∗ 106 N/m

siła w osi Z 18 ∗ 106 N/m

moment w osiach X i Y 11 ∗ 103 N-m/rad

moment w osi Z 16 ∗ 103 N-m/rad

Wymiary

Waga ± 250g

Srednica ± 75.4 mm

Wysokosc ± 33.3 mm

Tabela 6.1: Parametry czujnika siły Gamma.

38


Kalibracja

rodzaj kalibracji SI-32-2.5 SI-65-5 SI-130-10

Fx,Fy (±N ) 32 65 130

Fz (±N ) 100 200 400

Tx,Ty (± N-m) 2.5 5 10

Tz (± N-m) 2.5 5 10

Rozdzielczosc

rodzaj systemu F/T CON DAQ CON DAQ CON DAQ

Fx,Fy (±N ) 1/80 1/640 1/40 1/320 1/20 1/160

Fz (±N ) 1/40 1/320 1/20 1/160 1/10 1/80

Tx,Ty (± N-m) 1/1000 1/8000 1/667 3/16000 1/400 1/3200

Tz (± N-m) 1/1000 1/8000 1/667 3/16000 1/400 1/3200

Tabela 6.2: Parametry kalibracji i rozdzielczosc czujnika siły Gamma. CON = Controller F/T

system, DAQ = 16 bit F/T System

Czujnika Gamma FT6284 firmy ATI został skalibrowany w systemie DAQ (system 16 bito-

wy F/T), w systemie kalibracji SI-65-5. Dane o systemie oraz rodzaju kalibracji sa zawarte w

pliku ft6284.caldołaczonym do zakupionego czujnika.

39


Rys

unek

6.1:

Sch

emat

czuj

nika

Gam

ma

firm

yAT

I([2

5]).

40


Parametry karty PCI-6034E firmy National Instruments

Karta 6034E firmy National Instruments posiada złacze PCI. Ma 16 wejsc analogowych

pojedynczych lub 8 róznicowych (sparowanych) oraz 8 wejsc cyfrowych. Rozdzielczosc na

wejsciu wynosi 16 bitów, maksymalna osiagana czestotliwosc próbkowania to 200 kS/s. Zakres

wejsciowy karty znajduje sie w przedziale od±0.05 do±10 V. Karta posiada 8 wyjsc cyfro-

wych. W tabeli 6.3 pokazano precyzje pomiaru w zaleznosci od wybranego zakresu sygnałów

wejsciowych.

Wzmocnienie Zakres wejsciowy Rozdzielczosc

0.5 -10 do +10 V 305.2µV

1.0 -5 do +5 V 152.6µV

10.0 -500 do +500 mV 15.3µV

100.0 -50 do +50 mV 1.53µV

Tabela 6.3: Rozdzielczosc pomiaru karty PCI-6034E.

Karta posiada ochrone przeciwprzepieciowa. Przy załaczaniu – do±25V, przy wyłaczaniu

– do±15V. Kolejka FIFO, w której karta umieszcza dane o zmierzonych sygnałach, ma pojem-

nosc 512 próbek. Rekomendowne jest odczekanie 15 minut od właczenia karty do rozpoczecia

pracy. Dodatkowe informacje o karcie PCI-6034E firmy National Instruments mozna znalezc w

[23].

Karta PCI-6034E usprawnia działanie czujnika w stosunku do poprzedniej karty Advantech.

Dzieki nowemu sposobowi przeliczania danych otrzymywanych z czujnika, znacznie uprosz-

czona została komunikacja z czujnikiem, co wpłyneło na wzrost niezawodnosci działania tego

urzadzenia. W nowym rozwiazaniu nie jest wykorzystywana, niezbedna w poprzednim przy-

padku, skrzynka akwizycji danych firmy Schunk. Skrzynka ta przygotowuje dane odebrane z

czujnika, a karta Advantech komunikuje sie z nia i sprawdza poprzez złacze szeregowe, czy sa

juz gotowe do odebrania nowe dane. Obróbka sygnału z nowego czujnika wykonywana jest w

całosci na komputerze, w którym znajduje sie karta PCI-6034E.

6.2.2 Metody implementacji sterownika czujnika siły w systemie QNX

W pierwszej fazie implementacji nowego sterownika zaznajomiono sie z mozliwymi spo-

sobami obsługi czujnika siły firmy ATI Gamma przez karte PCI-6034E firmy National Instru-

ments. Program obsługujacy czujnik siły powinien odpowiednio ustawic parametry karty PCI-

6034E w celu zebrania danych z czujnika, a nastepnie zebrane dane przetworzyc na informacje

w postaci sił i momentów sił wskazywanych przez czujnik. Do realizacji przeliczenia sygnału

napieciowego z karty PCI na siłe i momenty wymagane w systemieMRROC++wykorzystano

41


bibliotekeATI DAQ F/T Library (v.1.0.5). Bilioteka ta przelicza dostarczony szescioelemento-

wy wektor, zawierajacy napiecia odpowiadajace siłom oddziałujacym na czujnik siły w trzech

kierunkach oraz zwiazanym z nimi momentom sił, do postaci sił i momentów w odpowiednio

Niutonach i Niutonometrach.

Przy implementacji czesci sterownika obsługujacej karte PCI-6034E, skorzystano z pakie-

tu Measurement Hardware Development Kit(MH DDK), udostepnianego przez firme National

Instruments. Pakiet ten jest zbiorem funkcji ułatwiajacych komunikacje z karta PCI oraz szablo-

nów funkcji umozliwiajacych dostep do sprzetu. Szablony te sa wypełniane przez programiste,

w zaleznosci od docelowego systemu operacyjnego, w którym ma działac karta.

W poczatkowym etapie prac, rozwazano wykorzystanie do komunikacji z karta PCI-6034E

biblioteki COMEDI [18]. Biblioteka ta jest jednak przeznaczona dla systemów linuksowych, z

tego wzgledu bardziej odpowiednim do wykorzystania w systemieQNX 6.3.0okazał sie pakiet

MH DDK firmy National Instruments.

6.2.3 Wstepna implementacja sterownika czujnika siły poza systemem

MRROC++

Pierwszym etapem niniejszej pracy było zaimplementowanie sterownika czujnika Gamma

firmy ATI w srodowiskuQNX 6.3.0. Ze wzgledu na przewidywana duza ilosc testów oraz nie-

zbedne dokładne zbadanie własciwosci karty oraz czujnika siły, zdecydowano sie na wstepna

implementacje sterownika czujnika w systemieQNX 6.3.0poza systememMRROC++. Dzieki

temu mozna było szybko sprawdzic wybrane rozwiazanie, bez potrzeby czasochłonnego prze-

noszenia niesprawdzonego sterownika do struktury systemu MRROC++.

Pozwoliło to na przeprowadzenie wielu badan przed docelowa implementacja czujnika w

systemie. Kod sterownika czujnika działajacego poza systememMRROC++ został dołaczo-

ny do niniejszej pracy i moze słuzyc jako program do kontroli stanu czujnika lub spełniajacy

funkcje precyzyjnej wagi, gdy czujnik nie jest zamontowany na ramieniu robota.

Implementacja sterownika czujnika

Implementacje sterownika poza systememMRROC++ rozpoczeto od zebrania danych z

czujnika i przedstawienia ich w postaci jednostek napiecia – woltów [V]. W tym celu napisano,

w oparciu o pakietMeasurement Hardware Development Kit(MH DDK) firmy National In-

struments, program uruchamiajacy i konfigurujacy karte, zbierajacy dane z czujnika i nastepnie

przetwarzajacy te dane do postaci wyjsciowej w woltach.

W trakcie pisania programu zdecydowano sie na ustawienie rejestrów karty w taki sposób,

by zbierała ona 6 pomiarów z czujnika, w odstepie10µs kazdy, w zakresie±10V , który od-

powiada zakresowi sygnałów otrzymywanych z czujnika. W ten sposób wykorzystano pełne

mozliwosci szybkosci zbierania danych przez karte. Karta zbiera kolejno 6 pomiarów, z któ-

42


rych trzy pierwsze to pomiary siły w trzech kierunkach, a kolejne trzy to momenty sił wokół

osi X, Y, Z.

Porównanie wyników otrzymywanych z programu zaimplementowanego w systemieQNX

6.3.0oraz z programu firmowego ATI wsrodowiskuWindows, potwierdziło poprawnosc zaim-

plementowanego rozwiazania – odczyty w woltach były identyczne. W kolejnym kroku skon-

centrowano sie na przeliczeniu otrzymanych wyników z postaci woltów na jednostki siły oraz

momentów.

W tym celu rozbudowano dotychczasowy program i dołaczono do niego bibliotekeATI DAQ

F/T Library (v.1.0.5). Biblioteka ta pozwala na dołaczanie nowych czujników firmy ATI. Da-

ne o kalibracji oraz mozliwosciach czujnika sa wczytywane w trakcie jego inicjalizacji. Dzieki

temu uzyskana została uniwersalnosc rozwiazania (łatwe podłaczenie innych czujników firmy

ATI) oraz mozliwosc uwzglednienia przypadków przeciazenia czujnika. Mozliwe jest równiez

ustawianie rodzaju jednostek miar i sił, w jakich sa podawane dane niezbedne do działania

programu oraz wyswietlane wyniki. Ponizej przedstawiono zawartosc wczytywanego w cza-

sie inicjalizacji pliku konfiguracyjnego dla czujnika Gamma firmy ATI o oznaczeniu seryjnym

FT6284.

43


<?xm

l ver

sion

="1.

0" e

ncod

ing=

"utf-

8"?>





<FTS

enso

r S

eria

l="F

T628

4" B

odyS

tyle

="G

amm

a" F

amily

="D

AQ

" Num

Gag

es="

6" C

alFi

leVe

rsio

n="1

.0">

<Cal

ibra

tion

Par

tNum

ber=

"SI-6

5-5"

Cal

Dat

e="7

/21/

2005

" For

ceU

nits

="N

" Tor

queU

nits

="N

-m" D

istU

nits

="m

" Out

putM

ode=

"Gro

und

Ref

eren

ced

Diff

eren

tial"

Out

putR

ange

="20

" HW

Tem

pCom

p="T

rue"

Gai

nMul

tiplie

r="1

" Cab

leLo

ssD

etec

tion=

"Fal

se" O

utpu

tBip

olar

="Tr

ue">

<Axi

s N

ame=

"Fx"

val

ues=

" -0.

4070

9 -0

.273

18

0.34

868

-33.

5815

6 -0

.326

09 3

3.54

162

" max

="65

" sca

le="

4.55

1197

2116

989"

/><A

xis

Nam

e="F

y" v

alue

s="

0.35

472

38.

2273

0 -0

.411

73 -1

9.49

156

0.4

9550

-19.

1527

1 " m

ax="

65" s

cale

="4.

5511

9721

1698

9"/>

<Axi

s N

ame=

"Fz"

val

ues=

" 18.

7263

5 -0

.596

76 1

9.27

843

-0.5

6931

18.

6935

2 -0

.676

33 "

max

="20

0" s

cale

="1.

4124

4051

3975

52"/>

<Axi

s N

ame=

"Tx"

val

ues=

" -0.

4083

6 -0

.959

08 -3

3.37

957

1.3

8537

32.

5252

2 -0

.511

56 "

max

="5"

sca

le="

84.8

8432

4557

6086

"/><A

xis

Nam

e="T

y" v

alue

s=" 3

7.13

715

-1.0

2875

-20.

0047

4 -0

.279

59 -1

9.34

135

1.4

2577

" m

ax="

5" s

cale

="84

.884

3245

5760

86"/>

<Axi

s N

ame=

"Tz"

val

ues=

" -0.

1577

5 -1

8.16

831

-0.0

0133

-18.

7896

1 0

.318

95 -1

8.38

586

" max

="5"

sca

le="

80.9

4720

3752

5247

"/><B

asic

Tran

sfor

m D

x="0

" Dy=

"0" D

z="0

.013

4355

078"

Rx=

"0" R

y="0

" Rz=

"0"/>

</C

alib

ratio

n></

FTS

enso

r>

Rys

unek

6.2:

Plik

zin

form

acja

kalib

racy

jna

czuj

nika

Gam

ma

FT

6284

firm

yAT

I.

44


Powyzszy kod pochodzacy z pliku konfiguracyjnego dla czujnika Gamma FT6284 przeka-

zuje do programu informacje na temat jednostek, kalibracji oraz zakresu obciazen czujnika.

Po przekształceniu danych z czujnika z postaci wyrazonej w woltach do postaci wyrazo-

nej w jednostkach sił – Niutonach oraz momentów sił – Niutonometrach, ponownie przepro-

wadzono serie badan porównujacych działanie programu napisanego w systemieQNX 6.3.0i

programu firmy ATI uruchamianego wsrodowiskuWindows. Tym razem porównywano odczy-

ty przedstawiane w Niutonach i Niutonometrach. Do badan wykorzystano stalowy odwaznik o

masie 800g. Wskazania wygenerowane w napisanym programie oraz programie firmowym by-

ły identyczne, coswiadczy o poprawnosci zaimplementowanego algorytmu pobierania danych

z czujnika i dalszej ich obróbki, do postaci wyswietlanej w jednostakach sił i momentów sił.

Ze wzgledu na poczatkowe niezerowe wskazania czujnika siły, wynikajace z ciezaru urza-

dzenia i jego orientacji w przestrzeni, wprowadzono dodatkowy mechanizm zerowania odczytu

(tzw. bias). Dzieki temu odczyt nie jest zniekształcony przez mase i połozenie czujnika.

Po sprawdzeniu poprawnosci działania programu, zdecydowano sie na próbe usprawnienia

odczytu danych z czujnika, realizowanego przez karte PCI-6034 firmy National Instruments,

poprzez wykorzystanie przerwan generowanych przez karte. W pierwotnej wersji odczyt da-

nych z czujnika odbywał sie w petli. Po uruchomieniu programu nawiazywano komunikacje z

karta i ustawiano odpowiednie rejestry karty w celu poprawnego jej działania, a nastepnie karta

wykonywała odczyt danych z czujnika. Zakonczenie odczytu i gotowosc danych do odczytu by-

ły sygnalizowane poprzez ustawienie odpowiedniego bitu w jednym z rejestrów karty. Program

odczytywał ten rejestr w petli i sprawdzał czy dane z czujnika sa juz gotowe do odczytu. Jedno-

czesnie wprowadzono mechanizm uniemozliwiajacy zawieszenie sie w tej petli. Jesli bit został

ustawiony, dane odczytywano z kolejki FIFO karty PCI-6034 i przetwarzano w programie do

postaci sił i momentów.

Po zaimplementowaniu obsługi przerwania karty i przeprowadzeniu badan z programem

działajacym w pierwotnej oraz nowej wersji z przerwaniem, nie stwierdzono poprawy w szyb-

kosci działania programu. W trakcie badan stwierdzono,ze kolejne czynnosci: obsługa przerwa-

nia z karty i okreslenie,ze dotyczy ono pojawienia sie pojedynczego pomiaru w kolejce FIFO,

dostep do i odczyt pojedynczego pomiaru z rejestru kolejki FIFO, wyzerowanie rejestru infor-

mujacego o pojawieniu sie pomiaru; wykonywane szesciokrotnie w celu uzyskania kompletu

pomiarów z czujnika, powoduja,ze znacznie wydłuza sie czas uzyskaniazadanych pomiarów

z karty. Znacznie efektywniejsze i niezawodne okazało sie rozwiazanie, w którym czeka sie

0.0007s na zebranie szesciu pomiarów z czujnika przez karte, a nastepnie realizowane sa jedno-

razowy dostep do rejestru kolejki FIFO i odczyt do momentu opróznienia kolejki z wczesniej

okreslonej w sposobie akwizycji liczby pomiarów. W celu uzyskania niezawodnosci takiego

rozwiazania (wykluczenia zawieszenia) wprowadzono mechanizm przerwania odpytywania re-

jestru FIFO karty po 0.002 sekundy, w przypadku braku pojawienia sie nowego pomiaru w

45


rejestrze kolejki FIFO. Takie rozwiazanie uniemozliwia zawieszenie w przypadku wadliwego

działania karty.

Wprowadzone rozwiazanie zostało wielokrotnie przetestowane i działało niezawodnie. Ze

wzgledu na wymaganie szybkiego przetwarzania danych w systemieMRROC++zdecydowano

sie na wykorzystanie rozwiazania z odpytywaniem. Rozwiazanie takie nie obciaza w duzym

stopniu systemu, poniewaz czas realizacji odczytu jest bardzo krótki.

W programie pozostawiono jednak rozwiaznie opierajace sie na obsłudze przerwania z kar-

ty, które moze byc wybrane za pomoca ustawienia odpowiedniej wartosci w pliku nagłówko-

wym display.h.

Rozwiaznie opierajace sie na obsłudze przerwania z karty nie okazało sie lepsze od rozwia-

znia z odpytywaniem, ze wzgledu na niewielki wybór przerwan generowanych na podstawie

zapełnienia kolejki FIFO karty. Do wyboru sa nastepujace: kolejka pusta, zapełniona w poło-

wie i pełna. Pełna pojemnosc kolejki FIFO karty PCI-6034E to 512 pomiarów. Nie udało sie

uzyskac przerwania wskazujacego na pojawienie sie kompletu 6 danych odczytanych z czujnika

siły.

Badanie własciwosci nowego czujnika siły

Po zaimplementowaniu sterownika czujnika siły w systemieQNX 6.3.0przeprowadzono

serie badan i porównan jego działania z programem firmy ATI wsrodowiskuWindows. Nowy

program wskazywał te same odczyty co program firmowy, czujnik siły ATI Gamma działał po-

prawnie.

W zwiazku z tym zdecydowano sie na przejscie do kolejnego etapu pracy, czyli do przenie-

sienia sterownika czujnika do systemuMRROC++.

6.2.4 Implementacja czujnika siły w systemie MRROC++

Czujnik siły w systemie MRROC++

W systemieMRROC++(opisanym dokładniej w rozdziale 4) implementacja czujników po-

winna byc standardowo powiazana z procesemVSP(Virtual Sensor Process). W procesieVSP

wykonywanoby pomiary siły,ECPzawierałoby regulator siłowy, aEDP pozycyjny. Tego typu

architektura była wygodna w implementacji i dawała sie łatwo dostosowac do róznorodnych

zadan. Niestety miała istotne wady. Utworzony w oparciu o nia regulator pozycyjno–siłowy był

wolny, a obciazenie komputera, na którym uruchomiony był system, było znaczne. W zwiazku

z tym zdecydowano sie na modyfikacje (opisane w [8]), które przyspieszyły działanie regulato-

ra pozycyjno–siłowego.

Regulator pozycyjno–siłowy został wbudowany w całosci, wraz z pomiarami siły, w proces

EDP. Rozszerzyło to wewnetrzny stan robota o wektor sił i momentów sił rejestrowanych w

jego koncówce, a czujnik siły stał sie proprioreceptorem.

46


Wykonywanie odczytów siły w procesieEDP pociagneło za soba stworzenie połaczenia

pomiedzyEDP i VSP. Dzieki temuVSPmoze przetwarzac pomiary siły i nadal pełnic role

„eksteroreceptora siłowego”.VSPuzyskuje nie tylko informacje o sile, ale takze pozycji ro-

bota. PodobnieECP moze dodatkowo otrzymac pomiar siły odEDP i takze go przetworzyc.

Przyjeto,ze procesVSPbedzie wykorzystywany do wykrywania złozonych zdarzen wystepuja-

cych w systemie, natomiastECPdo biezacej modyfikacji parametrów (np.: wartosci zadanych)

regulatorów pozycyjno–siłowych wEDP. Dzieki tej redundancji mozliwe stało sie pisanie róz-

norodnych, wydajnych obliczeniowo aplikacji.

SystemMRROC++opiera sie na kilku procesach, które moga składac sie z kilku watków.

Zaimplementowany regulator pozycyjno–siłowy wykorzystuje bezposrednio 4 watki procesu

EDP i otrzymuje wartosc zadana siły i połozenia od procesuECP. Przepływ informacji na tle

struktury regulatora przedstawia rys. 2.3 na stronie 14. Dokładny opis poszczególnych watków

mozna znalezc w [8].

Implementacje sterownika nowego czujnika siły umieszczono w watku pomiarów sił i mo-

mentów siłEDP_FORCE, skad pomiary siły trafiaja do pozostałych watków.

Dołaczenie sterownika nowego czujnika do systemu MRROC++

Sprawdzony w systemieQNX 6.3.0sterownik nowego czujnika ATI Gamma FT6284 prze-

niesiono do systemuMRROC++ i dowiazano do watkuEDP_SERVO. W celu umozliwienia

łatwego ewentualnego pózniejszego przeniesienia sterownika czujnika do procesuVSP, zacho-

wano strukture funkcji zawartych w szablonieVSP. Dotychczasowy program został zmodyfi-

kowany i podzielony na nastepujace funkcje:

• void configure_sensor (void) – konfiguracja czujnika

• void wait_for_event(void) – oczekiwanie na zdarzenie

• void initiate_reading (void) – zadanie odczytu

• void get_reading (void) – odebranie odczytu

• void terminate (void) – rozkaz zakonczenia odczytu

Zmodyfikowano równiez strukture zródeł systemuMRROC++ tworzac oddzielna bibliote-

ke czujnika ATI Gamma FT6284, która jest kompilowana i dołaczana na etapie kompilacji do

pozostałej czesci systemu. Podobnie zmodyfikowany został sposób dołaczania plików zródło-

wych dla znajdujacego sie juz w laboratorium działajacego czujnika siły.

W ten sposób uległa zmianie hierarchia plików zródłowych dla czujników siły, przez co

uzyskano mozliwosc bardziej elastycznego dołaczania poszczególnych czujników siły (starego

i nowego) do odpowiednich robotów (on_tracklub postument). Wybór dowiazania odpowied-

niego czujnika siły do wybranego robota odbywa sie poprzez zmodyfikowanie jednej linii w

47


plikachMakefileznajdujacych sie w katalogachirp6_on_trackorazirp6_postument.

Po modyfikacji hierarchii zródeł ujednolicono w systemieMRROC++nazwy dotyczace ste-

rowania siłowego zschunknaforce. Dzieki powyzszym zmianom usystematyzowano i uprosz-

czono nazewnictwo odnoszace sie do czujników siły.

W wyniku powyzszych zmian otrzymano całkowicie zintegrowany z systemem MRROC++

sterownik nowego czujnika siły ATI Gamma FT6284 oraz ujednolicona hierarchie zródeł dla

nowego czujnika i poprzedniego czujnika ATI FT3084.

W trakcie przenoszenia sterownika nowego czujnika siły do systemuMRROC++ napo-

tykano na wiele problemów zwiazanych z zaleznosciami czasowymi dotyczacymi zbierania i

udostepniania danych przez karte PCI-6034E. Z tego wzgledu wprowadzono kilka opóznien,

bez których dane zebrane przez karte nie byłyby poprawne. Sa to:

• START_TO_READ_TIME_INTERVAL=0.0007s – okreslajace czas odzadania pomia-

rów do rozpoczecia ich odczytu,

• INTERRUPT_INTERVAL=0.00007s – okreslajace czas oczekiwania zanim uaktywnione

zostanie przerwanieswiadczace o pojawieniu sie danych w rejestrze,

• START_TO_READ_FAILURE=0.002s – okreslajace czas spedzany w petli oczekujacej

na pojawienie sie danych w rejestrze.

Wszystkie powyzsze opóznienia zostały wprowadzone dla przyspieszenia odczytu danych

z karty. W trakcie badan okazało sie,ze znacznie szybsze jest jednorazowe odebranie komple-

tu pomiarów z karty, niz na przykład szesciokrotne wykonywanie odczytu danych z rejestrów

karty w odpowiedzi na przerwanie z karty, informujace o pojawieniu sie pomiaru w rejestrze

kolejki FIFO karty.

Pierwsze opóznienie START_TO_READ_TIME_INTERVAL wynika z mozliwosci prze-

twarzania przez karte zebranych sygnałów i czasu niezbednego do powiadomienia o zebraniu

danych, czyli modyfikacji odpowiedniego bitu rejestru karty. Drugie z opóznien jest wymagane

w przypadku wykorzystania przerwania generowanego przez karte,swiadczacego o tym,ze w

kolejce FIFO jest gotowy do odebrania komplet szesciu danych pomiarowych. Trzecie unie-

mozliwia zawieszenie w petli odpytujacej rejestr karty o gotowe do odebrania wyniki. Jest to

niezbedne w przypadku, gdy karta z jakiegos powodu nie działa poprawnie. Wszystkie powyz-

sze opóznienia zostały zdefiniowane po to, by przyspieszyc odczyt danych z karty oraz by ewen-

tualne nieprawidłowe działanie tej czesci systemu nie wpłyneło na cały systemMRROC++.

W trakcie dołaczania sterownika nowego czujnika siły do systemuMRROC++, zdefiniowa-

no wiele komunikatów, które wyswietlaja sie w oknie operatora systemu, informujac o stanie

pracy czujnika. Sa to m. in. informacja o nadmiernym obciazeniu czujnika oraz powrocie do

stanu normalnej pracy.

48


W przypadku przeciazenia czujnika, generowany jest komunikat o przeciazeniu, a do sys-

temu jest dostarczany ostatni poprawny pomiar. Dzieje sie tak do momentu wyeliminowania

przeciazenia.

Po dołaczeniu czujnika przeprowadzono wiele badan w trybie testowymMRROC++. Na-

stepnie uruchomiono i zbadano działanie czujnika w rzeczywistosci. Czujnik działał poprawnie,

aplikacja wielorobotowa (w wersji przed modernizacja robotów na poczatku 2006r.), wykorzy-

stujaca dwa manipulatory ze starym (FT3084) i nowym (FT6284) czujnikiem oraz tasmociag,

uruchomiona pod koniec 2005 roku, potwierdziła poprawne działanie zaimplementowanego

sterownika czujnika siły.

49

7 OPIS FUNKCJI I KOD STEROWNIKA NOWEGO CZUJNIKA SIŁY GAMMA FT6284 FIRMY ATI

7 Opis funkcji i kod sterownika nowego czujnika siły Gam-

ma FT6284 firmy ATI

W tym podrozdziale zostana opisane wybrane, najwazniejsze funkcje znajdujace sie w ko-

dzie nowego sterownika czujnika siły oraz lokalizacja nowych plikówzródłowych i nagłówko-

wych, dotyczacych zaimplementowanego sterownika czujnika siły.

Program sterownika czujnika siły Gamma firmy ATI został napisany w jezyku C++. Do

napisania sterownika wykorzystano bibliotekeATI DAQ F/T Library (v.1.0.5)oraz pakietMe-

asurement Hardware Development Kit(MH DDK) firmy National Instruments.

Bibilioteka ATI DAQ F/T Library (v.1.0.5)zawiera zbiór funkcji ładujacych i parsujacych

plik konfiguracyjny, dołaczony do czujnika firmy ATI napisanych w jezyku C. W przypadku

czujnika Gamma FT6284 jest to plikft6284.cal, którego nazwa jest oznaczeniem seryjnym

zakupionego czujnika Gamma firmy ATI. Dodatkowo biblioteka ta zawiera funkcje umozliwia-

jace przeliczenie dostarczonych pomiarów w postaci woltów, otrzymanych z systemu akwizycji

danych, na siły i momenty sił. Biblioteka nie zawiera obsługi wejsc–wyjsc (I/O) sprzetowych.

Wszystkie funkcje zawarte w bibliotece mozna wykorzystywac bez ograniczen prawnych. Bi-

blioteka została zmodyfikowana tak, by wszystkie funkcje działały w jezyku C++.

PakietMeasurement Hardware Development Kit(MH DDK) jest kolekcja przykładowego

kodu, dokumentacji sprzetowej oraz klas w jezyku C++, które odpowiadaja układom scalo-

nym znajdujacym sie na kartach akwizycji danych firmy National Instruments. Zawarte sa w

nim funkcje znacznie upraszczajace dostep do sprzetu, dzieki czemu mozna prosciej zaimple-

mentowac obsługe kart akwizycji danych, w systemach nie wspieranych bezposrednio przez

producenta.

Wspomniane pakiet i biblioteka zostały połaczone, a nastepnie znacznie zmodyfikowane i

rozbudowane, tak by jak najlepiej funkcjonowały w systemieQNX.

W zwiazku z wyraznym podziałem plików na czesc dotyczaca odbierania danych z czujnika

ATI i ich obróbki oraz czesci dotyczacej obsługi karty PCI-6034E firmy NI, zdecydowano sie

na podzielenie plików nagłówkowych i umieszczenie ich w dwóch katalogach, odpowiednioati

orazni. Z tego samego wzgledu, w plikach*.cc, bezposrednio dotyczacych nowego sterownika

czujnika siły, zaznaczono, z która z czesci NI lub ATI jest zwiazany dany plik.

Ponizej podano lokalizacje plików sterownika nowego czujnika siły zaimplementowanego

w systemieMRROC++wraz z krótkim opisem spełnianych przez nich funkcji.

Pliki zródłowe, wchodzace w skład sterownika nowego czujnika siły, mozna podzielic na

dwie grupy.

Pliki zródłowe zawierajace funkcje niezbedne do parsowania pliku XMLft6284.cal, z zapi-

sanymi danymi konfiguracyjnymi czujnika siły:

50


../mrrocpp/src/edp/ati6284/

ati/

xmltok_impl.cc, xmltok_ns.cc,

dom.cc, expatls.cc, node.cc, stack.cc, xmlparse.cc, xmlrole.cc, xmltok.cc,

Pliki zródłowe zawierajace funkcje realizujace obsługe karty akwizycji danych oraz zbiera-

nie i przetwarzanie danych pomiarowych:

../mrrocpp/src/edp/

irp6s/

force.cc– zawiera funkcje realizujaca watek siłowy, niezbedny do inicjalizacji oraz

akwizycji danych z czujnika

ati6284/

edp_s.cc– główny plik sterownika czujnika zawierajacy funkcje ustawiajace para-

metry karty akwizycji danych oraz słuzace do komunikacji z karta PCI-6034E,

ftconfig.cc– funkcje ustawiajace jednostki miar i wag uzywane do przeliczania

wejsciowych napiec na siły i momenty sił,

ftconvert.cc– funkcje główne, obsługujace przeliczanie wejsciowych napiec (w

woltach) na siły i momenty (w Niutonach i Niutionometrach) oraz wyswietlanie

postaci pliku kalibracyjnego,

ftrt.cc – funkcje przeliczajace wejsciowe napiecia na siły i momenty,

osiBus.cc– funkcje ustawiajace rodzaj magistrali PCI wykorzystywanej przez kar-

te,

osiUserCode.cc– uruchomienie karty akwizycji danych w systemie QNX, znale-

zienie karty na magistrali PCI, mapowanie rejestrów karty,

tESeries.cc– funkcje resetujaca i inicjalizujaca rejestry karty firmy NI specyficzne

dla serii E kart NI,

tSTC.cc– funkcje resetujaca i inicjalizujaca wszyskie rejestry karty firmy NI,

display.h– zawiera stałe, których zdefiniowanie jako TRUE lub FALSE umoz-

liwia właczenie odpowiednich fragmentów kodu, uzywanych do sprawdzenia

poprawnosci wyników, mozna tu równiez ustawic sposób odbierania danych z

karty, wykorzystujacy odpytywanie lub przerwanie.

ft6284.cal– zawiera tekst zapisany w postaci XML, opisujacy parametry konkret-

nego czujnika siły, w tym przypadku czujnika Gamma firmy ATI o numerze

seryjnym FT6284. Postac tego pliku została pokazana na rysunku 6.2 na stro-

nie 44.

51


Pliki nagłówkowe umieszczono nastepujaco:

../mrrocpp/include/edp_ecp/ati6284/

ati/ – nagłówki dołaczane do plików w trakcie kompilacji, zwiazane ze strona czujnika

ATI,

ni/ – nagłówki dołaczane do plików w trakcie kompilacji, powiazane z plikami dotycza-

cymi karty PCI-6034E firmy National Instruments,

edp_s.h– zawiera definicje klasyedp_ATI6284_force_sensor, dziedziczacej po klasie

edp_force_sensor, która jest główna klasa nowego sterownika czujnika siły.

W dalszej czesci rozdziału zostana szczegółowo opisane najwazniejsze czesci kodu sterow-

nika nowego czujnika siły.

7.1 Watek force_threadrealizujacy odczyt danych z czujnika

Sterownik nowego czujnika siły umieszczono w watku pomiarów sił i momentów sił

EDP_FORCE, skad pomiary siły trafiaja do pozostałych watków. Funkcja realizujaca watek

force_threadjest zdefiniowana w plikuforce.ccw katalogu../mrrocpp/src/edp/irp6s/. Ponizej

pokazano najbardziej znaczaca czesc kodu watku siłowegoforce_thread, obsługujacego czujnik

siły, zawarta w plikuforce.cc.

while(!TERMINATE){

try{

if (force_sensor_do_configure == TRUE; //!< polecenie konfiguracji czujnika

vs->configure_sensor(); //!< wstepna konfiguracja czujnika + ustawienie biasu

force_sensor_do_configure = FALSE; //!< czujnik jest ponownie skonfigurowany

sem_trywait(&new_ms);

sem_post(&new_ms); //!< jest gotowy nowy pomiar

}

else {

vs->wait_for_event(); //!<odczekanie a z pomiar bedzie gotowy

vs->initiate_reading(); //!<odczyt pomiarów z rejestru karty

if (force_sensor_do_configure == FALSE) {

sem_trywait(&new_ms);

sem_post(&new_ms); //!< jest gotowy nowy pomiar

}

}

}

catch (sensor::sensor_error e){

printf("sensor_error in force thread EDP\n");

switch(e.error_no){

case SENSOR_NOT_CONFIGURED:

vs->from_vsp.vsp_report=VSP_SENSOR_NOT_CONFIGURED;

break;

52


case READING_NOT_READY:

vs->from_vsp.vsp_report=VSP_READING_NOT_READY;

break;

}; // end switch

srp_msg->message (FATAL_ERROR, e.error_no);

} // end CATCH

catch(...) {

printf("unidentified error force thread w EDP\n");

};

} // end while(;;)

Listing 1: Watekforce_threadkomunikujacy sie z czujnikiem siły

Komunikacja z czujnikiem, realizowana w watkuforce_thread, odbywa sie za pomoca funk-

cji configure_sensor(), wait_for_event()oraz initiate_reading(). Funkcje te sa zdefiniowane w

pliku edp_s.cc.

7.2 Klasaedp_ATI6284_force_sensor : public edp_force_sensorrealizuja-

ca sterownik czujnika siły FT6284

Klasaedp_ATI6284_force_sensorjest klasa konkretna i dziedziczy po bazowej klasie abs-

trakcyjnej edp_force_sensor, przeznaczonej dla czujników siły dołaczanych do systemu

MRROC++.

Klasa realizujaca sterownik nowego czujnika siły została zdefiniowana w pliku nagłówko-

wym edp_s.hw kataloguinclude/edp_ecp/ati6284/.

class edp_ATI6284_force_sensor : public edp_force_sensor{

private:

unsigned uCount; //!< zmienna indeksuj ˛aca

unsigned uStatus; //!< wskazanie zapełnienia kolejki

float last_correct[6];

short int overload ; //!< wskazanie przeci ˛a zenia

short int show_no_result; //!< czy komunikat wy swietlony

unsigned Samples_Acquired, Total_Number_of_Samples;

short int uValues[6]; //!< binarne napiecie otrzymane z karty

float sVolt[6]; //!< tablica wartosi podanych w Voltach

float sBias[6]; //!< tablica Bias

public:

short int id;

short int irq_no;

short int szafa_id;

short int index;

char * calfilepath; //!< scie zka do pliku zawieraj ˛acego parametry konfiguracyjne czujnika

short ERROR_CODE;

53


edp_ATI6284_force_sensor(); //!< konstruktor uruchamiaj ˛acy czujnik i ustawiaj ˛acuy bias

virtual ~edp_ATI6284_force_sensor(); //!< destruktor odł ˛aczaj ˛acy karte z magistrali PCI

void configure_sensor (void); //!< konfiguracja czujnika

void wait_for_event(void); //!< oczekiwanie na zdarzenie

void initiate_reading (void); //!< zadanie odczytu od VSP

void get_reading (void); //!< odebranie odczytu od VSP

void terminate (void); //!< rozkaz zakonczenia procesu VSP

//!< Deklaracje funkcji słu z ˛acych do komunikacji i ustawiania karty akwizycji danych

void Configure_Board(void); //!< ustawienie karty

void MSC_Clock_Configure(void); //!< konfiguracja zegarów karty

void AI_Reset_All(void); //!< wykasowanie wszystkich rejestrów

void AI_Board_Personalize(void); //!< ustawienie karty

void AI_Initialize_Configuration_Memory_Output(void); //!< inicjalizacaja rejestrów

void AI_Board_Environmentalize(void); //!< ustawienie parametrów karty

void AI_Trigger_Signals(void); //!< sygnały wyzwalaj ˛ace

void Number_of_Scans(void); //!< ustawienie ilo sci skanów

void AI_Scan_Start(void); //!< start skanowania

void AI_End_of_Scan(void); //!< koniec skanowania

void Convert_Signal(void); //!< konwersja sygnału

void Clear_FIFO(void); //!< wyczyszczenie kolejki FIFO

void AI_Interrupt_Enable(void); //!< uruchomienie przerwa n

void AI_Arming(void); //!< uruchomienie wej s c analogowych karty

void Interrupt_Service_Routine(void); //!< Interrupt service routine

void AI_Start_The_Acquisition(void); //!< start akwizycji

void InitMite(void); //!< Inicjalizacja chipu Mite

void Input_to_Volts(void); //!< przekształcenie wej sciowych danych na wolty

}; //!< end: class vsp_sensor

const struct sigevent * isr_handler (void * area, int id); //!< przerwanie od karty

const struct sigevent * szafa_handler (void * area, int szafa_id); //!< przerwanie od szafy

Listing 2: Klasaedp_ATI6284_force_sensorrealizujaca sterownik nowego czujnika siły.

Funkcje klasy, realizujace komunikacje z czujnikiem siły, zorganizowano w taki sposób, by

mozliwe było łatwe przeniesienie sterownika nowego czujnika siły do procesuVSP. Obecnie

sterownik czujnika siły jest umieszczony w procesieEDP i uruchamiany w watkuforce_thread.

7.3 Funkcje i metody sterownika czujnika siły Gamma FT6284

W tym podrozdziale opisano funkcje i metody, które sa istotne dla działania zaimplemen-

towanego sterownika czujnika Gamma FT6284. Zamieszczono tu metody i funkcje, które sa

przydatne dla uzytkownika przy konstruowaniu sterowników innych czujników firmy ATI oraz

pomagaja w zrozumieniu sposobu działania nowego sterownika czujnika siły.

Funkcje i metody zostały wymienione w porzadku alfabetycznym.

54


Definicja: Calibration *createCalibration(char *CalFilePath,unsigned short index)

Lokalizacja: src/edp/ati6284/ftconfig.cc

Opis: Funkcja z pakietu ATI, wczytujaca plik kalibracyjny czujnika i ustawiajaca wczytane

parametry czujnika.

Rezultat: Utworzenie strukturycal, przechowujacej dane o czujniku, wykorzystywanej w dal-

szych obliczeniach.

Błedy: Funkcja nie zgłasza wyjatków.

——————————————————————————————————————

Definicja: const struct sigevent *isr_handler (void *area, int id)

Opis: Metoda obsługuje przerwanie generowane przez karte akwizycji danych, dotyczace za-

pełnienia kolejki FIFO, w której znajduja sie pomiary.

Rezultat: Metoda zwraca NULL jesli przerwanie nie zostało wygenerowane przez karte, w

przeciwnym razie obsługuje przerwanie i uruchamia dalsze działanie programu.

Błedy: Metoda nie zgłasza wyjatków.

——————————————————————————————————————

Definicja: const struct sigevent *szafa_handler (void *area, int szafa_id)

Lokalizacja: src/edp/ati6284/edp_s.cc

Opis: Metoda obsługuje przerwanie generowane z szafy mikrokomputerowego sterownika ma-

nipulatora. Jesli program działa w trybie testowym, obsługuje przerwanie generowane z

zegara komputera, na którym jest uruchomiony program.

Rezultat: Metoda zwraca NULL jesli przerwanie nie zostało wygenerowane przez karte. W

przeciwnym razie obsługuje przerwanie i uruchamia dalsze działanie programu.

Błedy: Metoda nie zgłasza wyjatków.

——————————————————————————————————————

55


Definicja: edp_ATI6284_force_sensor::edp_ATI6284_force_sensor(void)


Opis: Konstruktor czujnika realizuje wstepna inicjalizacje czujnika, wczytuje plik konfigu-

racyjny czujnikaft6284.cal, podłacza karte do magistrali PCI i ustawia jej parametry.

Po ustawieniu karty konstruktor uruchamia obsługe przerwania generowanego zszafy

w przypadku trybu rzeczywistego lub z zegara komputera, na którym jest uruchomiony

program, w trybie testowym.

Rezultat: Metoda nie zwracazadnej wartosci (jest konstruktrem obiektu).

Błedy: W przypadku braku mozliwosci podłaczenia przerwania od szafy lub od zegara kom-

putera, generowany jest komunikat o błedzie.

Siec działan: Rysunek 7.1 na stronie 69.

——————————————————————————————————————

Definicja: edp_ATI6284_force_sensor::∼edp_ATI6284_force_sensor(void)


Opis: Destruktor odłaczajacy karte z magistrali PCI oraz odmapowujacy pamiec karty. Odła-

czane jest przerwanie generowane przez karte.

Rezultat: Metoda nie zwracazadnej wartosci (jest destruktorem obiektu).

Błedy: Metoda nie zgłasza błedów

——————————————————————————————————————

Definicja: iBus* acquireBoard(const u32 devicePCI_ID)

Lokalizacja: src/edp/ati6284/osiUserCode.cc

Opis: Funkcja inicjalizujaca magistrale PCI, oraz podłaczajaca karte akwizycji danych do ma-

gistrali PCI. Po podłaczeniu karty do magistrali mapowana jest pamiec karty i ustawiane

sa zmienne, niezbedne dla poprawnego działania funkcji z pakietu MH DDK, umozliwia-

jacych konfiguracje karty oraz dostep do rejestrów karty.

Rezultat: W przypadku pomyslnej inicjalizacji karty, zwracana jest strukturabuszawierajaca

dane umozliwiajace ustawianie i odwoływanie sie do karty.

56


Błedy: Funkcja zwraca wyjatki zwiazane z brakiem szukanej karty PCI, błedem mapowania

pamieci karty oraz błedami zwiazanymi z ewentualnie uruchamionymi przerwaniami ge-

nerowanymi przez karte akwizycji danych.

Listing:

iBus * acquireBoard(const u32 devicePCI_ID)

{

u32 devBAR0,BAR0sz,devBAR1,BAR1sz;

iBus * bus;

//!< Znalezienie zakresów pamieci BAR PCI

memset( &info, 0, sizeof( info ) );//!< obsługa przerwania

//!< Podł ˛aczenie do serwera PCI

phdl = pci_attach( 0 );

if( phdl == -1 ) {

fprintf( stderr, "Unable to initialize PCI\n" );

exit( EXIT_FAILURE ) ;

}

#if DEBUG

printf("Program testowy PCI-6034E\n");

#endif

//!< Mapowanie pamieci (BAR) w celu dostepu do pamieci karty

//!< Inicjalizacja struktury pci_dev_info

#if INTERRUPT

memset( &ati6284event, 0, sizeof( ati6284event ) );//!< obsluga przerwania

ati6284event.sigev_notify = SIGEV_INTR;

#endif

pidx = 0;

info.VendorId = 0x1093;

info.DeviceId = 0x2CA0;

//!< doł ˛aczenie sterownika do urz ˛adzenia na PCI

hdl = pci_attach_device( NULL, PCI_INIT_ALL, pidx, &info );

if( hdl == NULL ) {

fprintf( stderr, "Unable to locate NI-6034E\n" );

exit( EXIT_FAILURE ) ;

}

else {

#if DEBUG

printf("connected to NI-6034E\n");

for (int i = 0; i < 6; i++) {

if (info.BaseAddressSize[i] > 0)

printf("Aperture %d: Base 0x%llx Length %d bytes Type %s\n", i,

PCI_IS_MEM(info.CpuBaseAddress[i]) ?PCI_MEM_ADDR(info.CpuBaseAddress[i]) :

PCI_IO_ADDR(info.CpuBaseAddress[i]),

info.BaseAddressSize[i], PCI_IS_MEM(info.CpuBaseAddress[i]) ? "MEM" : "IO");

}

#endif

57


devBAR0=(u32)info.CpuBaseAddress[0];

devBAR1=(u32)info.CpuBaseAddress[1];

mem0 = mmap_device_memory( NULL, info.BaseAddressSize[0], PROT_READ|PROT_WRITE|

PROT_NOCACHE, 0, info.CpuBaseAddress[0] );

mem1 = mmap_device_memory( NULL, info.BaseAddressSize[1], PROT_READ|PROT_WRITE|

PROT_NOCACHE, 0, info.CpuBaseAddress[1] );

if ( mem0 == MAP_FAILED ) {

perror( "mmap_device_memory for physical address 0 failed" );

exit( EXIT_FAILURE );

}

if ( mem1 == MAP_FAILED ) {

perror( "mmap_device_memory for physical address 1 failed" );

exit( EXIT_FAILURE );

}

#if DEBUG

printf("Memory mapped address 0 <0x%llx> of size <%d> CPU <%d> to address of <0x%llx>.\n",

info.PciBaseAddress[0],info.BaseAddressSize[0],info.BaseAddressSize[0], mem0);

printf("Memory mapped address 1 <0x%llx> of size <%d> CPU <%d> to address of <0x%llx>.\n",

info.PciBaseAddress[1],info.BaseAddressSize[1],info.BaseAddressSize[0], mem1);

#endif

}

//!< utworzenie nowego iBus , który wykorzystuje zmapowane adresy pamieci

bus = new iBus(0, 0, mem0, mem1);

bus->_physBar[0] = (u32)devBAR0;

bus->_physBar[1] = (u32)devBAR1;

bus->_physBar[2] = (u32)NULL;




#if DEBUG

printf("InterruptAttach\n");

#endif

#if INTERRUPT

memset( &ati6284event, 0, sizeof( ati6284event ) );///!< obsluga przerwania

SIGEV_INTR_INIT( &ati6284event );

if (( id = InterruptAttach (info.Irq, isr_handler, NULL , NULL , 0))==-1)

printf("Error InterruptAttach\n");

#if DEBUG

else

printf("InterruptAttach OK\n");

#endif

#endif

return bus;

}

Listing 3: FunkcjaiBus* acquireBoard(const u32 devicePCI_ID)uruchamiajaca karte akwizy-

cji danych NI-6034E w systemieQNX.

——————————————————————————————————————

58


Definicja: int ftconvert(float SampleReading[6],float SampleBias[6],float FT[6])

Lokalizacja: src/edp/ati6284/ftconvert.cc

Opis: Funkcja ta ustawia parametry narzedzia, wczytuje bias czujnika, a nastepnie uruchamia

funkcje przeliczajaca dane z postaci woltów na jednostki sił i momentów sił.

Rezultat: Zwracany jest szescioelementowy wektor zawierajacy odczytane pomiary z czujnika

w postaci kolejno trzech sił w osiach X, Y, Z oraz momentów wokół osi X, Y, Z.


——————————————————————————————————————

Definicja: short TTM(Transform xform,float result[6][6],Units ForceUnits,Units TorqueUnits)

Lokalizacja: src/edp/ati6284/ftconfig.cc

Opis: Funkcja z pakietu ATI, wyliczajaca macierz transformacji narzedzia, wykorzystywana

w przypadku przesuniecia i zmiany orientacji narzedzia robota wzgledem koncówki. Siła

i momenty sił sa odpowiednio przeliczone tak, by mierzone były siły i momenty sił na

koncówce narzedzia.

Rezultat: Uzyskiwana jest macierz ze zdefiniowanym narzedziem, która jest wykorzystywana

w dalszych obliczeniach sił i momentów sił.


——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::AI_Arming(void)


Opis: Funkcja z pakietu MH DDK, inicjalizujaca liczniki analogowych wejsc karty.

Rezultat: Uruchomienie liczników wejsc analogowych.


——————————————————————————————————————

59


Definicja: void edp_ATI6284_force_sensor::AI_Board_Personalize(void)


Opis: Funkcja z pakietu MH DDK, umozliwiajaca ustawienie własnych parametrów karty

akwizycji danych. Miedzy innymi sposobu konwersji sygnału i jego przetwarzania.

Rezultat: Karta zostaje skonfigurowana według potrzeb uzytkownika.


——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::AI_End_of_Scan(void)


Opis: Funkcja z pakietu MH DDK, umozliwiajaca wybranie sposobu zakonczenia akwizycji

danych z czujnika siły.

Rezultat: Ustawienie sposobu zakonczenia akwizycji pomiarów.


——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::AI_Interrupt_Enable(void)


Opis: Funkcja z pakietu MH DDK aktywujaca obsługe przerwania generowanego z karty.

Mozliwy jest wybór zdarzenia generujacego przerwanie.

Rezultat: Ustawienie wymaganego sposobu generacji przerwania.


——————————————————————————————————————

60


Definicja: void edp_ATI6284_force_sensor::AI_Scan_Start(void)


Opis: Funkcja z pakietu MH DDK, ustawiajaca sygnał rozpoczecia akwizycji pomiarów prze-

prowadzanych przez karte, dla poprzednio okreslonych wejsc karty oraz ustalonej ilosci

pomiarów.

Rezultat: Ustawienie sposobu rozpoczecia akwizycji danych.


——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::AI_Start_The_Acquisition(void)


Opis: Funkcja z pakietu MH DDK uruchamiajaca akwizycje danych realizowana przez karte.

Rezultat: Karta zbiera pomiary.


——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::AI_Reset_All(void)


Opis: Funkcja z pakiety MH DDK, zatrzymujaca wszystkie czynnosci jakie wykonuje obecnie

karta.

Rezultat: Zastopowanie wszelkich czynnosci wykonywanych przez karte.


——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::Clear_FIFO(void)


Opis: Funkcja z pakietu MH DDK, usuwajaca pomiary z kolejki FIFO.

61


Rezultat: Kolejka FIFO pomiarów wykonanych przez karte akwizycji danych jest pusta.


——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::Configure_Board(void)


Opis: Funkcja z pakietu MH DDK umozliwiajaca ustawienie parametrów wejsc karty akwizy-

cji danych, m. in. wzmocnienia na poszczególnych wejsciach, ustawienia wejsc róznico-

wych oraz sposobu wyrazenia pomiaru wykonanego na danym wejsciu (np.: unipolarne,

bipolarne, czyli ze znakiem lub bez znaku).

Rezultat: Po uruchomieniu tej funkcji uzyskiwane jest ustawienie dwunastu analogowych wejsc

karty akwizycji danych, do których podłaczone sa sygnały. Sygnały te sa sparowane przez

co otrzymywane sa szesc wejsc róznicowych, umozliwiajacych dokładniejszy pomiar.


——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::configure_sensor (void)


Opis: Metoda konfiguruje karte akwizycji danych, a nastepnie dokonuje pojedynczej sekwen-

cji szesciu pomiarów. Pomiary te sa pierwszymi pomiarami z czujnika, które zostaja prze-

tworzone z postaci bitowej na wolty, a nastepnie na siły i momenty sił. Pomiar zostaje

zapisany jako bias czujnika, dzieki czemu zwiekszona zostanie dokładnosc kolejnych po-

miarów.

Rezultat: Otrzymujemy skonfigurowane do odczytów karte i czujnik. Zapisany zostaje bias

niezbedny do wyeliminowania wpływu pozycji czujnika w przestrzeni na pomiar. Je-

sli pomiar jest prawidłowy, generowany jest komunikat o poprawnym skonfigurowaniu

czujnika.

Błedy: W przypadku przeciazenia czujnika podczas pomiaru, generowany jest komunikat o

błedzie i pomiar zostaje powtórzony.


——————————————————————————————————————

62


Definicja: void edp_ATI6284_force_sensor::get_reading (void)


Opis: Szkielet funkcji, niezbedny dla zachowania kompatybilnosci z poprzednim czujnikiem

siły. W sterowniku czujnika Gamma FT6284 funkcja ta nie jest wymagana.


——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::initiate_reading (void)


Opis: Metoda odbiera pomiary zebrane przez karte akwizycji danych, a nastepnie przetwarza

je z postaci bitowej do woltów, i nastepnie do jednostek sił i momentów. Przetworzone

dane sa wysyłane do procesu VSP

Rezultat: Jesli pomiar został wykonany poprawnie zmienna from_vsp.vsp_report przyjmuje

wartosc VSP_REPLY_OK.

Błedy: W przypadku przeciazenia czujnika podczas odczytu, generowany jest komunikat o

przeciazeniu pojawiajacy sie w UI uzytkownika. Do systemu dostarczany jest ostatni po-

prawny pomiar. Po usunieciu przeciazenia generowany jest komunikat o przywróceniu

poprawnego działania czujnika. Metoda zgłasza równiez wyjatki

SENSOR_NOT_CONFIGURED jesli czujnik nie został wczesniej skonfigurowany (pa-

rametr is_sensor_configuredTRUE) oraz READING_NOT_READY jesli parametr

is_reading_readynie został wczesniej ustawiony na TRUE.

Siec działan: Rysunki 7.3 na stronie 71 i 7.4 na stronie 72.

——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::InitMite(void)


Opis: Funkcja z pakietu MH DDK inicjalizujaca układ scalony MITE znajdujacy sie na karcie

akwizycji danych. Ustawiany jest adres fizyczny karty i dostep do rejestrów karty.

Rezultat: Umozliwiony jest dostep do rejestrów karty.

63



——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::Input_to_Volts(void)


Opis: Ta funkcja realizuje przeliczenie pomiarów z postaci bitowej na wolty oraz odpowiednio

je szereguje tak by pierwsze trzy pomiary dotyczyły sił, a kolejne trzy momentów sił

wskazywanych przez czujnik siły w kierunkach X, Y, Z.

Rezultat: Otrzymywany jest szescioelementowy wektorsVolt z danymi wyrazonymi w wol-

tach.


——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::Interrupt_Service_Routine(void)


Opis: Funkcja z pakietu MH DDK, reagujaca na pojawienie sie pomiaru w kolejce FIFO karty.

Rezultat: Do wektorauValueswstawiana jest kolejna wartosc z kolejki FIFO pomiarów z czuj-

nika siły.


——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::MSC_Clock_Configure(void)


Opis: Funkcja z pakietu MH DDK umozliwiajaca konfiguracje opcji czasowych systemu akwi-

zycji danych.

Rezultat: Ustawiany jest czas taktowania karty.


64


——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::Number_of_Scans(void)


Opis: Funkcja z pakietu MH DDK, umozliwiajaca ustawienie ilosci wymaganych pomiarów.

W przypadku zdefiniowania wykorzystania szesciu róznicowych sygnałów analogowych,

w pojedynczym pomiarze ("skanie") zostanie zmierzonych szesc róznicowych wejsc kar-

ty.

Rezultat: Uzyskane zostaje ustawienie karty do wykonania ustalonej ilosci pomiarów.


——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::terminate(void)


Opis: Szkielet funkcji, niezbedny dla zachowania kompatybilnosci z poprzednim czujnikiem

siły. W sterowniku czujnika Gamma FT6284 funkcja ta nie jest wymagana.


——————————————————————————————————————

Definicja: void edp_ATI6284_force_sensor::wait_for_event()


Opis: Inicjalizacja zbierania danych z czujnika. W tej metodzie rozpoczynana jest akwizycja

pomiarów. Nastepnie metoda oczekuje na przerwanie do momentu upłyniecia czasu nie-

zbednego dla zebrania pomiarów. W tym czasie moga sie wykonywac inne czesci systemu

MRROC++.

Rezultat: Po upłynieciu odpowiedniego czasu zmiennais_reading_readyustawiana jest na

TRUE. W przeciwnym razie zmienna ta przyjmuje wartosc FALSE.

Błedy: Metoda zgłasza wyjatek jesli sensor nie został uprzednio skonfigurowany

(SENSOR_NOT_CONFIGURED).

65



——————————————————————————————————————

Definicja: void mmult(float *a, unsigned short ra, unsigned short ca, unsigned short dca, float

*b, unsigned short cb, unsigned short dcb, float *c, unsigned short dcc)

Lokalizacja: src/edp/ati6284/ftrt.cc

Opis: Funkcja z pakietu ATI realizujaca przeliczenie wejsciowych danych w postaci woltów

na jednostki sił i momentów sił. W tej funkcji realizowane jest przeliczenie wczesniej

zdefiniowanych macierzy, zawierajacych informacje o typie czujnika, jednostkach sił,

wymaganych wyjsciowych jednostkach pomiaru oraz o zdefiniowanym narzedziu, przy

uwzglednieniu wektora zawierajacego dane pomiarowe, odczytane z czujnika siły, w wol-

tach.

Rezultat: Wynikiem jest macierz z danymi wyrazonymi w jednostkach sił i momentów sił.


Listing:

void mmult(float * a, unsigned short ra, unsigned short ca, unsigned short dca,

float * b, unsigned short cb, unsigned short dcb,

float * c, unsigned short dcc) {

//!< wymno zenie dwóch macierzy

//!< a - macierz zawieraj ˛aca odpowiednio przetworzone dane o czujniku

//!< b - wektor zawieraj ˛acych pomiary w postaci woltów

//!< c - macierz wynikowa

unsigned short i,j,k;

for (i=0;i<ra;i++)

for (j=0;j<cb;j++) {

c[i * dcc+j]=0;

for (k=0;k<ca;k++)

c[i * dcc+j] = c[i * dcc+j] + a[i * dca+k] * b[k * dcb+j];

}

}

Listing 4: Funkcjavoid mmult(float *a, unsigned short ra, unsigned short ca, unsigned short

dca, float *b, unsigned short cb, unsigned short dcb, float *c, unsigned short dcc)wykonujaca

przeliczenie wejsciowych pomiarów w woltach na jednostki sił i momentów sił.

——————————————————————————————————————

66


Definicja: void releaseBoard(iBus *&bus)

Lokalizacja: src/edp/ati6284/osiUserCode.cc

Opis: Funkcja odmapowywujaca pamiec przydzielona do obsługi karty i zwalniajaca wszelkie

przydzielone wczesniej zasoby systemu. Odłaczane jest przerwanie generowane przez

karte. Karta zostaje odłaczona z serwera PCI.

Rezultat: Karta zostaje odłaczona, a wszelkie przydzielone do obsługi karty zasoby zostaja

zwolnione.

Błedy: Generowane sa wyjatki zwiazane z błedem odmapowywania pamieci przydzielonej kar-

cie.

Listing:

void releaseBoard(iBus * &bus)

{

//!< odmapowanie pamieci i zwolnienie przydzielonych zasobów systemowych

if(munmap_device_memory( mem0, info.BaseAddressSize[0])==-1 )

perror( "munmap_device_memory for physical address 0 failed" );

if(munmap_device_memory( mem1, info.BaseAddressSize[1] )==-1)

perror( "munmap_device_memory for physical address 1 failed" );

#if INTERRUPT

InterruptDetach (id);

#endif

pci_detach_device( hdl ); //!< odlacza driver od danego urzadzenia na PCI

//!< Odł ˛aczenie z serwera PCI.

pci_detach( phdl );

#if DEBUG

printf("Odlaczono uklad PCI-6034E\n");

#endif

delete bus;

}

Listing 5: Funkcjavoid releaseBoard(iBus *&bus)odłaczajaca karte akwizycji danych NI-

6034E w systemieQNX.

——————————————————————————————————————

67


Definicja: void RTConvertToFT(RTCoefs *coefs, float voltages[],float result[],BOOL temp-

comp)

Lokalizacja: src/edp/ati6284/ftrt.cc

Opis: Funkcja z pakietu ATI realizujaca przeliczenie wejsciowych danych w postaci woltów na

jednostki sił i momentów sił. W tej funkcji realizowane jest przemnozenie wczesniej zde-

finiowanych macierzy, zawierajacych informacje o typie czujnika, jednostkach sił, wyma-

ganych wyjsciowych jednostkach pomiaru oraz o zdefiniowanym narzedziu, przez wektor

zawierajacy dane odczytane z czujnika w postaci woltów.

Rezultat: Wynikiem jest szescioelementowy wektor z siłami i momentami sił.


Listing:

void RTConvertToFT(RTCoefs * coefs, float voltages[],float result[],BOOL tempcomp) {

//!< uwzglednij kompensacje temperatury o ile nie jest realizowana przez czujnik

float cvoltages[MAX_GAUGES];

unsigned short i;

for (i=0; i<coefs->NumChannels-1; i++) {

if (tempcomp==TRUE) {

cvoltages[i]=TempComp(coefs,voltages[i],voltages[coefs->NumChannels-1],i)

- coefs->TCbias_vector[i];

} else {

cvoltages[i]=voltages[i]-coefs->bias_vector[i];

}

}

//!< przeliczenie macierzy

mmult( * coefs->working_matrix,coefs->NumAxes,(unsigned short)(coefs->NumChannels-1),MAX_GAUGES,

cvoltages,1,1,

result,1);

}

Listing 6: Funkcjavoid RTConvertToFT(RTCoefs *coefs, float voltages[],float result[],BOOL

tempcomp)przeliczajaca podany szescioelementowy wektor napiec na odpowiadajace im siły i

momenty sił.

——————————————————————————————————————

68


Wczytanie pliku kalibracyjnego czujnika

createCalibration(calfilepath,index)

Ustawienie przerwań

Inicjalizacja karty akwizycji danych

InitMite(); Configure_Board(); MSC_Clock_Configure(); Clear_FIFO();

AI_Reset_All();AI_Board_Personalize();

AI_Initialize_Configuration_Memory_Output(); AI_Board_Environmentalize(); AI_Trigger_Signals();

Number_of_Scans(); AI_Scan_Start();Convert_Signal();

Uzyskanie dostępu do sprzętu

acquireBoard(const u32 devicePCI_ID)

Rysunek 7.1: Siec działan metodyedp_ATI6284_force_sensor::edp_ATI6284_force_sensor(void).

69


Liczba odczytanych pomiarów < 6

Komunikat o błędzie konfiguracji czujnika

show_no_result=1;

Ustawienie karty akwizycji danych

Clear_FIFO(); AI_Interrupt_Enable(); AI_Arming();AI_Start_The_Acquisition();

Interrupt_Wait()

Przeliczenie woltów na jednostki sił i momentów sił

ftconvert(sVolt,sBias,force_torque);

Odczyt zmierzonych danych z rejestru kolejki FIFO karty akwizycji danych

NIE

TAK

czas < START_TO_READ_FAILURE

TAK

NIE

Komunikat o skonfigurowaniu czujnika

show_no_result=0;

Przeliczenie danych z postaci bitowej na wolty

Input_to_Volts(); Clear_FIFO(); Samples_Acquired=0;

Sprawdzenie poprawności pomiaru (brak przeciążenia)

TAK

NIE

InterruptWait(NULL, NULL);

Rysunek 7.2: Siec działan metodyvoid edp_ATI6284_force_sensor::configure_sensor (void).

70


Liczba odczytanych pomiarów < 6

Błąd odczytu czujnikano_result=1;

Invalid_value=1;

Przeliczenie woltów na jednostki sił i momentów sił

ftconvert(sVolt,sBias,force_torque);

Odczyt pomiaru z rejestru kolejki FIFO karty akwizycji danych

NIE

czas < START_TO_READ_FAILURE

TAK

NIE

Poprawny odczyt danychno_result=0;

Invalid_value=0;

Przeliczenie danych z postaci bitowej na wolty

Input_to_Volts(); Clear_FIFO(); Samples_Acquired=0;

TAK

Rysunek 7.3: Siec działan metodyvoid edp_ATI6284_force_sensor::initiate_reading (void),

czesc I.

71


1in

valid

_val

ue =

?

0

from

_vsp

.vsp

_rep

ort=

VS

P_R

EP

LY_O

K;

is_r

eadi

ng_r

eady

=TR

UE

;

no_r

esul

t = ?

over

load

= ?

0

1

Kom

unik

at błę

dush

ow_n

o_re

sult=

1;se

nsor

_sta

tus=

ED

P_F

OR

CE

_SE

NS

OR

_RE

AD

ING

_ER

RO

R;

0

Kom

unik

at błę

duov

erlo

ad=1

;se

nsor

_sta

tus=

ED

P_F

OR

CE

_SE

NS

OR

_OV

ER

LOA

D

show

_no_

resu

lt =

?1

1

0

Kom

unik

atSe

nsor

initi

ate_

read

ing

-wyn

ik

otrz

yman

ysh

ow_n

o_re

sult=

0;

over

load

= ?

Kom

unik

at

Sens

or in

itiat

e_re

adin

g -

OVE

RLO

AD

REM

OVE

Dov

erlo

ad=0

;

Pom

iar p

opra

wny

last

_cor

rect

[i]=f

orce

_tor

que[

i];

0

Pom

iar n

iepo

praw

ny(w

czyt

anie

ost

atni

ego

popr

awne

go)

forc

e_to

rque

[i]=l

ast_

corr

ect[i

];

1

Rys

unek

7.4:

Siec

dzia

łanm

etod

yvo

ide

dp

_A

TI6

28

4_

forc

e_

sen

sor:

:initi

ate

_re

ad

ing

(vo

id)

,cze

scII.

72


Czas < START_TO_READ_TIME_INTERVAL

Ustawienie karty akwizycji danych i rozpoczęcie akwizycji danych

Clear_FIFO(); AI_Interrupt_Enable(); AI_Arming();AI_Start_The_Acquisition();

InterruptWait(NULL, NULL);

NIE

is_reading_ready=TRUE;

TAK

Rysunek 7.5: Siec działan metodyvoid edp_ATI6284_force_sensor::wait_for_event().

73


7.4 Stałe modyfikujace działanie programu

W pliku display.hznajdujacym sie w katalogusrc/edp/ati6284/zdefiniowano stałe, które

słuza do uruchamiania sterownika w trybie do wykrywania błedów.

#define BILLION 1000000000L

#define INFO 0

#define BIAS 0 //wypisanie informacji o biasie

#define WYNIKI 0 //wypisanie informacji o wynikach po srednich

#define DEBUG 0

#define TIME 0 //zmienna wykorzystywana przy mierzeniu czasu

#define WYNIKI_MRROCPP 0 //wypisanie wyników dostarczanych do MRROC++

#define INTERRUPT 0 //wybór sposobu pobierania wyników z karty

#define WITHOUT_INTERRUPT 1

Listing 7: Plik display.h.

74

8 PRZYGOTOWANIA DO REALIZACJI APLIKACJI WIELOROBOTOWEJ

8 Przygotowania do realizacji aplikacji wielorobotowej

8.1 Zmiany konstrukcji robotów

Na poczatku 2006 roku w laboratorium Instytutu Automatyki i Informatyki Stosowanej

przeprowadzono modernizacje robotów, polegajaca na dołaczeniu nowych członów do istnieja-

cych robotów IRp-6. W zwiazku ze zmiana konfiguracji mechanicznej robotów konieczne stało

sie przeprowadzenie modyfikacji w oprogramowaniuMRROC++oraz zmodyfikowanie modeli

kinematyki robotów.

Ponizszy rozdział opisuje prace wykonane w zwiazku z modyfikacjami wprowadzonymi

w laboratorium, stanowisko badawcze, na którym przeprowadzono badania oraz zmiany jakie

wprowadzono w systemieMRROC++ oraz kinematyce piecioosiowej, która została urucho-

miona na zmodyfikowanym robociepostument. Pokazano równiez schematy nowej kinematyki

szescioosiowej, wykorzystywanej w robocieon_track, która została gruntownie przetestowana

i czesciowo zmodyfikowana.

8.2 Schemat stanowiska badawczego

Ponizej, na rysunku 8.1, zaprezentowano schemat stanowiska badawczego, na którym te-

stowano nowa aplikacje wielorobotowa. Poszczególne elementy połaczonych stanowisk robo-

tów IRp-6postumentorazon_trackopisano w rozdziale 3. Nalezy zaznaczyc, ze robot IRp-6

on_trackjest pierwotnie wyposazony w tor jezdny, który w konfiguracji do ponizszej pracy nie

bedzie wykorzystywany. W zwiazku z tym na rysunku 8.1 tor robotaon_tracknie jest uwzgled-

niony. Warto zwrócic uwage na zaznaczone nowe dołaczone elementy manipulatora i zmiane

umieszczenia czujników w stosunku do konfiguracji opisanych w rozdziale 3.

75


sieć

eth

erne

tm

anip

ulat

or

kom

pute

r op

erat

ora

syst

emu

mik

roko

mpu

tero

wy

ster

owni

k m

anip

ulat

ora

czuj

nik

siły

końc

ówka

m

anip

ulat

ora

(kiść)

końc

ówka

m

anip

ulat

ora

(kiść)

czuj

nik

siły

kom

pute

r ste

rują

cy

robo

tem

kom

pute

r ste

rują

cy

robo

tem

mik

roko

mpu

tero

wy

ster

owni

k m

anip

ulat

ora

mik

roko

mpu

tero

wy

ster

owni

k cz

ujni

ka s

iły

sieć

eth

erne

tm

anip

ulat

or

sieć

eth

erne

t

sprzęg

robo

ta

sprzęg

czu

jnik

a

sprzęg

robo

ta

kart

a w

ejść

-w

yjść

prze

twor

nik

Rys

unek

8.1:

Sta

now

isko

doba

dani

aap

likac

jiw

ielo

robo

tow

ejz

dwom

aro

bota

miI

Rp-

6.K

olor

emza

znac

zono

now

ero

zwia

zani

e

76


8.3 Zmiany zwiazane z modelem kinematyki robotów

W tym podrozdziale zostana opisane dwie kinematyki wykorzystane przy realizacji apli-

kacji dwurobotowej oraz dodane elementy obsługi narzedzia osiowosymetrycznego, o które

rozbudowano systemMRROC++.

Robot IRp-6on_trackwykorzystuje kinematyke szescioosiowa, natomiast robotpostument

wykorzystuje kinematyke piecioosiowa dla narzedzia osiowosymetrycznego.

W trakcie pracy istotnie zmodyfikowano sposób zadawania i uruchamiania kinematyki w

systemieMRROC++. Poczatkowo przełaczenie kinematyki robotapostumentz piecioosiowej

na kinematyke szescioosiowa realizowano poprzez modyfikacje jednej linii w plikuMakefile

dotyczacym robotaon_tracklub postument.

W wyniku modyfikacji uruchomiono obsługe zmiany kinematyki wewnatrz systemu

MRROC++ poprzez wysłanie odpowiedniej instrukcji z generatora trajektorii z poziomuMP

lub ECP. Nowy sposób przełaczania kinematyki został zaimplementowany zarówno dla robota

postumentjak i robotaon_track. Dla robotaon_trackmozliwy jest wybór pomiedzy:

• kinematyka szescioosiowa bez toru jezdnego, z obsługa nowodołaczonego członu i chwy-

taka;

• poprzednia kinematyka szescioosiowa z obsługa toru jezdnego bez obsługi nowodołaczo-

nego stopnia swobody i chwytaka.

Dla robotapostumentmozliwy jest wybór pomiedzy:

• kinematyka szescioosiowa obsługujaca nowa konfiguracje robota z szóstym stopniem

swobody i chwytakiem;

• kinematyka piecioosiowa wykorzystujaca narzedzie osiowosymetryczne, nie uwzglednia-

jaca nowodołaczonego szóstego stopnia swobody i chwytaka.

W trakcie inicjalizacji systemuMRROC++ domyslnie ładowana jest kinematyka szescio-

osiowa bez toru jezdnego dla robotaon_trackoraz kinematyka szescioosiowa dla robotapostu-

ment. Zmiana wykorzystywanej kinematyki jest mozliwa w generatorze na poziomieMP lub

ECP.

Dzieki powyzej opisanym modyfikacjom znacznie uproszczono i uporzadkowano hierarchie

plików zwiazanych z modelami kinematyki w systemieMRROC++. Obecnie modele kinema-

tyk dla robotaon_tracksa zdefiniowane w plikuirp6_kin_ot.cc, a dla robotapostumentw pliku

irp6_kin_p.cc. Do wyboru wykorzystywanej kinematyki słuzy parametrkinematic_model, któ-

ry domyslnie przyjmuje wartosc 0 i wczytuje domyslne, szescioosiowe modele kinematyk.

W plikach nagłówkowychirp6.h (róznych dla obu robotów), dołaczanych do plików zawie-

rajacych modele kinematyk, sa zdefiniowane parametryGEOM_SYNCH_POS_MODEL_*_*

77


modyfikujace i kalibrujace kinematyki.

W zwiazku z modernizacja robotów na poczatku 2006 roku nalezało zmodyfikowac po-

przednia wersje kinematyki piecioosiowej oraz sprawdzic nowa kinematyke szescioosiowa.

Konieczne było dodanie obsługi nowych elementów manipulatorów dołaczonych do robotów

IRp-6 postumenti on_track. Elementami dodanymi do kazdego z robotów sa jeden dodatkowy

stopien swobody oraz chwytak (gripper). Parametry nowych członów, pomimo iz nie sa bez-

posrednio wykorzystywane w kinematyce piecioosiowej do wyliczania prostego i odwrotnego

zadania kinematyki, sa niezbedne do prawidłowego działania robota jako całosci.

8.3.1 Kinematyka robotów

Manipulator składa sie z ogniw (członów) sztywnych połaczonych przegubami. Ogniwa

połaczone przegubami tworza strukture kinematyczna.

Zagadnienie kinematyki robotów sprowadza sie do odpowiedniego obliczenia wymaganego

połozenia (a wiec ustawienia silników poruszajacych poszczególnymi członami robota) tak, by

uzyskac zadana orientacje koncówki robota w przestrzeni.

Rozróznia sie dwa zadania kinematyki:

proste – sprowadzajace sie do obliczenia, na podstawie podanych katów pomiedzy poszczegól-

nymi członami oraz długosci poszczególnych członów, pozycji koncówki robota wzgle-

dem układu bazowego,

odwrotnego – polegajacego na obliczeniu, przy zadanej pozycji koncówki robota w przestrzeni

3D, takiego ułozenia poszczególnych członów robota, by koncówka osiagneła zadana po-

zycje. Zagadnienie to jest znacznie trudniejsze, dla szeregowych łancuchów kinematycz-

nych jakimi sa wykorzystywane w labolatorium manipulatory, od prostego zadania kine-

matyki, poniewaz wymaga rozpatrzenia wielu mozliwosci osiagniecia zadanego punktu w

przestrzeni trójwymiarowej przez koncówke robota, jednoczesnie uwzgledniajac obszar

roboczy robota, jego ogranicznia i połozenia osobliwe.

Istnieje wiele sposobów zadawania orientacji koncówki robota lub narzedzia w przestrzeni

trójwymiarowej. Zagadnienia kinematyki oraz notacje okreslania orientacji w przestrzeni zo-

stały szczegółowo opisane w [1].

8.3.2 Kinematyka piecioosiowa

Zaimplementowana przed modernizacja robotów w laboratorium Instytutu Automatyki i In-

formatyki Stosowanej kinematyka dla robota o pieciu stopniach swobody wymagała przebudo-

wania w zwiazku z nowa konfiguracja robotów. Informacje na temat kinematyki piecioosiowej

zaczerpnieto z pozycji [9]. Nastepnie rozpoczeto uaktualnianie starej kinematyki piecioosiowej,

78


tak by mogła działac na zmodyfikowanym robocie IRp-6postument.

W załozeniu ponizszej pracy, robotpostumentma nadal działac według kinematyki piecio-

osiowej, ale dołaczone elementy manipulatora musza zostac odpowiednio obsłuzone, poniewaz

istnieje mozliwosc zmiany ich połozenia niezaleznie od kinematyki piecioosiowej np.: poprzez

interfejs uzytkownikaUI.

W zwiazku z tym dokonano modyfikacji m.in. funkcjicheck_jointsoraz dodano przepisy-

wanie aktualnych pozycji nowodołaczonych elementów manipulatora.

Prace nad modyfikacja kinematyki zakonczone zostały badaniami, które potwierdziły pra-

widłowe dołaczenie zmodyfikowanej kinematyki piecioosiowej do systemuMRROC++. Na ry-

sunku 8.2 pokazany został schemat przedstawiajacy robota IRp-6postumentz poszczególnymi

układami odniesienia, w szczególnosci z układem współrzednych umiejscowionym w kołnie-

rzu, którego poznanie jest istotne dla poprawnego zdefiniowania orientacji narzedzia osiowosy-

metrycznego, które jest opisane w rozdziale 8.3.4.

1θ

1d

2a

3a

x0

x1

x3

x2

x5

x4

x6

y3

y2

y4

z0

z1

z5

z6

2θ

5θ

4θ

3θ

3θ′

4θ ′

Rysunek 8.2: Robot IRp-6postumentw ujeciu kinematyki piecioosiowej.

79


8.3.3 Kinematyka szescioosiowa

W zwiazku z dołaczeniem nowego stopnia swobody zwiazanego z chwytakiem, konieczne

stało sie opracowanie nowej kinematyki szescioosiowej. Niniejsza praca wykorzystuje nowa

kinematyke szescioosiowa, której postac została szczegółowo opisana w oddzielnej pracy.

Kinematyka szescioosiowa jest wykorzystywana w robocieon_track. Ponizej na rysunku

8.3 zamieszczono schemat wykorzystywanej kinematyki szescioosiowej, a na rysunku 8.4 za-

mieszczono powiekszony schemat koncówki robota z zaimplementowana kinematyka szescio-

osiowa, która w pełni wykorzystuje nowe elementy dołaczone do robota IRp-6 po przeprowa-

donej modernizacji.

80


1θ1d

2a

3a

x0

x1

x3

x2

x5

x4

x6

y3

y4

z0

z1

z5

z6

2θ

5θ

4θ

3θ

3θ′

4θ′'

5x

5d

6θ

z7

x7

y2

Rys

unek

8.3:

Rob

otIR

p-6on

_tr

ack

wuj

eciu

kine

mat

ykis

zesci

oosi

owej

zdo

łacz

onym

now

ymst

opni

emsw

obod

yic

hwyt

akie

m.

81


3a

x

x5

x4

x6

y4

z5

z6

5θ

4θ ′'5 x5d

6θ

z7

x7

Rysunek 8.4: Robot IRp-6on_trackw ujeciu kinematyki szescioosiowej – koncówka robota.

Zrozumienie schematu przedstawiajacego układy odniesienia robota IRp-6on_trackw uje-

ciu nowej kinematyki szescioosiowej jest istotne dla poprawnego zdefiniowania orientacji na-

rzedzia osiowosymetrycznego, które jest opisane w rozdziale 8.3.4.

8.3.4 Narzedzie osiowosymetryczne

W zwiazku z planowanym wykorzystaniem definicji narzedzia osiowosymetrycznego w

zmodyfikowanej kinematyce piecioosiowej oraz nowej kinematyce szescioosiowej, dodano ob-

sługe zadawania parametrów takiego narzedzia w notacjiTOOL_XYZ_ANGLE_AXISorazTO-

OL_AS_XYZ_EULER_ZY. Na rysunkach 8.5 oraz 8.6 pokazano istote wymienionych dwóch

sposobów zadawania orientacji narzedzia w przestrzeni. Wektor pokazany przerywana linia

wskazuje kirunek orientacji narzedzia zakonczonego w punkcie przesunietym o wektor Q wzgle-

dem kołnierza robota.

82


X r

Y r

Z rko

ńców

ka ro

bota

Q =

[Qx,

Qy,

Qz]T

narzęd

zie

V x

V y

V z

V =

[Vx,

V y, V

z]T

Rys

unek

8.5:

Nar

zedz

iezd

efini

owan

ew

nota

cji

TO

OL

_X

YZ

_A

NG

LE

_A

XIS.

83


r

r

r

xy

zT

r’

r’

r’

ϕψ

r

r

rr’’

r’’

r’’

xy

zT

Rys

unek

8.6:

Nar

zedz

iezd

efini

owan

ew

nota

cji

TO

OL

_A

S_

XY

Z_

EU

LE

R_

ZY

.

84


W przypadku wykorzystywanej kinematyki piecioosiowej dla robotapostument, piec stopni

swobody oznacza,ze pełne mozliwosci maipulacyjne (tzn. osiagniecie dowolnej pozycji zada-

nej w przestrzeni roboczej) moga byc osiagniete jedynie przez narzedzia osiowosymetryczne.

W przypadku innych narzedzi, kazda zadana pozycja jest nierealizowalna z prawdopodobien-

stwem zblizonym do jednosci.

Definiowanie narzedzi, tzn. okreslanie pozycji koncówki roboczej narzedzia w układzie

współrzednych kołnierza, jest mało intuicyjne. Najbardziej przejrzystym sposobem zadawa-

nia orientacji narzedzia jest notacja TOOL_XYZ_ANGLE_AXIS, w której szesc elementów

definiujacych połozenie koncówki narzedzia oznacza odpowiednio:

pierwsze trzyQ = [Qx, Qy, Qz]T – połozenie koncówki w układzie kołnierza,

kolejne trzy V = [Vx, Vy, Vz]T – wektor (przeliczany nastepnie do wersora) okreslajacy kieru-

nek i zwrot osi narzedzia w układzie kołnierza.

W zaleznosci od wykorzystywanej kinematyki mozliwe jest równiez obliczenie, na podstawie

otrzymanych parametrów, kata obrotuα wokół osi narzedzia. W przypadku kinematyki pie-

cioosiowej kat obrotuα nie jest obliczany, ale jest zmienny w czasie ruchu narzedzia. Jest to

wynikiem niewystarczajacej ilosci stopni swobody (5). Dlatego własnie dla manipulatorów z

piecioma osiami swobody tylko narzedzie osiowosymetryczne daje pełne mozliwosci manipu-

lacyjne.

W przypadku zadawania pozycji narzedzia w postaciTOOL_AS_XYZ_EULER_ZYwyma-

gane jest podanie pieciu parametrów:

pierwsze trzyQ = [Qx, Qy, Qz]T – okreslajacych połozenie koncówki w układzie kołnierza,

kat nutacji φ – okreslajacy obrót wokół osiZ, miedzyX, aX ′,

kat precesji ψ – okreslajacy obrót wokół osiY ′, miedzyX ′, aX ′′.

Na podstawie podanych parametrów kata nutacji i precesji, w wyniku odpowiednich przeliczen,

otrzymywany jest wersor osi narzedziaV . WersorV wraz z wektorem Q przesuniecia konców-

ki narzedzia wzgledem koncówki robota, opisuja połozenie narzedzia osiowosymetrycznego

wzgledem koncówki robota.Ten sposób zadawania narzedzia osiowosymetrycznego jest jednak

znacznie trudniejszy od poprzednio opisanego.

Po zdefiniowaniu nowych sposobów definicji narzedzi dodano obsługe zadawania parame-

trów narzedzi poprzez interfejs uzytkownikaUI. Dzieki temu, mozliwe stało sie przetestowanie

działania nowych kinematyk wraz z zadanymi narzedziami bez uruchamiania robotów w trybie

rzeczywistym. Dodana mozliwosc okazała sie bardzo przydatna i wydatnie pomogła w analizie

w poczatkowej, testowej fazie badania działania robotów.

85


Koncówka narzedzia powinna byc zadawana w odniesieniu do układu współrzednych, umiesz-

czonego w koncówce ramienia robota. W przypadku robotapostumentw konfiguracji z piecio-

ma stopniami swobody jest to kisc, natomiast dla robotaon_trackz szescioma stopniami swo-

body (bez toru) jest to punkt poczatku układu współrzednychX6Z6, pokazanego na rysunku

8.3.

Układ współrzednych do definicji narzedzia zwiazany z koncówka dla robotapostumentw

kinematyce piecioosiowej rózni sie od układu współrzednych dla robotaon_trackw kinematyce

szescioosiowej. Nalezy o tym pamietac przy definicji narzedzia w odpowiedniej kinematyce.

8.3.5 Kod funkcji realizujacych przeliczanie narzedzia w notacji

TOOL_XYZ_ANGLE_AXISoraz TOOL_AS_XYZ_EULER_ZY

W tym podrozdziale zamieszczono kod funkcji realizujacych przeliczanie zadanej orientacji

w postaciTOOL_XYZ_ANGLE_AXISorazTOOL_AS_XYZ_EULER_ZYdo postaci trójscianu

uzywanego w systemieMRROC++ i w przeciwna strone.

Funkcje te znajduja sie w plikuedp_irp6s.ccw katalogu../mrrocpp/src/edp/irp6s.

Funkcja command_buffer::tool_xyz_aa_2_frame

Funkcja przeliczajaca polecenie z postaci XYZ_ANGLE_AXIS do postaci trójscianu uzy-

wanego wewnatrz systemuMRROC++.

/ * -------------------------------------------------------------------------- * /

void command_buffer::tool_xyz_aa_2_frame (c_buffer * instruction) {

// Przekształcenie definicji narzedzia z postaci

// TOOL_XYZ_ANGLE_AXIS do postaci TOOL_FRAME oraz przepisanie wyniku

// przekształcenia do wewnetrznych struktur danych

// TRANSFORMATORa

double alfa; // k ˛at obrotu

double x, y, z; // wspołrzedne wektora przesuniecia

double kx, ky, kz; // specyfikacja wektora, wokół ktorego obracany jest układ

// przepisanie z tablicy pakietu komunikacyjnego

x = ( * instruction).rmodel.tool_coordinate_def.tool_coordinates[0];

y = ( * instruction).rmodel.tool_coordinate_def.tool_coordinates[1];

z = ( * instruction).rmodel.tool_coordinate_def.tool_coordinates[2];

// przepisanie warto sci pomno zone s ˛a przez k ˛at alfa

kx = ( * instruction).rmodel.tool_coordinate_def.tool_coordinates[3];

ky = ( * instruction).rmodel.tool_coordinate_def.tool_coordinates[4];

kz = ( * instruction).rmodel.tool_coordinate_def.tool_coordinates[5];

// obliczenie k ˛ata obrotu alfa i warto sci funkcji trygonometrycznych

alfa = sqrt(kx * kx + ky * ky + kz * kz);

// korekta wartosci x, y, z

if((alfa < dalfa) && (alfa > -dalfa))

{

86


homog_matrix A_B_T(x, y, z);

A_B_T.to_table(tool); // przepisanie uzyskanego wyniku do transformera

}

else

{

kx = kx/alfa;

ky = ky/alfa;

kz = kz/alfa;

homog_matrix A_B_T(kx, ky, kz, alfa, x, y, z);

A_B_T.to_table(tool); // przepisanie uzyskanego wyniku do transformera

}

// sprawdzenie poprawno sci macierzy

homog_matrix A_B_TOOL (tool);

if (!(A_B_TOOL.is_valid())) {

// bład: niewła sciwy format macierzy jednorodnej

// ustawi numer błedu

throw NonFatal_error_2(INVALID_HOMOGENEOUS_MATRIX);

}; // end: if

}; // end: command_buffer::tool_xyz_aa_2_frame

/ * -------------------------------------------------------------------------- * /

Listing 8: Funkcjatool_xyz_aa_2_framerealizujaca przeliczenie współrzednych narzedzia z

postaciTOOL_XYZ_ANGLE_AXISdo trójscianu.

—————————————————————————————————————————————————————————

Funkcja reply_buffer::tool_frame_2_xyz_aa

Funkcja przygotowujaca odpowiedz w postaci XYZ_ANGLE_AXIS na podstawie trójscia-

nu uzywanego wewnatrz systemuMRROC++.

/ * -------------------------------------------------------------------------- * /

void reply_buffer::tool_frame_2_xyz_aa (void) {


// TOOL_FRAME do postaci TOOL_XYZ_ANGLE_AXIS oraz przepisanie wyniku


// REPLY_BUFFER

double xyz_aa[6]; // tablica z wynikiem przekształcenia do rozkazu w formie XYZ_ANGLE_AXIS

homog_matrix A(tool);

A.to_xyz_aa(xyz_aa);

reply.rmodel_type = TOOL_XYZ_ANGLE_AXIS;

switch (reply.reply_type) {

case RMODEL:

case RMODEL_INPUTS:

case ARM_RMODEL:

case ARM_RMODEL_INPUTS:

for (int i = 0; i < 6; i++)

reply.rmodel.tool_coordinate_def.tool_coordinates[i] = xyz_aa[i];

87


break;

default: // bład: z reply_type wynika, ze odpowiedz nie zawiera narzedzia

throw NonFatal_error_2(ERROR_IN_RMODEL_REQUEST); // ustawi numer błedu

} // end: switch (reply_type)

}; // end: reply_buffer::tool_frame_2_xyz_aa

/ * -------------------------------------------------------------------------- * /

Listing 9: Funkcjatool_frame_2_xyz_aarealizujaca przeliczenie współrzednych narzedzia z

postaci trójscianu doTOOL_XYZ_ANGLE_AXIS.

—————————————————————————————————————————————————————————

Funkcja command_buffer::tool_axially_symmetrical_xyz_eul_zy_2_frame

Funkcja przeliczajaca polecenie z postaci XYZ_EULER_ZY do postaci trójscianu uzywa-

nego wewnatrz systemuMRROC++.

/ * -------------------------------------------------------------------------- * /

void command_buffer::tool_axially_symmetrical_xyz_eul_zy_2_frame (c_buffer * instruction) {


// TOOL_AS_XYZ_EULER_ZY do postaci TOOL_FRAME oraz przepisanie wyniku


// TRANSFORMATORa

double * t;

t = &( * instruction).rmodel.tool_coordinate_def.tool_coordinates[0];

double psi, fi; //wspolrzedne wektora przesuniecia

double v6[3];

psi = t[3];

fi = t[4];

// void Euler_to_Vector( double fi, double psi, double v[3])

v6[0] = cos(psi) * cos(fi);

v6[1] = cos(psi) * sin(fi);

v6[2] = -sin(psi);

double alfa; //k ˛at obrotu

double x, y, z; //wspołrzedne wektora przesuniecia

double kx, ky, kz; //specyfikacja wektora, wokól ktorego obracany jest układ

homog_matrix G_R_T;

//pobranie aktualnej macierzy przekształcenia

homog_matrix G_K_T(tool);

//przepisanie z tablicy pakietu komunikacyjnego

x = * t;

y = * (t+1);

z = * (t+2);

//przepisane warto sci pomno zone s ˛a przez kat alfa

88


kx = v6[0];

ky = v6[1];

kz = v6[2];

//obliczenie k ˛ata obrotu alfa i warto sci funkcji trygonometrycznych

alfa = sqrt(kx * kx + ky * ky + kz * kz);

//korekta wartosci x, y, z

if((alfa < dalfa) && (alfa > -dalfa))

{

homog_matrix K_R_T(x, y, z);

//obliczenie macierzy przekształcenia

G_R_T = G_K_T * K_R_T;

}

else

{

kx = kx/alfa;

ky = ky/alfa;

kz = kz/alfa;

homog_matrix K_R_T(kx, ky, kz, alfa, x, y, z);

//obliczenie macierzy przekształcenia

G_R_T = G_K_T * K_R_T;

}

G_R_T.to_table(tool);

}; // end: command_buffer::tool_axially_symmetrical_xyz_eul_zy_2_frame

/ * -------------------------------------------------------------------------- * /

Listing 10: Funkcjatool_axially_symmetrical_xyz_eul_zy_2_framerealizujaca przeliczenie

współrzednych narzedzia z postaciTOOL_AS_XYZ_EULER_ZYdo ramki .

—————————————————————————————————————————————————————————

Funkcja reply_buffer::tool_axially_symmetrical_frame_2_xyz_eul_zy

Funkcja przygotowujaca odpowiedz w postaci XYZ_EULER_ZY na podstawie trójscianu

uzywanego wewnatrz systemuMRROC++.

/ * -------------------------------------------------------------------------- * /

void reply_buffer::tool_axially_symmetrical_frame_2_xyz_eul_zy (void) {


// TOOL_FRAME do postaci TOOL_AS_XYZ_EULER_ZY oraz przepisanie wyniku


// REPLY_BUFFER

double psi, fi;

double v6[3],q6[3];

double xyz_aa[6];

homog_matrix A(tool);

A.to_xyz_aa(xyz_aa);

q6[0] =xyz_aa[0];

89


q6[1] = xyz_aa[1];

q6[2] = xyz_aa[2];

v6[0] = xyz_aa[3];

v6[1] = xyz_aa[4];

v6[2] = xyz_aa[5];

#define EPS_V 1.0e-6

if ( fabs(v6[0]) > EPS_V)

{

fi = atan2(v6[1],v6[0]);

psi = atan2(-v6[2], sqrt(v6[0] * v6[0]+v6[1] * v6[1]));

}

else

if (fabs(v6[1]) >EPS_V)

{

psi = atan2(-v6[2], fabs(v6[1]));

if (v6[1] > 0)

fi = M_PI/2;

else

fi = -M_PI/2;

}

else

{

fi = 0; / * Dowolna wartož T k†ta fi, poniewa§ Vx = Vy = 0 * /

if (v6[2] > 0)

psi = -M_PI/2;

else

psi = M_PI/2;

}

reply.rmodel_type = TOOL_AS_XYZ_EULER_ZY;

reply.rmodel.tool_coordinate_def.tool_coordinates[0]=q6[0];



reply.rmodel.tool_coordinate_def.tool_coordinates[3]= fi;

reply.rmodel.tool_coordinate_def.tool_coordinates[4]= psi;

reply.rmodel.tool_coordinate_def.tool_coordinates[5]= 0;

}; // end: reply_buffer::tool_axially_symmetrical_frame_2_xyz_eul_zyz

/ * -------------------------------------------------------------------------- * /

Listing 11: Funkcjatool_axially_symmetrical_xyz_eul_zy_2_framerealizujaca przeliczenie

współrzednych narzedzia z postaci trójscianu doTOOL_AS_XYZ_EULER_ZY.

—————————————————————————————————————————————————————————

90

9 IMPLEMENTACJA APLIKACJI WIELOROBOTOWEJ

9 Implementacja aplikacji wielorobotowej

W tym rozdziale opisano implementacje nowej aplikacji wielorobotwej dla nowej konfigu-

racji robotów, realizujacej zadanie rysowania przez dwa roboty IRp-6, znajdujace sie w labo-

ratorium IAiIS. Badania oraz efekt prac zostana zaprezentowane w kolejnym rozdziale. Nowa

aplikacja wielorobotowa wykorzystuje generatory, które istniały w systemieMRROC++, ale

wymagały modyfikacji w zwiazku ze zmiana konfiguracji robotów, oraz nowoutworzony gene-

ratormp_gen_kd_generator..

9.1 Generator trajektorii w systemie MRROC++

W systemieMRROC++ ruch robota realizowany jest przy uzyciu dwóchinstrukcji rucho-

wych(9.1), które powoduja przemieszczanie efektorów.

Rysunek 9.1: Instrukcje ruchu systemu MRROC++ ([2]).

Instrukcja Move – odpowiada za sterowanie ruchem oraz monitorowanie warunku koncowe-

go,

Instrukcja Wait – monitoruje warunek poczatkowy.

91


Generator trajektorii jest ostatnim argumentem proceduryMove. Okresla on warunek kon-

cowy oraz sposób sterowania ruchem efektorów. Spełnienie warunku koncowego jest równo-

znaczne z wykonaniem instrukcji i powoduje przerwanie ruchu.

Przemieszczenie z punktu startowego do docelowego zostało zdefiniowane w systemie

MRROC++ jako sekwencjamakrokroków. Makrokrok to pojedyncze zlecenie ruchu dla ste-

rownika robota. Zawiera sie w nim pojedynczyprzedział interpolacji. W jednym makrokroku

nastepuje przejscie z jednego wezła interpolacji do nastepnego.Przedział interpolacjipodzie-

lony jest nakroki ruchu, które sa przyrostami połozenia ramienia robota, realizowanymi w

pojedynczym okresie próbkowania, który dla obecnego okresu pracy algorytmu regulacji wy-

nosi 2 ms. W pojedynczym kroku ruchu odbywa sie prosta liniowa interpolacja dla kazdego

stopnia swobody. W kolejnych makrokrokach interpolacje miedzy wezłami realizuje konkret-

ny generator ruchu. Na rysunku 9.2 pokazane zostały kroki, makrokroki oraz sposób realizacji

makrokroku zadanego w procesieMP.

92


Proces MP

generator.first_stepgenerator.next_step

Proces ECP

generator.first_stepgenerator.next_step

Proces EDP

Servo_group

Realizacja makrokroku węzeł interpolacji

makrokrok = przedział interpolacji

Zadany przyrost położenia / orientacji pomiędzy dwoma punktami węzłowymi

kroki makrokroku

Rysunek 9.2: Makrokroki i kroki ruchu w systemie MRROC++

Utworzenie nowego generatora wymaga zdefiniowania nowej klasy, pochodnej od klasy ba-

zowej (procesyMP lub ECP) i napisania dwóch jej metod :first_steporaz next_step. Dwie

rózne procedury zostały wprowadzone ze wzgledu na róznice pomiedzy obliczeniami w pierw-

szym i kolejnych krokach realizacji trajektorii. Obie metody moga zlecac dowolne polecenia

do realizacji przez sterownik efektora – procesEDP. Sa to zlecenia ruchu lub odczytu aktu-

alnego połozenia ramienia w dowolnej reprezentacji, zmiany parametrów lub korekty modelu

kinematycznego, zmiany stosowanych algorytmów regulacji lub przełaczenia wyjsc i odczytu

wejsc binarnych. Ogólny schemat realizacji ruchu efektorów w systemieMRROC++pokazano

na rysunku 9.3.

93


Proces MP

instrukcje ruchowe Move( , , )

Wait( , , )

Wątek EDP_Servoj

generator trajektorii na first_step poziomie MP next_step

wewnętrzne strukturydanych

Proces ECPj

instrukcje ruchowe Move( , , )

Wait( , , )

generator trajektorii na first_step poziomie ECP next_step


Proces EDPj


proste i odwrotne zagadnienie kinematyki

obsługa serwomechanizmu danej osi

polecenia ECPj dla EDPj

polecenia MP dla ECPj odpowiedzi ECPj dla MP

polecenia EDPj dla ECPj

Proces VSPl

polecenie MP dla czujników wirtualnych

Odczyty czujników wirtualnych

Proces VSPl

polecenie ECP dla czujników wirtualnych

Odczyty czujników wirtualnych

Rysunek 9.3: Schemat realizacji ruchu efektorów w systemie MRROC++.

W przypadku wykorzystywania generatorów na poziomieMP, na poziomieECP urucha-

miany jest generator transparentny. Jego rola sprowadza sie do przekazywania polecen z proce-

suMP do znajdujacych sie nizej w hierarchii procesówEDP.

Szczegółowe informacje na temat generatorów i ich implementacji mozna znalezc w [10].

9.2 Implementacja prostego generatora trajektorii wykonujacego ruch

pozycyjno-siłowy

Po zakonczonym sukcesem dołaczeniu nowego czujnika siły do systemu MRROC++, roz-

poczeto prace nad generatorem trajektorii (dla pojedynczego robota), który wykorzystywałby

nowy czujnik i pomógł w sprawdzeniu działania czujnika.

Generator ten realizuje siłowy ruch w dół, rysowanie kwadratu, a nastepnie ponowny ruch

w góre. Generator ten był "wprawka" przed napisaniem aplikacji dwurobotwej, która pokaza-

94


ła poprawnosc działania czujnika siły oraz mozliwosci wykorzystania kinematyki dla robota z

zamontowanym narzedziem osiowosymetrycznym.

9.3 Aplikacja wielorobotowa

W tym podrozdziale opisano implementacje zmodyfikowanego generatora trajektorii na po-

ziomieMP, wykorzystujacego informacje otrzymywane zarówno z czujnika FT3084 oraz no-

wego czujnika siły Gamma FT6284.

Poprzez zaimplementowanie tego generatora sprawdzono poprawnosc działania nowego

czujnika siły i przetestowano rozległe zmiany wprowadzone w systemieMRROC++ po mo-

dyfikacji konfiguracji manipulatorów, znajdujacych sie w laboratorium Instytutu Automatyki i

Informatyki Stosowanej, opisanych w rozdziale 3.1.

Aplikacja wielorobotowa, sterujaca dwoma manipulatorami IRp-6, wykorzystuje dwie ki-

nematyki:piecioosiowa(z załozenia wykorzystywana dla narzedzia osiowosymetrycznego) dla

robotapostumenti nowa kinematykeszescioosiowadla robotaon_track (bez wykorzystania

toru jezdnego).

9.3.1 Wykorzystywane generatory

W systemieMRROC++ jest zdefiniowana klasa bazowa (klasa abstrakcyjna)generator, dla

generatorów trajektorii. Wszystkie generatory dla procesuMP, wykorzystywane przez instruk-

cje Move, musza byc wywiedzione z tej klasy. Słuzy ona do wyznaczenia nastepnej wartosci

zadanej oraz sprawdzenia spełnienia warunku koncowego. W programie uzytkowym procesu

MP metody konkretnego generatora sprawdzaja warunek koncowy, na podstawie informacji,

która zawarta jest w liscie czujników i robotów. Jezeli nie bedzie on spełniony, to wygeneru-

ja nowe wartosci zadane, które zapisane zostana w wewnetrznych strukturach danych robotów

konkretnych (obrazów robotów), umieszczonych na liscie. W przypadkuscisłej współpracy ro-

botów, wystepujacej w opisanej w tym podrozdziale aplikacji wielorobotowej, generator na

poziomieMP, poza pobudzaniem do działania generatorów na poziomieECP, ma obowiazek

obliczenia kolejnych wartosci zadanych dla wszystkich robotów bioracych udział w ruchu. W

tym przypadku, generatory na poziomieECP ograniczaja sie jedynie do przekazywania ode-

branych odMP przesyłek do procesówEDP.

Główna czescia kazdego generatora sa jego dwie metodyfirst_stepi next_step. Wprowadzo-

ne zostały dwie rózne procedury, poniewaz obliczenia w pierwszym kroku realizacji trajektorii

sa zazwyczaj rózne od tych, wykonywanych w kazdym nastepnym kroku. Obie metody moga

zlecac dowolne polecenia dlaECP, który przekazuje je procesowiEDP. Metodafirst_stepge-

neruje pierwszy makrokrok ruchu, anext_stepkazdy nastepny. Dwie kolejne metodycopy_data

orazcopy_generator_commandsłuza do kopiowania danych miedzy buforami przesyłowymi,

95


a obrazami robotów w procesie MP.

Z bazowej klasygeneratorwywiedziona jest klasa abstrakcyjnarobot_generator, która

dziedziczy wszystkie pola z klasy macierzystejgenerator. W klasierobot_generatorzdefinio-

wane sa ciała dwóch funkcjicopy_dataorazcopy_generator_command, które były wirtualne w

klasiegenerator.

Metodacopy_datakopiuje dane z buforów przesyłowych, otrzymanych z procesówECP, do

odpowiednich obrazów robotów. Metodacopy_generator_commandkopiuje polecenia stwo-

rzone przez generator, a umieszczone w obrazach robotów, do buforów przesyłowych odpo-

wiednich procesówECP.

W oparciu o klaserobot_generatorzostał zaimplementowany nowy generator trajektorii,

wykorzystywany w nowej aplikacji wielorobotowej. Pozostałe, istniejace juz wczesniej, gene-

ratory wymagaly modyfikacji w zwiazku ze zmiana konfiguracji robotów. Ponizej zamieszczo-

no kod zródłowy, znajdujacy sie w pliku nagłówkowymmp.h(../include/mp/), definiujacy nowy

generator mp_kd_generator.

class mp_kd_generator : public mp_robot_generator{

protected:

int idle_step_counter; //!< Licznik jalowych krokow sterowania (bez wykonywania ruchu)

int node_counter; //!< biezacy wezel interpolacji

mp_robot * irp6ot, * irp6p, * conv;

sensor * vsp_force_irp6ot, * vsp_force_irp6p;

public:

trajectory_description td;

int step_no;

double delta[6];

//!< konstruktor

mp_kd_generator(){ printf("UWAGA niepozadane wywolanie konstruktora bezargumentowego

mp_kd_generator - spodziewany blad srp_ecp_msg\n"); };

mp_kd_generator(srp_ecp * mp_msg_input, int step=0): mp_robot_generator (mp_msg_input){

step_no = step;

};

virtual BOOLEAN first_step (mlst<sensor> * sensor_list, mlst<mp_robot> * robot_list);

virtual BOOLEAN next_step (mlst<sensor> * sensor_list, mlst<mp_robot> * robot_list);

}; //!< end : class MP_kd_generator

Listing 12: Klasa definiujaca dodany generator mp_kd.

Zaimplementowana aplikacja wielorobotowa realizuje:

• przełaczenie kinematyki na robocie postument z szescioosiowej (domyslna) na piecio-

osiowa,

96


• ustawienie parametrów narzedzia osiowosymetrycznego dla kazdego z robotów;

• ustawienie rozwarcia chwytaka trzymajacego narzedzie osiowosymetryczne,

• zapamietanie uczonej, poprzez wodzenie koncówki robotaon_track, trajektorii ruchu w

przestrzeni trójwymiarowej;

• równoczesne odtworzenie zapamietanej trajektorii zadanej przez dwa robotypostument

orazon_track.

Kod aplikacji wielorobotowej został umieszczony w dwóch plikach:

mp_m_kd.cc – zawierajacy główna czesc procesuMP, realizujacego powielanie rysunku;

mp_gen_force_kd.cc– zawierajacy generatory wykorzystywane przy realizacji aplikacji wie-

lorobotowej.

Do realizowanego przez aplikacje wielorobotowa zadania powielania rysunku wykorzystywane

sa nastepujace generatory:

mp_kd_generator

ustawianie rozwarcia szczek, definiowanie wykorzystywanych narzedzi, przełaczenie ki-

nematyk,

mp_drawing_teach_in_force_generator

uczenie trajektorii poprzez reczne rysowanie trajektorii robotemon_track, powielanie za-

pamietanych trajektorii jednoczesnie przez oba roboty.

mp_nose_run_force_generator

siłowe wodzenie za "nos" robota, czyli wykorzystanie wskazan czujnika siły do porusza-

nia robotem i przesuwania koncówki robota dozadanej pozycji.

mp_short_move_up

krótki ruch w góre po zakonczeniu zadania rysowania;

Na schematach pokazanych na rysunkach 9.4, 9.5 oraz 9.6 pokazano siec działan programu,

realizujacego zadanie uczenia oraz powielania rysunku przez dwa roboty, zamieszczonego w

pliku mp_m_kd.cc. Ciagła linia pokazano przebieg działan w przypadku, gdy ruch w genera-

torach jest przerywany na skutek spełnienia warunku koncowego (prawidłowe zakonczenie),

wtedy zmiennabreak_stateprzyjmuje wartosc FALSE. Przerywana linia zaznaczono przebieg

działan w przypadku przerwania działania generatora na skutek polecenia operatora (STOP).

Zmiennabreak_stateprzyjmuje wtedy wartosc TRUE.

Program ma postac automatu skonczonego, którego kolejne stany odwołuja sie do genera-

torów ruchu, które z jednej strony wykorzystuja informacje o sile i połozeniu pochodzacym z

97


procesuEDP, a z drugiej przygotowuja wartosc zadana dla wybranych składowych połozenia i

siły, przesyłana do procesuEDP.

98


Inic

jaliz

acja

kom

unik

acji,

usta

wie

nie

prze

rwań

sign

al(S

IGTE

RM

, &ca

tch_

sign

al);

mp_

initi

aliz

e_co

mm

unic

atio

n(co

nfig

);

Utw

orze

nie

listy

robo

tów

Wcz

ytan

ie n

a po

dsta

wie

plik

u ko

nfig

urac

yjne

go

dostęp

nych

robo

tów

Inic

jaliz

acja

gen

erat

orów

mp_

kd_g

ener

ator

mp_

kd_g

en(m

p_m

sg, 8

);

m

p_no

se_r

un_f

orce

_gen

erat

or m

p_nr

f_ge

n(m

p_m

sg, 8

);

m

p_dr

awin

g_te

ach_

in_f

orce

_gen

erat

or m

p_dt

if_ge

n(m

p_m

sg, 8

);

Ocz

ekiw

anie

na

star

t od

UI

wai

t_fo

r_st

art (

);

Wysła

nie

sygn

ału

star

t do

wsz

ystk

ich

ECP

star

t_al

l (*r

obot

_lis

t_he

ad);

Ust

awie

nie

narzęd

zia,

ro

zwar

cia

szczęk

, wyb

ór k

inem

atyk

iM

ove

(rob

ot_l

ist_

head

, slh

ead,

mp_

kd_g

en)

NIE

TAK

0 1

AC

B

Rys

unek

9.4:

Siec

dzia

łanpr

ogra

mu

real

izuj

aceg

oza

dani

ery

sow

ania

,um

iesz

czon

ego

wpl

iku

mp

_m

_kd

.cc,c

zesc

1.

99


Wod

zeni

e za

nos

Mov

e (r

obot

_lis

t_he

ad, s

lhea

d, m

p_nr

f_ge

n)br

eak_

stat

e =

TRU

E lu

b FA

LSE;

Wyb

ór o

pcji

1 -L

oad

draw

ing,

2

-Lea

rn d

raw

ing

Wcz

ytan

ie tr

ajek

torii

mp_

load

_file

(mp_

dtif_

gen,

mp_

msg

);

Ucz

enie

traj

ekto

riim

p_dt

if_ge

n.te

ach_

or_m

ove=

YG

_TE

AC

H;

Mov

e (r

obot

_lis

t_he

ad, s

lhea

d, m

p_dt

if_ge

n)br

eak_

stat

e =

TRU

E lu

b FA

LSE;

Wod

zeni

e za

nos

Mov

e (r

obot

_lis

t_he

ad, s

lhea

d, m

p_nr

f_ge

n)br

eak_

stat

e =

TRU

E lu

b FA

LSE;

Kró

tki r

uch

w g

órę

Mov

e (r

obot

_lis

t_he

ad, s

lhea

d, m

p_nr

f_ge

n)1

2

Wod

zeni

e za

nos

Mov

e (r

obot

_lis

t_he

ad, s

lhea

d, m

p_nr

f_ge

n)br

eak_

stat

e =

TRU

E lu

b FA

LSE;

Ust

awie

nie

czuj

nikó

wco

nfig

ure_

sens

or()

;

AC

B

DF

EH

IG

Rys

unek

9.5:

Siec

dzia

łanpr

ogra

mu

real

izuj

aceg

oza

dani

ery

sow

ania

,um

iesz

czon

ego

wpl

iku

mp

_m

_kd

.cc,c

zesc

2.

100


Odt

war

zani

e na

uczo

nej t

raje

ktor

iim

p_dt

if_ge

n.te

ach_

or_m

ove=

YG

_MO

VE

;M

ove

(rob

ot_l

ist_

head

, slh

ead,

mp_

dtif_

gen)

brea

k_st

ate

= TR

UE

lub

FALS

E;

Nar

ysow

ać z

apam

ięta

ny

rysu

nek?

brea

k_st

ate?

Wod

zeni

e za

nos

Mov

e (r

obot

_lis

t_he

ad, s

lhea

d, m

p_nr

f_ge

n)br

eak_

stat

e =

TRU

E lu

b FA

LSE;

Kró

tki r

uch

w g

órę

Mov

e (r

obot

_lis

t_he

ad, s

lhea

d, m

p_nr

f_ge

n)

Zapi

sać

traj

ekto

rię?

Zapi

sani

e tr

ajek

torii

mp_

save

_file

(mp_

dtif_

gen,

PO

SE

_FO

RC

E_L

INE

AR

, mp_

msg

);

Zako

ńczn

ie p

rogr

amu

wai

t_fo

r_st

op (M

P_T

HR

OW

);te

rmin

ate_

all (

*rob

ot_l

ist_

head

);

Zako

ńcze

nie

proc

esu

MP?

TRU

E

NIE

TAK

TRU

ETA

K

FALS

EN

IE

brea

k_st

ate?

TRU

E

FALS

E

FALS

E

DF

EH

IG

Rys

unek

9.6:

Siec

dzia

łanpr

ogra

mu

real

izuj

aceg

oza

dani

ery

sow

ania

,um

iesz

czon

ego

wpl

iku

mp

_m

_kd

.cc,c

zesc

3.

101


9.3.2 Generatory wykorzystywane w aplikacji wielorobotowej

W tym podrozdziale opisano zadania realizowane przez metody zdefiniowane w pliku

mp_gen_force_kd.cc.

Definicja: void mp_short_move_up(mlst<mp_robot>* robot_list, srp_ecp* mp_msg)

Opis: Metoda wykonujaca krótki ruch w góre, realizowany na przykład po zakonczeniu ryso-

wania rysunku. Metoda ta została utworzona ze wzgledu na czeste wykorzystywanie tego

typu ruchu.Zadana pozycja robota jest zadawana w notacji XYZ_EULER_ZYZ.

Rezultat: Robot unosi sie o 0.5 cm w góre w osi Z układu bazowego robota.

——————————————————————————————————————

Definicja: BOOLEAN mp_kd_generator::first_step (mlst<sensor>* sensor_list, mlst<mp_robot>*

robot_list)

Opis: Pierwszy krok generatoramp_kd_generator. Ustawienie kinematyk dla robotapostu-

ment(piecioosiowa) orazon_track(szescioosiowa).

Rezultat: Ustawienie nowych kinematyk dla poszczególnych robotów.

——————————————————————————————————————

Definicja: BOOLEAN mp_kd_generator::next_step (mlst<sensor>* sensor_list, mlst<mp_robot>*

robot_list)

Opis: Kolejne kroki generatoramp_kd_generator. Ustawienie narzedzi osiowosymetrycznych

dla robotówpostumentorazon_track. Definicje narzedzi róznia sie z powodu wykorzy-

stania róznych kinematyk dla poszczególnych robotów. Zadawane sa szerokosci rozwar-

cia chwytaków robotów.

Rezultat: Ustawienie narzedzi oraz rozwarcia szczek robotów.

——————————————————————————————————————

102


Definicja: BOOLEAN mp_nose_run_force_generator::first_step (mlst<sensor>* sensor_list,

mlst<mp_robot>* robot_list)

Opis: Pierwszy krok generatoramp_nose_run_force_generator, realizujacego wodzenie kon-

cówka robota. Robot jest podatny na siłe – operator wodzac koncówka robota oddziałuje

na nia siła, która odczytuje czujnik siły. Robot reaguje na odczyt przesuwajac sie w kie-

runkach zgodnych z przyłozona siła.

Rezultat: Ustawienie parametrów niezbednych do poprawnego realizowania wodzenia kon-

cówka robota.

——————————————————————————————————————

Definicja: BOOLEAN mp_kd_generator::next_step (mlst<sensor>* sensor_list, mlst<mp_robot>*

robot_list)

Opis: Kolejne kroki generatoramp_nose_run_force_generatorrealizujacego wodzenie kon-

cówka robota. Ta metoda realizuje ruch robota w zaleznosci od sił mierzonych przez

czujnik siły.

Rezultat: Realizacja wodzenia koncówki robota.

——————————————————————————————————————

Definicja: BOOLEAN mp_drawing_teach_in_force_generator::first_step (mlst<sensor>* sen-

sor_list, mlst<mp_robot>* robot_list)

Opis: Metoda realizujaca pierwszy krok generatoramp_drawing_teach_in_force_generator.

W zaleznosci od parametruteach_or_moveroboty działaja w trybie nauki rysowanej

trajektorii (teach_or_move=YG_TEACH) lub powielania wczesniej nauczonego rysunku

(teach_or_move=YG_MOVE). W zaleznosci od trybu pracy robotpostumentjest przygo-

towywany do ruchu lub nie. W trybie uczenia trajektorii wykorzystywany jest tylko robot

on_track

Rezultat: Robotypostumenti on_tracksa ustawione do powielania rysunku lub roboton_track

jest przygotowany do nauki rysunku.

——————————————————————————————————————

103


Definicja: BOOLEAN mp_drawing_teach_in_force_generator::next_step (mlst<sensor>* sen-

sor_list, mlst<mp_robot>* robot_list)

Opis: Metoda realizujaca kolejne kroki generatoramp_drawing_teach_in_force_generator. W

zaleznosci od parametruteach_or_moveroboty realizuja rysowanie nauczonego rysunku

(teach_or_move=YG_MOVE) lub zapamietywana jest trajektoria zakreslana przez robota

on_track(teach_or_move=YG_TEACH). W trakcie uczenia zapamietywane sa zdarzenia

zwiazane z oderwaniem pisaka od kartki i jego ponownym zetknieciem. Dzieki temu ist-

nieje mozliwosc rysowania z odrywaniem pisaka od kartki. Komunikaty informujace o

kolejnych etapach rysowania: Uniesienie, Opuszczanie, Powierzchnia, Zetkniecie, Uno-

szenie, sa przekazywane do interfejsu uzytkownika UI. W procedurze rysowania ruch w

osiach X i Y odbywa sie na podstawie wczesniej zapamietanej trajektorii, a w osi Z pro-

ces EDP dazy do utrzymania stałej siły docisku na odcinkach trajektorii zapamietanych

jako Powierzchniaoraz przesuwania koncówki pisaka na odpowiedniej wysokosci nad

powierzchnia pisania na odcinkach trajektorii zapamietanych jakoUniesienie. Dla proce-

dury uczenia ruch w osiach X i Y odbywa sie na podstawie sił wywieranych przez ope-

ratora na koncówke manipulatora, a w osi Z proces EDP sprawdza odczyty siły czujnika

i okresla czy pisak jest uniesiony, czy dotyka powierzchni. Jednoczesnie zapamietywana

jest trajektoria w płaszczyznie X,Y.

Rezultat: Zapisywana jest trajektoria kreslona przez pisak zamontowany na robocieon_track

lub realizowane jest równoczesne rysowanie zapamietanej trajektorii przez dwa roboty.

Jesli koncówki pisaka nie znajduja sie na tej samej wysokosci, to rysowanie rozpocznie

sie dopiero po wykryciu zetkniecia sie z kartka pisaków zamontowanych na obu robotach.

——————————————————————————————————————

9.3.3 Interfejs uzytkownika

Interfejs uzytkownika umozliwia poczatkowa synchronizacje systemu, a nastepnie informu-

je operatora o stanie, w którym znajduje sie system. Poprzez interfejs mozna wybrac startowa,

wczesniej zdefiniowana pozycje robota lub ustawic go wykorzystujac jedna z dostepnych nota-

cji zadawania orientacji koncówki robota, na przykład XYZ_EULER_ZYZ,

XYZ_ANGLE_AXIS, współrzednych wewnetrznych lub inkrementów wzgledem połozenia

synchronizacji.

Aplikacja wielorobotowa jest uruchamiana jako procesMP, którego parametry sa zdefinio-

wane we wczytywanym przy uruchamianiu systemuMRROC++pliku konfiguracyjnymkd.ini,

znajdujacym sie w katalogu../configs/. Plik konfiguracyjny moze byc równiez załadowany po

104


uruchomieniu systemu. Polecenia operatora sterujace działaniem aplikacji sa wydawane po-

przez wysyłanie pulsów systemu QNX 6.3.0.

9.3.4 Działanie aplikacji

Proces uczenia i rysowania zapamietanych rysunków składa sie z kilku etapów, mogacych

wystepowac wielokrotnie w czasie działania aplikacji wielorobotowej:

Synchronizacja

Synchronizacja (ustawienie manipulatora w pozycji odniesienia – pozycji synchronizacji)

wykonywana jest jednokrotnie podczas uruchamiania systemu.

Ustawienie pozycji startowej

Pozycja, z której rozpoczynane jest działanie aplikacji moze zostac wczesniej zdefiniowana

w pliku konfiguracyjnym (tutaj plikkd.ini). Dzieki temu mozna szybko przejsc do zapamietanej

wczesniej pozycji, z której rozpoczete zostanie zadanie rysowania lub nauki rysunku.

Wodzenie manipulatora

Koncówka manipulatora jest przemieszczana przez nauczyciela do wymaganej pozycji. Wo-

dzenie odbywa sie poprzez zamiane siły wywieranej na koncówke manipulatora we wszystkich

trzech kierunkach na przyrosty połozenia koncówki manipulatora w tych samych kierunkach.

Uchyb siły przetwarzany jest na przyrost połozenia poprzez czynnik proporcjonalny.

Uczenie trajektorii

Rysowanie jest podobne do etapu wodzenia manipulatora. Dodatkowo zapamietywana jest

trajektoria na płaszczyznie kartki (dwie składowe ruchu), a regulator 3 składowej (pionowej),

utrzymuje stała siłe docisku do powierzchni papieru. W etapie uczenia mozliwe jest odrywanie

i ponowne docisniecie pisaka. Zdarzenia te wykrywane sa w automacie skonczonym, zaszytym

w procesieVSP. Podczas uczenia wykorzystywane sa nastepujace stany automatu:

• powierzchnia (rysowanie po powierzchni),

• unoszenie (pisaka),

• uniesienie (wodzenie manipulatorem bez kontaktu pisaka z kartka),

105


• opuszczanie (pisaka),

• zetkniecie (stan pomiedzy opuszczaniem, a powierzchnia, potrzebny do unikniecia odbi-

cia od powierzchni).

Przejscie pomiedzy stanami nastepuje tylko w jednym kierunku (na przykład po "opuszczaniu”

jest "zetkniecie”). Przejscie pomiedzy stanami uzaleznione jest od progowego kryterium war-

tosci siły w osi pionowej. Uniesienie i ponowne zetkniecie z powierzchnia pisaka składa sie z

kolejnych kroków: "unoszenia”, "uniesienia", "opuszczania", "zetkniecia" i "powierzchni". Ze

wzgledu na zastosowanie progowego kryterium wartosci siły w osi pionowej, operator powinien

gładko przesuwac pisak podczas uczenia rysunku, poniewaz szarpniecia moga zostac potrakto-

wane jako chwilowe uniesienie pisaka.

Nauka rysunku rozpoczyna sie od "uniesienia" pisaka. Rysunek jest zapamietywany od

pierwszego zetkniecia pisaka z powierzchnia papieru.

Powielanie rysunku

Ten etap rozpoczyna sie z pisakiem uniesionym w powietrzu. Koncówka manipulatora

opuszcza sie do momentu osiagniecia powierzchni papieru, wykrywanego przez proces VSP.

Nastepnie odtwarzany jest pierwszy zapamietany odcinek trajektorii z regulacja docisku flama-

stra do kartki. Jesli pojawi sie informacja,ze danemu punktowi trajektorii towarzyszy uniesienie

pisaka, wtedy wykonywany jest krótki ruch w góre. Kolejna faza to przemieszczenie manipu-

latora w powietrzu az do momentu wykrycia,ze kolejnemu, zapamietanemu punktowi trajek-

torii towarzyszy informacja o osiagnieciu powierzchni kartki. Wówczas pisak jest opuszczany

pionowo w dół do momentu wykrycia zetkniecia z powierzchnia kartki. Sekwencja ruchu jest

powtarzana do osiagniecia konca zapamietanej trajektorii.

106

10 BADANIA PRZEPROWADZONE W OPARCIU O NOWA APLIKACJE WIELOROBOTOWA

10 Badania przeprowadzone w oparciu o nowa aplikacje wie-

lorobotowa

W tym rozdziale opisano badania przeprowadzone przy wykorzystaniu aplikacji wieloro-

botowej realizujacej zadanie uczenia i powielania rysunku, opisanej w poprzednim rozdziale.

Wyposazenie stanowiska badawczego zostało opisane w rozdziale 3 na stronie 16. Na rysunku

8.1 na stronie 76 przedstawiono schemat stanowiska badawczego. Kartki, na których rysowano

w trakcie badan, zostały umieszczone na blatach na tasmociagu znajdujacym sie w obszarze

działania robotów. Oba roboty w poczatkowej fazie rysowania sa umieszczone prostopadle do

tasmociagu i równolegle do siebie.

Narzedziami osiowosymetrycznymi, wykorzystywanymi w aplikacji wielorobotowej sa fla-

mastry zamontowane w szczekach chwytaka za pomoca przygotowanych drewnianych kostek.

Kostki sa niezbedne dla prawidłowego umocowania flamastrów w szczekach chwytaka, które

zostały zaprojektowane jako uchwyty dla kostki Rubika. Flamastry sa umieszczone posrodku

chwytaka (szczek) i skierowane pionowo w dół.

10.1 Kalibracja robotów

Oba robotypostumentorazon_trackmaja taka sama konfiguracje mechaniczna. Wystepuja

jednak róznice w odczytach z enkoderów w piatym stopniu swobody, czyli kisci. Z tego wzgle-

du na poczatku badan nalezało przeprowadzic kalibracje kinematyki robotów, tak by rzeczy-

wisty układ współrzednych odpowiadał układowi współrzednych załozonemu w kinematykach

(rysunek 8.2 strona 79 oraz rysunek 8.3 strona 81). Z tego wzgledu zdefiniowano parametry

GEOM_SYNCH_POS_*MODEL_*, które modyfikuja obliczanie połozenia poszczególnych

stopni swobody przez zagadnienia kinematyki w taki sposób, by układy współrzedne rzeczy-

wisty i "wewnetrzny" systemuMRROC++były tozsame. Ponizej zamieszczono wartosci para-

metrów jakie otrzymano w wyniku przeprowadzonych badan.

Parametry korygujace kinematyke szescioosiowa robotaon_track:

GEOM_SYNCH_POS_0_MODEL_0_OT SYNCH_POS_0

GEOM_SYNCH_POS_1_MODEL_0_OT -13.819



GEOM_SYNCH_POS_4_MODEL_0_OT 155.997



GEOM_SYNCH_POS_7_MODEL_0_OT 4830

Parametry korygujace kinematyke szescioosiowa robotapostument:

GEOM_SYNCH_POS_0_MODEL_0_P SYNCH_POS_0

GEOM_SYNCH_POS_1_MODEL_0_P -15.9

GEOM_SYNCH_POS_2_MODEL_0_P -5

107



GEOM_SYNCH_POS_4_MODEL_0_P 153.31



GEOM_SYNCH_POS_7_MODEL_0_P 4830

Parametry korygujace kinematyke piecioosiowa robotaon_track:

GEOM_SYNCH_POS_0_MODEL_1_P SYNCH_POS_0


GEOM_SYNCH_POS_2_MODEL_1_P -5





GEOM_SYNCH_POS_7_MODEL_1_P 4830

Powyzsze ustawienia, wyrazone w inkrementach, pozwoliły na skalibrowanie kinematyki w

stopniu umozliwiajacym poprawne wykonania zadania rysowania (z dokładnoscia do około

1mm). Obecnie prowadzone sa badania, bedace tematem oddzielnej pracy, które maja znacznie

poprawic dokładnosc kinematyk dla poszczególnych robotów.

Przy badaniach kalibracji robotów, skorzystano z wbudowanego w systemMRROC++me-

chanizmu do zbierania danych pomiarówych tzw.readera. Umozliwia on zbieranie danych po-

miarowych w dowolnym momencie w trakcie działania robotów. Na rysunkach 10.1, 10.2, 10.3

pokazano wyniki kalibracji dla kinematyki szescioosiowej dla robotaon_track, oraz piecio- i

szescioosiowej dla robotapostument.

Pomiary zostały opracowane w programieMatlab. W celu porównania otrzymanych po-

miarów opracowano skrypt matlabowy, który przelicza macierze wyników w taki sposób, by

wybrany punkt wykresu dla poszczególnych kinematyk pokrył sie. W oparciu o wyliczone

przesuniecia wgledem wybranego punktu charakterystyki jednej z kinematyk, korygowane sa

punkty pozostałych kinematyk. W ten sposób uzyskiwane jest nałozenie sie trzech trajektorii

uzyskanych w pomiarach dla poszczególnych kinematyk. Dzieki temu mozna zaobserwowac

czy otrzymane wykresy pokrywaja sie podczas całego ruchu robota oraz czy poszczególne od-

cinki ruchu maja taka sama długosc.

Przy prezentowaniu wykresów przyjeto nastepujace oznaczenia:

kin6 ot – oznacza wykres dla robotaon_trackz kinematyka szescioosiowa,

kin6 p – oznacza wykres dla robotapostumentz kinematyka szescioosiowa,

kin5 p – oznacza wykres dla robotapostumentz kinematyka piecioosiowa.

108


0.720.74

0.760.78

0.80.82

−0.05

0

0.050.92

0.94

0.96

0.98

1

1.02

wspolrzedna x [m]wspolrzedna y [m]

wsp

olrz

edna

z [m

]

6kin p5kin p6kin ot

Rysunek 10.1: Wykres ruchu w trzech kierunkach w wykorzystywanych kinematykach szescio-

i piecioosiowej dla robotówpostumenti on_track.

0.7 0.72 0.74 0.76 0.78 0.8 0.82−0.06

−0.04

−0.02

0

0.02

0.04

0.06

polozenie koncowki manipulatora dla kartezjanskiej wsp.x [m]

polo

zeni

e ko

ncow

ki m

anip

ulat

ora

dla

kart

ezja

nski

ej w

sp.y

[m]

6kin p5kin p6kin ot

Rysunek 10.2: Wykres ruchu w osiach x oraz y dla wykorzystywanych kinematyk.

109


0.7 0.72 0.74 0.76 0.78 0.8 0.820.9

0.92

0.94

0.96

0.98

1

1.02

1.04


polo

zeni

e ko

ncow

ki m

anip

ulat

ora

dla

kart

ezja

nski

ej w

sp.z

[m]

6kin p5kin p6kin ot

Rysunek 10.3: Wykres ruchu w osiach x oraz z dla wykorzystywanych kinematyk.

Przeprowadzone po kalibracji badania ruchu robotami w kinematyce szescioosiowej dla ro-

botówpostumenti on_trackoraz piecioosiowej dla robotapostument, zadawane poprzez zmiane

współrzednych w notacji XYZ_EULER_ZYZ, potwierdziły poprawne skalibrowanie robotów.

Osie x,y,z sa pod katem prostym wzgledem siebie, linie zakreslone w trakcie ruchu w poszcze-

gólnych osiach nakładaja sie, a bład nie jest wiekszy niz 0.1 mm.

10.2 Badania poprawnosci działania kinematyk

Kolejne badania polegajace na odwzorowywaniu nauczonej trajektorii pozwoliły na spraw-

dzenie, czy rysunki otrzymane w trybie pisania sa takie same jak te narysowane przez operatora

w trybie uczenia.

Zebrane wyniki zostały przetworzone i opracowane w programieMatlab.

W kolejnych podrozdziałach zostana przedstawione wyniki badan dla czterech nauczonych

trajektorii.

Przy prezentowaniu wykresów przyjeto nastepujace oznaczenia:

teach ot – oznacza wykres dla robotaon_trackw trybie uczenia,

draw ot – oznacza wykres dla robotaon_trackw trybie powielania,

draw p – oznacza wykres dla robotapostumentw trybie powielania.

110


10.2.1 Kwadrat

Na rysunkach 10.4, 10.5, 10.6 przedstawiono naniesione na siebie pomiary wykonane dla

kinematyki szescioosiowej robotaon_trackw trybie uczenia oraz kinematyki szescioosiowej

dla robotaon_tracki piecioosiowej dla robotapostumentw trybie powielania nauczonego ry-

sunku kwadratu.

0.780.79

0.80.81

0.82

−0.04

−0.02

0

0.020.88

0.9

0.92

0.94

0.96


wsp

olrz

edna

z [m

]

teach otdraw otdraw p

Rysunek 10.4: Trójwymiarowy wykres trajektorii realizowanych w trakcie uczenia i powielania

rysunku przez robotypostumenti on_track.

111


0.77 0.78 0.79 0.8 0.81 0.82 0.83−0.04

−0.035

−0.03

−0.025

−0.02

−0.015

−0.01

−0.005

0

0.005


polo

zeni

e ko

ncow

ki m

anip

ulat

ora

dla

kart

ezja

nski

ej w

sp.y

[m]


Rysunek 10.5: Wykres ruchu w osiach x oraz y uczonej i powielanych trajektorii.

0.77 0.78 0.79 0.8 0.81 0.82 0.830.89

0.9

0.91

0.92

0.93

0.94

0.95

0.96

0.97

0.98

0.99


polo

zeni

e ko

ncow

ki m

anip

ulat

ora

dla

kart

ezja

nski

ej w

sp.z

[m]


Rysunek 10.6: Wykres ruchu w osiach x oraz z uczonej i powielanych trajektorii.

112


10.2.2 Okregi

Na rysunkach 10.7, 10.8, 10.9 przedstawiono naniesione na siebie pomiary wykonane dla

kinematyki szescioosiowej robotaon_trackw trybie uczenia oraz kinematyki szescioosiowej

dla robotaon_tracki piecioosiowej dla robotapostumentw trybie powielania nauczonego ry-

sunku okregów.

0.80.82

0.840.86

−0.04

−0.02

0

0.89

0.895

0.9

0.905

0.91

0.915

0.92


wsp

olrz

edna

z [m

]


Rysunek 10.7: Trójwymiarowy wykres trajektorii realizowanych w trakcie uczenia i powielania

rysunku przez robotypostumenti on_track.

113


0.8 0.81 0.82 0.83 0.84 0.85 0.86 0.87−0.04

−0.03

−0.02

−0.01

0

0.01

0.02


polo

zeni

e ko

ncow

ki m

anip

ulat

ora

dla

kart

ezja

nski

ej w

sp.y

[m]



0.8 0.81 0.82 0.83 0.84 0.85 0.86 0.870.885

0.89

0.895

0.9

0.905

0.91

0.915

0.92

0.925

0.93

0.935


polo

zeni

e ko

ncow

ki m

anip

ulat

ora

dla

kart

ezja

nski

ej w

sp.z

[m]



114


10.2.3 Gwiazda

Na rysunkach 10.10, 10.11, 10.12 przedstawiono naniesione na siebie pomiary wykonane

dla kinematyki szescioosiowej robotaon_trackw trybie uczenia oraz kinematyki szescioosio-

wej dla robotaon_tracki piecioosiowej dla robotapostumentw trybie powielania nauczonego

rysunku gwiazdy.

0.820.84

0.860.88

0.9

−0.1

−0.08

−0.06

−0.04

−0.02

0.89

0.9

0.91

0.92

0.93

0.94

wspolrzedna x [m]

wspolrzedna y [m]

wsp

olrz

edna

z [m

]


Rysunek 10.10: Trójwymiarowy wykres trajektorii realizowanych w trakcie uczenia i powiela-

nia rysunku przez robotypostumenti on_track.

115


0.81 0.82 0.83 0.84 0.85 0.86 0.87 0.88 0.89−0.1

−0.09

−0.08

−0.07

−0.06

−0.05

−0.04

−0.03

−0.02


polo

zeni

e ko

ncow

ki m

anip

ulat

ora

dla

kart

ezja

nski

ej w

sp.y

[m]



0.81 0.82 0.83 0.84 0.85 0.86 0.87 0.88 0.890.88

0.89

0.9

0.91

0.92

0.93

0.94

0.95

0.96

0.97

0.98


polo

zeni

e ko

ncow

ki m

anip

ulat

ora

dla

kart

ezja

nski

ej w

sp.z

[m]



116


10.2.4 Dom

Na rysunkach 10.13, 10.14, 10.15 przedstawiono naniesione na siebie pomiary wykonane

dla kinematyki szescioosiowej robotaon_trackw trybie uczenia oraz kinematyki szescioosio-

wej dla robotaon_tracki piecioosiowej dla robotapostumentw trybie powielania nauczonego

rysunku domu.

0.85

0.9

0.95

−0.05

0

0.05

0.88

0.9

0.92

0.94

0.96


wsp

olrz

edna

z [m

]


Rysunek 10.13: Trójwymiarowy wykres trajektorii realizowanych w trakcie uczenia i powiela-

nia rysunku przez robotypostumenti on_track.

117


0.84 0.86 0.88 0.9 0.92 0.94−0.06

−0.05

−0.04

−0.03

−0.02

−0.01

0

0.01

0.02

0.03

0.04


polo

zeni

e ko

ncow

ki m

anip

ulat

ora

dla

kart

ezja

nski

ej w

sp.y

[m]



0.84 0.86 0.88 0.9 0.92 0.940.89

0.9

0.91

0.92

0.93

0.94

0.95

0.96

0.97

0.98

0.99


polo

zeni

e ko

ncow

ki m

anip

ulat

ora

dla

kart

ezja

nski

ej w

sp.z

[m]



10.2.5 Wnioski

Przeprowadzone badania potwierdziły prawidłowe działanie kinematyk. Rysunki przedsta-

wione na wykresach zostały przekształcone w sposób umozliwiajacy porównanie kreslonych

trajektorii. Mozna na nich zaobserwowac niewielkie odchylenia, około 0.1 mm, pomiedzy na-

118


uczona, a odtwarzanymi trajektoriami. Niedokładnosci te wynikaja z obliczen wykonywanych

przez wykorzystywane kinematyki. Wpływ na dokładnosc rysowania ma równiez przyjety spo-

sób kalibracji.

Charakterystyczne róznice pomiedzy rysunkami odpowiadajacymi fazie uczenia i powiela-

nia sa widoczne w torze unoszenia i opuszczania pisaka. Operator wykonuje "płynne" ruchy w

góre i dół, a odcinek pokonywany ponad kartka jest pofalowany i nieregularny w płaszczyznie

pionowej. Trajektorie zakreslone przez robota, w trybie powielania rysunku, w trakcie ruchu w

góre i dół sa pionowe, o takiej samej wysokosci, a odcinek pokonywany ponad kartka lezy w

płaszczyznie poziomej.

10.3 Wykresy siły dla trajektorii nauczonej przez operatora

Na ponizszych rysunkach pokazano wyniki pomiaru sił odczytywanych przez czujnik siły

FT3084 zamontowany na robociepostumentoraz czujnik siły FT6284 zamontowany na robocie

on_track, w trakcie uczenia trajektorii oraz w trakcie powielania rysunku realizowanego przez

dwa roboty.

W opisie wykresów zastosowano nastepujace oznaczenia:

* – zetkniecie pisaka z podłozem,

a – ruch po powierzchni,

b – podnoszenie pisaka,

c – ruch ponad powierzchna kartki,

d – opuszczanie pisaka.

119


0.87

0.88

0.89

0.9

−0.04−0.02

00.02

0.040.06

−15

−10

−5

0

5

10

15

wpsolrzedna X [m]

Linia przerywana w osi X,Y, sila w osi Z dla robota on_track, kinematyka 6 osiowa, uczenie

wspolrzedna Y [m]

sila

[N]

Rysunek 10.16: Rysowanie linii przerywanej w przestrzeni dwuwymiarowej x,y. W osi z za-

znaczono odczytywana siłe.

−0.04 −0.03 −0.02 −0.01 0 0.01 0.02 0.03 0.04 0.05 0.06−15

−10

−5

0

5

10

15

Linia przerywana narysowana w osi Y,sila w osi Z dla robota on_track, kinematyka 6 osiowa, uczenie

wspolrzedna Y [m]

sila

[N]

a

d

c

b

Rysunek 10.17: Odczyt siły w trakcie uczenia rysunku (roboton_track).

120


−0.05 −0.04 −0.03 −0.02 −0.01 0 0.01 0.02 0.03 0.04 0.05−1.5

−1

−0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

Linia przerywana narysowana w osi Y,sila w osi Z dla robota on_track, kinematyka 6 osiowa, pisanie

wspolrzedna Y [m]

sila

[N]

a

b

cd

*

Rysunek 10.18: Odczyt siły w trakcie powielania rysunku (roboton_track).

−0.05 −0.04 −0.03 −0.02 −0.01 0 0.01 0.02 0.03 0.04 0.05

−2

−1

0

1

2

3

Linia przerywana narysowana w osi Y,sila w osi Z dla robota postument, kinematyka 5 osiowa, pisanie

wspolrzedna Y [m]

sila

[N]

*

a

b

c

d

Rysunek 10.19: Odczyt siły w trakcie powielania rysunku (robotpostument).

121


10.3.1 Wnioski

W trybie powielania nauczonego rysunku, podczas ruchu po powierzchni kartki, regula-

tor stara sie utrzymac siłe na poziomie około 1 Niutona, przy ruchu w stanie uniesienia siła

wskazywana przez czujnik siły wynosi około 0 Niutonów. W etapie ruchu w stanie uniesienia,

zaznaczonym na rysunkach 10.18 oraz 10.19 jakod, realizowany jest jedynie ruch pozycyjny.

W trakcie badan zauwazono silne oscylacje wystepujace podczas rysowania po powierzch-

ni. Jest to prawdopodobnie spowodowane nieoptymalnym doborem zastosowanego obecnie w

systemieMRROC++regulatora lub tarciem towarzyszacym ruchowi. Badania nad regulatorami

siłowymi sa obecnie przedmiotem oddzielnej pracy.

10.4 Wykresy siły dla trajektorii idealnych, wygenerowanych w progra-

mie Matlab

Badania sił przeprowadzone w oparciu o trajektorie nauczone przez operatora sa zakłócone

poprzez odchylenia trajektorii, jakie pojawiały sie w trakcie uczenia. W celu wyeliminowania

nieliniowosci w uczonej trajektorii, do celów badania sił, zdecydowano sie na wygenerowanie

idealnych trajektorii przy uzyciu programuMatlab. Trajektorie te musza miec okreslona postac,

tak by zostały poprawnie odczytane w systemieMRROC++.

Ponizej pokazano pierwsze trzy linie plikuidealnalinia.trjprzechowujacego informacje opi-

sujace zapamietana trajektorie ruchu linii przerywanej:

POSE_FORCE_LINEAR

4900

0 0 0.00002 0 0 0 0 0 0 2

W pierwszej linii zapisany jest typ ruchu w jakim zapamietana została trajektoria. W drugiej

linii znajduje sie liczba zapisanych danych o połozeniu. Kolejne linie zawieraja koordynaty wy-

razone we współrzednych kartezjanskich lub katach wewnetrznych manipulatora. Kazda z linii

zawiera 10 parametrów. Pierwszy parametr nie jest wykorzystywany w generatorach siłowych.

W generatorach pozycyjnych okresla on czas osiagniecia pozycji opisanej w kolejnych osmiu

parametrach wiersza. Liczba osmiu parametrów jest potrzebna w przypadku wykorzystywania

trajektorii podanej w katach wewnetrznych robota. W napisanej aplikacji wielorobotowej wy-

korzystywane sa druga, trzecia, czwarta oraz ostatnia pozycja z linii. Pierwsze trzy parametry

odpowiadaja współrzednym kartezjanskim zapisanym w trakcie nauki trajektorii. Ostatni para-

metr okresla stan, w jakim znajduje sie aktualnie robot ( 2 – powierzchnia, 3 – unoszenie, 4 –

uniesienie, 5 – opuszczanie).

Na rysunkach zamieszczonych ponizej pokazano wyniki pomiaru sił odczytywanych przez

czujnik siły FT3084 zamontowany na robociepostumentoraz czujnik siły FT6284 zamonto-

wany na robocieon_track, w trakcie powielania trajektorii "idealnej" linii przerywanej oraz

122


"idealnego" koła, wygenerowanych w programieMatlab.

W opisie wykresów zastosowano nastepujace oznaczenia:

* – zetkniecie pisaka z podłozem,

a – ruch po powierzchni,

b – podnoszenie pisaka,

c – ruch ponad powierzchna kartki,

d – opuszczanie pisaka.

10.4.1 Idealna przerywana linia prosta

−1−0.5

00.5

1

0

0.02

0.04

0.06

0.08−1

0

1

2

3

4

5

x 10−3

wspolrzedna x [m]

Zadana trajektoria idealnej linii przerywanej, rysowanej w plaszczyznie XY.

wspolrzedna y [m]

wsp

olrz

edna

z [m

]

Rysunek 10.20: Wygenerowana w programie Matlab trajektoria o postaci linii przerywanej.

123


0.987

0.9875

0.988

0.9885

−0.06

−0.04

−0.02

0

0.02−0.075

−0.07

−0.065

−0.06

−0.055

−0.05

wspolrzedna X [m]

Wykres trójwymiarowy linii X,Y,Z dla robota on_track, kinematyka 6 osiowa, pisanie

wspolrzedna Y [m]

wsp

olrz

edna

Z [m

]

Rysunek 10.21: Trajektoria zrealizowana przez robotaon_trackw trakcie powielania rysunku.

0.89650.897

0.89750.898

0.89850.899

−0.1

−0.08

−0.06

−0.04

−0.020.62

0.625

0.63

0.635

0.64

0.645

wspolrzedna X [m]

Wykres trójwymiarowy linii X,Y,Z dla robota postument, kinematyka 5 osiowa, pisanie

wspolrzzedna Y [m]

wsp

olrz

edna

Z [m

]

Rysunek 10.22: Trajektoria zrealizowana przez robotapostumentw trakcie powielania rysunku.

124


−0.06 −0.05 −0.04 −0.03 −0.02 −0.01 0 0.01 0.02−0.5

0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

Linia przerywana narysowana w osi Y,sila w osi Z dla robota on_track, kinematyka 6 osiowa, pisanie

wspolrzedna Y [m]

sila

[N]

*

ab

cd

Rysunek 10.23: Odczyt siły w trakcie powielania idealnej linii przerywanej przez robota

on_track.

−0.1 −0.09 −0.08 −0.07 −0.06 −0.05 −0.04 −0.03 −0.02−2.5

0

2.5

5

Linia przerywana narysowana w osi Y,sila w osi Z dla robota postument, kinematyka 5 osiowa, pisanie

wspolrzedna Y [m]

sila

[N]

*

a cb d

Rysunek 10.24: Odczyt siły w trakcie powielania idealnej linii przerywanej przez robotapostu-

ment.

125


10.4.2 Idealne koło

−0.04−0.02

00.02

0.04

−0.04

−0.02

0

0.02

0.040

1

2

3

4

5

x 10−3

wspolrzedna x [m]

Wykres trójwymiarowy kola

wspolrzedna y [m]

wsp

olrz

edna

z [m

]

Rysunek 10.25: Wygenerowana w programie Matlab trajektoria o postaci koła.

126


0.86

0.88

0.9

0.92

−0.08

−0.06

−0.04

−0.02

0−0.08

−0.07

−0.06

−0.05

−0.04

wspolrzedna X [m]

Wykres trójwymiarowy kola X,Y,Z dla robota on_track, kinematyka 6 osiowa, pisanie

wspolrzedna Y [m]

wsp

olrz

edna

Z [m

]

Rysunek 10.26: Trajektoria zrealizowana przez robotaon_trackw trakcie powielania rysunku

koła.

0.80.82

0.840.86

0.88

−0.02

0

0.02

0.040.62

0.625

0.63

0.635

0.64

0.645

wspolrzedna X [m]

Wykres trójwymiarowy kola X,Y,Z dla robota postument, kinematyka 5 osiowa, pisanie

wspolrzzedna Y [m]

wsp

olrz

edna

Z [m

]

Rysunek 10.27: Trajektoria zrealizowana przez robotapostumentw trakcie powielania rysunku

koła.

127


−0.065 −0.06 −0.055 −0.05 −0.045 −0.04 −0.035 −0.03 −0.025 −0.02 −0.015−0.5

0

0.5

1

1.5

2

2.5

3

3.5Kolo narysowane w osi Y,sila w osi Z dla robota on_track, kinematyka 6 osiowa, pisanie

wspolrzedna Y [m]

sila

[N]

a a

b

Rysunek 10.28: Odczyt siły w trakcie powielania idealnego koła przez robotaon_track.

−0.01 −0.005 0 0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04−1.5

−1

−0.5

0

0.5

1

1.5

2

2.5

3

3.5Kolo narysowane w osi Y,sila w osi Z dla robota postument, kinematyka 5 osiowa, pisanie

wspolrzedna Y [m]

sila

[N]

a a

b

Rysunek 10.29: Odczyt siły w trakcie powielania idealnego koła przez robotapostument.

10.4.3 Wnioski

Otrzymane trajektorie sa dokładniejsze, bez odchylen na odcinkach w osi Z. Tego typu tra-

jektorie jest bardzo trudno otrzymac poprzez uczenie reczne.

Na otrzymanych wykresach znacznie łatwiej jest zaobserwowac kolejne fazy ruchu robo-

tów i odpowiadajacych im sił, niz w przypadku wykresów powielanych na podstawie trajektorii

128


nauczonej przez operatora. Pomiary odczytywane przez czujniki siły tworza bardziej regularne

wzory.

Siła docisku przy pisaniu po powierzchni kartki utrzymywana jest na poziomie około 1 Niu-

tona. W trakcie pisania wystepuja drgania zwiazane z tarciem statycznym i dynamicznym na

styku pisaka z kartka.

W trakcie badan zauwazono zaleznosc amplitudy drgan odczytów siły od wykorzystywane-

go pisaka. Zmiany były widoczne przy zamianie zuzytego pisaka na nowy. Mało uzywany pisak

jest bardziej sztywny i z tego powodu drgania w trakcie pisania sa znaczaco wieksze. Mozna

to zaobserowowac na rysunkach 10.28 oraz 10.29 (strona 128). W trakcie powielania ideal-

nego koła roboton_trackwykorzystywał nowy flamaster, natomiast flamaster zamontowany

w uchwycie robotapostumentbył juz wczesniej kilkukrotnie wykorzystywany. Na wykresach

10.21 oraz 10.22 (strona 124), przedstawiajacych trajektorie w przestrzeni trójwymiarowej dla

robotówon_tracki postument, mozna zaobserwowac odchylenia w osi X spowodowane niedo-

kładnosciami obliczen poszczególnych kinematyk. Niedokładnosci te sa rzedu około 0.0005 m,

czyli 0.5 mm. Prawdopodobnie wynikaja one z błedów numerycznych oraz zaokraglen stoso-

wanych w obliczeniach kinematycznych. Niedokładnosci te moga byc równiez spowodowane

luzami wystepujacymi w mechanice wykorzystywanych robotów.

Badania przeprowadzone przy uzyciu trajektorii wygenerowanych poprzez skrypt urucho-

miony w programieMatlab, wskazuja na duza zaleznosc wyników od tarcia pomiedzy pisa-

kiem, a powierzchnia. Zagadnienie tarcia oraz jego wpływ na odczyty czujnika siły jest trudne

do rozwiazania ze wzgledu na jego silna nieliniowosc.

10.5 Podsumownie wyników badan

Przeprowadzone badania potwierdziły sprawne działanie nowego sterownika dołaczonego

do systemuMRROC++czujnika siły Gamma FT6284 oraz poprawne skalibrowanie kinematyk.

Zadania uczenia i powielania rysunku sa realizowane z wystarczajaca dokładnoscia. Obecnie

prowadzone badania nad kalibracja robotów, bedace tematem oddzielnej pracy, moga polep-

szyc dokładnosc obliczen kinematycznych i wpłynac na dokładnosc rysowania.

Modyfikacje wprowadzone po modernizacji robotów umozliwiły sprawdzenie i porównanie

dwóch kinematyk szescio- i piecioosiowej. W zaimplementowanej aplikacji wielorobotowej,

wykorzystujacej narzedzia osiowosymetryczne, obie kinematyki działaja prawidłowo i wystar-

czajaco dokładnie (dokładnosc rzedu 1 mm). Uruchomiony mechanizm przełaczania kinematyk

działa poprawnie.

129

11 PODSUMOWANIE

11 Podsumowanie

Efektem niniejszej pracy jest aplikacja wielorobotowa realizujaca sterowanie

pozycyjno–siłowe. Aplikacja ta wykorzystuje dwa czujniki siły zamontowane na dwóch ma-

nipulatorach. W trakcie pracy zaprojektowano i wykonano płytki drukowane, niezbedne do

usprawnienia komunikacji z zastanym czujnikiem siły FT3084 oraz komunikacji z nowym czuj-

nikiem siły FT6284 firmy ATI. Zaimplementowano nowy sterownik czujnika siły FT6284 fir-

my ATI, realizujacy odczytywanie danych pomiarowych poprzez karte akwizycji danych PCI-

6034E firmy National Instruments i przetwarzajacy otrzymane dane do postaci wykorzystywa-

nej przez systemMRROC++.

Przy realizacji aplikacji wielorobotowej wprowadzono wiele istotnych zmian i poprawek,

które zwiazane były ze zmiana konfiguracji robotów, wpływajacych na strukture systemu

MRROC++, na przykład realizacje buforów komunikacyjnych pomiedzy poszczególnymi pro-

cesami. Zmiany w konfiguracji robotów wymagały przebadania i wprowadzenia do systemu

MRROC++ nowych kinematyk robotów, obsłuzenia nowych, dodanych stopni swobody oraz

zaimplementowania mechanizmu przełaczania kinematyk.

Wszystkie modyfikacje prowadziły do uruchomienia aplikacji sterujacej dwoma robotami,

z których jeden korzysta z kinematyki szescioosiowej, natomiast drugi z piecioosiowej. Wy-

korzystanie kinematyki piecioosiowej wymagało uruchomienia mozliwosci zadawania narze-

dzia osiowosymetrycznego w systemieMRROC++. W trakcie realizacji zmian wprowadzono

mozliwosc zadawania parametrów narzedzia wykorzystywanego przez manipulatory poprzez

interfejs uzytkownika UI. Ze wzgledu na zmiany w konstrukcji robotów, niezbedne stało sie

przeprowadzenie kalibracji nowych kinematyk w taki sposób, by przyjety wewnatrz systemu

MRROC++układ koncówki narzedzia był tozsamy z układem koncówki w rzeczywistosci. W

przeciwnym razie, z powodu rozbieznosci robot działałby nieprawidłowo.

Zaimplementowana aplikacja wielorobotowa wykorzystuje szereg generatorów, umozliwia-

jacych ruch robotów, wybór odpowiedniej kinematyki, ustawienie parametrów narzedzi oraz

pozwalajacych na realizacje zadania uczenia i powielania nauczonego rysunku.

Po uruchomieniu aplikacji wielorobotowej przeprowadzono wiele badan, w trakcie których

sprawdzono poprawnosc działania zaimplementowanego sterownika czujnika siły FT6284 oraz

pozostałych wprowadzonych zmian i poprawek. Wyniki badan wskazuja na poprawne działanie

systemuMRROC++rozbudowanego o dodane funkcjonalnosci.

Niniejsza praca została zakonczona pomyslnie, a aplikacja wielorobotowa działa poprawnie.

Opracowana w ramach pracy aplikacja wielorobotowa, realizujaca zadanie sterowania

pozycyjno-siłowego dwoma robotami, jest istotnym etapem prac prowadzonych w laboratorium

Robotyki Instytutu Automatyki i Informatyki Stosowanej, zmierzajacych do implementacji za-

dania układania kostki Rubika przez dwa manipulatory. Zapewnienie dodatkowego czujnika

siły oraz zmiany wprowadzone do systemuMRROC++ znacznie rozszerzyły mozliwosci wy-

130

11 PODSUMOWANIE

korzystania znajdujacych sie w laboratorium robotów.

131

LITERATURA

Literatura

[1] J.J. Craig,Introduction to Robotics, Mechanics and Control, Addison-Wesley Publishing

Company, 1986r.,

[2] C.Zielinski, W. Szynkiewicz,Podrecznik programowania systemu MRROC++ dla robota

IRp-6 na torze jezdnym, Raport IAiIS nr 99-21, Warszawa, kwiecien 1999r.,

[3] C. Zielinski, W. Szynkiewicz,System MRROC++ dla robota IRp-6, Raport IAiIS nr 99-

20, Warszawa, maj 1999r.,

[4] C.Zielinski, W. Szynkiewicz, T. Zielinska,System MRROC++ dla robota RNT, raport

IAiIS nr 99-35, Warszawa, maj 1999r.,

[5] C. Zielinski, A Quasi-Gormal Approach to Structuring Multi-Robot System Controllers,

IAiIS, RoMoCo’01, 18-20 pazdziernik 2001r.,

[6] W. Szynkiewicz,Uruchamianie i obsługa systemu MRROC++, IAiIS, 2002r.,

[7] T. Winiarski, W. Szynkiewicz,Implementacja sterowania pozycyjno - siłowego w systemie

MRROC++, Raport IAiIS nr 04-01, Warszawa, styczen 2004r.,

[8] T. Winiarski, Wstepna implementacja sterowania pozycyjno-siłowego w systemie

MRROC++, Raport IAiIS nr 04-14, Warszawa, grudzien 2004r.,

[9] A. Sluzek, C. Zielinski, W. Szynkiewicz,Model kinematyczny i generator trajektorii li-

niowej dla robota IRp-6, Robbit, Warszawa, kwiecien 1991r.,

[10] J.K. Nowacki, Generatory trajektorii w systemie MRROC++, IAiIS, praca dyplomowa

2004r.,

[11] K Tchon, A. Mazur, I. Duleba, R. Hossa, R. Muszynski,Manipulatory i roboty mobilne,

Akademicka Oficyna Wydawnicza PLJ, Warszawa, 2000r.,

[12] W. Jacak, K. Tchon, Podstawy robotyki, wydawnictwo Politechniki Wrocławskiej, Wro-

cław 1992r.,

[13] Multi-Axis Force/Torque Sensor System, Installation and Operation Manual, Docu-

ment # : 9610-05-1017-06, pazdziernik 2003r.,

[14] M.W.Spong, M. Vidyasagar, Dynamika i Sterowanie Robotów, WNT, Warszawa, 1997r.

[15] G. Zeng, A. HemamiOverview of Robot Force Control, Robotica, 1997, 15(5), str.473-

482,

132

LITERATURA

[16] Herman Bruyninckx,Specification of Force-Controlled Actions in the "Task Frame For-

malism" - A Synthesis, IEEE Transactions on Robotics and Automation vol.12, No. 4,

sierpien 1996.

[17] T. Winiarski,Podstawy sterowania siłowego w robotach, IAiIS, czerwiec 2005r.

[18] David Schleef, Frank Hess, Herman Bruyninckx,The Control and Measurement Device

Interface handbook, Comedi, (http://www.comedi.org), 2005r.

[19] Strona główna programu Protel wykorzystywanego przy projektowaniu płytek,

(http://www.altium.com/protel/features.htm).

[20] DAQ Quick Start Guide, National Instruments Corporation, (http://www.ni.com), wrzesien

2003r.

[21] DAQCard E Series Register-Level Programmer Manual, National Instruments Corpora-

tion, (http://www.ni.com), listopad 1998r.

[22] E/S/M Series Calibration Procedure for NI-DAQ mx, National Instruments Corpora-

tion,(http://www.ni.com), marzec 2005r.

[23] NI 6034E/6035E/6036E Family Specifications, National Instruments Corpora-

tion,(http://www.ni.com), wrzesien 2003r.

[24] DAQ-STCTM Technical Reference Manual+, National Instruments Corporation,

(http://www.ni.com), styczen 1999r.

[25] DAQ F/T Multi-Axis Force/Torque Sensor System, ATI , Document #:9610-05-1017-

08,(http://www.ati-ia.com/), lipiec 2004r.

[26] Dokumentacja techniczno–ruchowa robotów IRb–6 i IRb–60,PIAP, Warszawa 1983.

133

http://www.altium.com/protel/features.htm

SPIS RYSUNKÓW

Spis rysunków

2.1 Wektory sił i momentów przyłozonych do czujnika ([25]). . . . . . . . . . . . 10

2.2 Schemat elektronicznego połaczenia czujnika siły z komputerem. . . . . . . . .11

2.3 Hybrydowy regulator pozycyjno–siłowy ([7]). . . . . . . . . . . . . . . . . . .14

3.1 Manipulator IRp-6 w wersji na postumencie.([26]) . . . . . . . . . . . . . . .17

3.2 Schemat robota IRp-6 z nowym członem. . . . . . . . . . . . . . . . . . . . .18

3.3 Schemat stanowiska badawczego z poprzednim czujnikiem siły. . . . . . . . .19

3.4 Schemat stanowiska badawczego z nowym czujnikiem siły. . . . . . . . . . . .20

4.1 Schemat blokowy systemu MRROC++, z uwzglednieniem sposobu komunika-

cji miedzywatkowej ([7]). . . . . . . . . . . . . . . . . . . . . . . . . . . . . .24

4.2 Struktura ogólna procesów MP, ECP, EDP oraz VSP ([2]). . . . . . . . . . . .26

4.3 Wewnetrzna struktura jadra procesu MP ([2]). . . . . . . . . . . . . . . . . . .28

4.4 Wewnetrzna struktura jadra procesu ECP ([2]). . . . . . . . . . . . . . . . . .29

5.1 Schemat zaprojektowanej płytki dwustronnej, łaczacej karte Advantech PCI-

1751 z mikrokomputerowym sterownikiem czujnika siły Schunk F/T 65/5. . . .33

5.2 Schemat połaczenia płytki PCI NI-6034E z czujnikiem siły ATI Gamma F/T. .35

5.3 Schemat zaprojektowanej płytki dwustronnej, umozliwiajacej połaczenie karty

PCI NI-6034E z nowym czujnikiem siły ATI Gamma F/T. . . . . . . . . . . .36

6.1 Schemat czujnika Gamma firmy ATI ([25]). . . . . . . . . . . . . . . . . . . .40

6.2 Plik z informacja kalibracyjna czujnika Gamma FT6284 firmy ATI. . . . . . .44

7.1 Siec działan metodyedp_ATI6284_force_sensor::edp_ATI6284_force_sensor(void). 69

7.2 Siec działan metodyvoid edp_ATI6284_force_sensor::configure_sensor (void). 70

7.3 Siec działan metodyvoid edp_ATI6284_force_sensor::initiate_reading (void),

czesc I. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .71

7.4 Siec działan metodyvoid edp_ATI6284_force_sensor::initiate_reading (void),

czesc II. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72

7.5 Siec działan metodyvoid edp_ATI6284_force_sensor::wait_for_event(). . . . . 73

8.1 Stanowisko do badania aplikacji wielorobotowej z dwoma robotami IRp-6. Ko-

lorem zaznaczono nowe rozwiazanie . . . . . . . . . . . . . . . . . . . . . . .76

8.2 Robot IRp-6postumentw ujeciu kinematyki piecioosiowej. . . . . . . . . . . .79

8.3 Robot IRp-6on_trackw ujeciu kinematyki szescioosiowej z dołaczonym no-

wym stopniem swobody i chwytakiem. . . . . . . . . . . . . . . . . . . . . . .81

8.4 Robot IRp-6on_trackw ujeciu kinematyki szescioosiowej – koncówka robota. 82

8.5 Narzedzie zdefiniowane w notacjiTOOL_XYZ_ANGLE_AXIS. . . . . . . . . . 83

8.6 Narzedzie zdefiniowane w notacjiTOOL_AS_XYZ_EULER_ZY. . . . . . . . . 84

9.1 Instrukcje ruchu systemu MRROC++ ([2]). . . . . . . . . . . . . . . . . . . .91

9.2 Makrokroki i kroki ruchu w systemie MRROC++ . . . . . . . . . . . . . . . .93

134

SPIS RYSUNKÓW

9.3 Schemat realizacji ruchu efektorów w systemie MRROC++. . . . . . . . . . .94

9.4 Siec działan programu realizujacego zadanie rysowania, umieszczonego w pli-

ku mp_m_kd.cc, czesc 1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99


ku mp_m_kd.cc, czesc 2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .100


ku mp_m_kd.cc, czesc 3. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .101

10.1 Wykres ruchu w trzech kierunkach w wykorzystywanych kinematykach szescio-

i piecioosiowej dla robotówpostumenti on_track. . . . . . . . . . . . . . . . . 109

10.2 Wykres ruchu w osiach x oraz y dla wykorzystywanych kinematyk. . . . . . .109

10.3 Wykres ruchu w osiach x oraz z dla wykorzystywanych kinematyk. . . . . . . .110

10.4 Trójwymiarowy wykres trajektorii realizowanych w trakcie uczenia i powiela-

nia rysunku przez robotypostumenti on_track. . . . . . . . . . . . . . . . . . 111

10.5 Wykres ruchu w osiach x oraz y uczonej i powielanych trajektorii. . . . . . . .112

10.6 Wykres ruchu w osiach x oraz z uczonej i powielanych trajektorii. . . . . . . .112

10.7 Trójwymiarowy wykres trajektorii realizowanych w trakcie uczenia i powiela-


10.8 Wykres ruchu w osiach x oraz y uczonej i powielanych trajektorii. . . . . . . .114

10.9 Wykres ruchu w osiach x oraz z uczonej i powielanych trajektorii. . . . . . . .114

10.10Trójwymiarowy wykres trajektorii realizowanych w trakcie uczenia i powiela-


10.11Wykres ruchu w osiach x oraz y uczonej i powielanych trajektorii. . . . . . . .116

10.12Wykres ruchu w osiach x oraz z uczonej i powielanych trajektorii. . . . . . . .116

10.13Trójwymiarowy wykres trajektorii realizowanych w trakcie uczenia i powiela-


10.14Wykres ruchu w osiach x oraz y uczonej i powielanych trajektorii. . . . . . . .118

10.15Wykres ruchu w osiach x oraz z uczonej i powielanych trajektorii. . . . . . . .118

10.16Rysowanie linii przerywanej w przestrzeni dwuwymiarowej x,y. W osi z zazna-

czono odczytywana siłe. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .120

10.17Odczyt siły w trakcie uczenia rysunku (roboton_track). . . . . . . . . . . . . . 120

10.18Odczyt siły w trakcie powielania rysunku (roboton_track). . . . . . . . . . . . 121

10.19Odczyt siły w trakcie powielania rysunku (robotpostument). . . . . . . . . . . 121

10.20Wygenerowana w programie Matlab trajektoria o postaci linii przerywanej. . .123

10.21Trajektoria zrealizowana przez robotaon_trackw trakcie powielania rysunku. .124

10.22Trajektoria zrealizowana przez robotapostumentw trakcie powielania rysunku.124

10.23Odczyt siły w trakcie powielania idealnej linii przerywanej przez robotaon_track.125

135

SPIS RYSUNKÓW

10.24Odczyt siły w trakcie powielania idealnej linii przerywanej przez robotapostu-

ment. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .125

10.25Wygenerowana w programie Matlab trajektoria o postaci koła. . . . . . . . . .126

10.26Trajektoria zrealizowana przez robotaon_trackw trakcie powielania rysunku

koła. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .127

10.27Trajektoria zrealizowana przez robotapostumentw trakcie powielania rysunku

koła. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .127

10.28Odczyt siły w trakcie powielania idealnego koła przez robotaon_track. . . . . 128

10.29Odczyt siły w trakcie powielania idealnego koła przez robotapostument. . . . . 128

136

SPIS TABEL

Spis tabel

5.1 Sposób połaczenia oraz funkcje linii sygnałowych karty Advantech PCI-1751 i

mikrokomputerowego sterownika czujnika siły Schunk F/T 65/5. . . . . . . . .32

5.2 Sposób połaczenia oraz funkcje linii sygnałowych łaczacych karte PCI NI-

6034E z nowym czujnikiem siły Gamma F/T firmy ATI. . . . . . . . . . . . .34

6.1 Parametry czujnika siły Gamma. . . . . . . . . . . . . . . . . . . . . . . . . .38

6.2 Parametry kalibracji i rozdzielczosc czujnika siły Gamma. CON = Controller

F/T system, DAQ = 16 bit F/T System . . . . . . . . . . . . . . . . . . . . . .39

6.3 Rozdzielczosc pomiaru karty PCI-6034E. . . . . . . . . . . . . . . . . . . . .41

137

SPIS ZAWARTEGO KODU

Spis zawartego kodu

1 Watekforce_threadkomunikujacy sie z czujnikiem siły . . . . . . . . . . . . .53

2 Klasaedp_ATI6284_force_sensorrealizujaca sterownik nowego czujnika siły. .54

3 FunkcjaiBus* acquireBoard(const u32 devicePCI_ID)uruchamiajaca karte akwi-

zycji danych NI-6034E w systemieQNX. . . . . . . . . . . . . . . . . . . . . 58

4 Funkcjavoid mmult(float *a, unsigned short ra, unsigned short ca, unsigned

short dca, float *b, unsigned short cb, unsigned short dcb, float *c, unsigned

short dcc)wykonujaca przeliczenie wejsciowych pomiarów w woltach na jed-

nostki sił i momentów sił. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .66

5 Funkcjavoid releaseBoard(iBus *&bus)odłaczajaca karte akwizycji danych

NI-6034E w systemieQNX. . . . . . . . . . . . . . . . . . . . . . . . . . . . 67

6 Funkcjavoid RTConvertToFT(RTCoefs *coefs, float voltages[],float result[],BOOL

tempcomp)przeliczajaca podany szescioelementowy wektor napiec na odpo-

wiadajace im siły i momenty sił. . . . . . . . . . . . . . . . . . . . . . . . . .68

7 Plik display.h. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .74

8 Funkcjatool_xyz_aa_2_framerealizujaca przeliczenie współrzednych narze-

dzia z postaciTOOL_XYZ_ANGLE_AXISdo trójscianu. . . . . . . . . . . . . . 87

9 Funkcjatool_frame_2_xyz_aarealizujaca przeliczenie współrzednych narze-

dzia z postaci trójscianu doTOOL_XYZ_ANGLE_AXIS. . . . . . . . . . . . . . 88

10 Funkcjatool_axially_symmetrical_xyz_eul_zy_2_framerealizujaca przelicze-

nie współrzednych narzedzia z postaciTOOL_AS_XYZ_EULER_ZYdo ramki

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .89

11 Funkcjatool_axially_symmetrical_xyz_eul_zy_2_framerealizujaca przelicze-

nie współrzednych narzedzia z postaci trójscianu doTOOL_AS_XYZ_EULER_ZY. 90

12 Klasa definiujaca dodany generator mp_kd. . . . . . . . . . . . . . . . . . . .96

138

DODATKI

Dodatki

Do pracy została dołaczona płyta zawierajaca :

• niniejsza prace w wersji elektronicznej,

• kod sterownikaMRROC++wraz z wprowadzonymi zmianami,

• kod sterownika czujnika siły poza systemem MRROC++.

139

Documents

Praca dyplomowa magisterska - robotyka.ia.pw.edu.pl · Politechnika Warszawska Rok akademicki 2005/2006 Wydział Elektroniki i Technik Informacyjnych Instytut Automatyki i Informatyki