X - Politechnika Warszawska

Transkrypt

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 Zieliński
Opiekun naukowy pracy
prof. nzw. dr hab. Cezary Zieliński
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
framework MRROC++ (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 endeffector 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 on MRROC++,
• investigation of position-force control while performing the drawing task,
• implementation of a new force-torque sensor driver in MRROC++,
• implementation of direct and inverse kinematics problem for the IRp-6 robot with 5 degrees of freedom and an axially symmetric tool,
• design of a printed circuit board (PCB) to establish the communication between the Gamma 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 the MRROC++ system.
The implemented multi-robot application is a significant step towards the design of a controller 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
˛ si˛e w laboratorium 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 mikroprocesorowych sterowników osi oraz wprowadzenia otwartego systemu MRROC++, służacego
˛
do tworzenia programowych sterowników robotów, umożliwiono uruchomienie sterowania pozycyjno–
siłowego robota. Poszerzone możliwości pozwalaja˛ na użycie robota IRp-6 jako robota usługowego. Ze wzgl˛edu na cz˛este działanie robotów usługowych w środowisku nieuporzadkowanym,
˛
jednym z podstawowych czynników zapewnienia bezpieczeństwa i ich efektywnego działania,
jest precyzyjne sterowanie siła˛ wywierana˛ przez końcówk˛e robota.
Realizacja aplikacji wielorobotowej wykorzystujacej
˛ sterowanie pozycyjno–siłowe wymagała wyposażenia obu robotów w czujniki sił i momentów sił, które zamontowano w nadgarstkach. Konieczne było zaprojektowanie elektronicznego połaczenia
˛
nowego czujnika sił Gamma
FT6284 firmy ATI z karta˛ PCI-6034E firmy NI umieszczona˛ w komputerze, opracowanie sterownika tego czujnika oraz modyfikacja połaczenia
˛
czujnika FT3084 firmy ATI znajdujacego
˛
si˛e w laboratorium.
W ramach pracy zrealizowano nast˛epujace
˛ zadania:
• zaimplementowano generator trajektorii dla aplikacji dwurobotowej wykorzystujacej
˛ dwa
czujniki siły w środowisku MRROC++,
• zbadano możliwości działania nowego czujnika,
• zaimplementowano sterownik nowego czujnika siły w MRROC++,
• zaimplementowano proste i odwrotne zagadnienia kinematyki dla robota IRp-6 o 5 stopniach swobody, wyposażonego w narz˛edzie osiowosymetryczne,
• zaprojektowana płytk˛e umożliwiajac
˛ a˛ transfer informacji pomi˛edzy czujnikiem zamontowanym na końcówce robota, a karta˛ PCI umieszczona˛ w komputerze.
Zamontowanie nowego czujnika siły na robocie IRp-6 oraz zrealizowanie powyższych zadań znacznie rozszerzyło możliwości wykorzystania robota. Przeprowadzone badania potwierdziły sprawne działanie zaimplementowanego sterownika czujnika siły FT6284 oraz poprawność zmian i poprawek wprowadzonych w systemie MRROC++.
Aplikacja wielorobotowa opracowana w ramach pracy, realizujaca
˛ zadanie sterowania pozycyjnosiłowego dwoma robotami, jest istotnym etapem prac prowadzonych w laboratorium Robotyki
IAiIS, zmierzajacych
˛
do implementacji zadania układania kostki Rubika przez dwa manipulatory.
3
Podzi˛ekowania
Składam podzi˛ekowania wszystkim pracownikom Instytutu Automatyki i Informatyki Stosowanej Politechniki Warszawskiej, którzy służyli mi rada˛ i pomoca˛ w realizacji niniejszej pracy.
Przede wszystkim słowa podzi˛ekowania kieruj˛e do opiekuna i kierownika
mojej pracy prof. nzw. Cezarego Zielińskiego, który nadawał właściwy kierunek moim działaniom. Bez jego zaangażowania i wsparcia nie dotarłbym do
wielu przydatnych informacji i trudno byłoby mi osiagn
˛ ać
˛ wyznaczony cel.
Duża˛ pomoc otrzymałem także ze strony dr Wojciecha Szynkiewicza, który
udost˛epnił mi wiele przydatnych publikacji i służył swoja˛ rozległa˛ wiedza˛ w
dziedzinie kinematyki robotów, a także ze strony mgr Andrzeja Rydzewskiego,
który przekazał swoja˛ wiedz˛e dotyczac
˛ a˛ zagadnień zwiazanych
˛
z projektowaniem obwodów drukowanych.
Chciałbym również bardzo podzi˛ekować mgr Tomaszowi Winiarskiemu, z
którym mogłem na bieżaco
˛ konsultować wszystkie watpliwości
˛
pojawiajace
˛ si˛e
w trakcie pracy i którego wiedza oraz pomoc były nieocenione przy poznawaniu, a nast˛epnie modyfikowaniu systemu MRROC++.
Warszawa, maj 2006 rok
4
SPIS TREŚCI
Spis treści
1
Wst˛ep
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
13
3
4
5
6
Regulacja pozycyjno-siłowa . . . . . . . . . . . . . . . . . . . . . . .
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
System MRROC++
22
4.1
System robotyczny . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
22
4.2
Struktura sterownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
23
Projektowanie płytek drukowanych
30
5.1
Opis narz˛edzi 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 umożliwiajaca
˛ komunikacj˛e z czujnikiem Gamma FT6284 . . . . . . .
33
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
Wst˛epna implementacja sterownika czujnika siły poza systemem
6.2.4
MRROC++ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
42
Implementacja czujnika siły w systemie MRROC++ . . . . . . . . . .
46
5
SPIS TREŚCI
7
8
Opis funkcji i kod sterownika nowego czujnika siły Gamma FT6284 firmy ATI
50
7.1
Watek
˛ force_thread realizujacy
˛ odczyt danych z czujnika . . . . . . . . . . . .
52
7.2
Klasa edp_ATI6284_force_sensor : public edp_force_sensor realizujaca
˛ sterownik 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
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 pi˛ecioosiowa . . . . . . . . . . . . . . . . . . . . . . . .
78
8.3.3
Kinematyka sześcioosiowa . . . . . . . . . . . . . . . . . . . . . . . .
80
8.3.4
Narz˛edzie osiowosymetryczne . . . . . . . . . . . . . . . . . . . . . .
82
8.3.5
Kod funkcji realizujacych
˛
przeliczanie narz˛edzia w notacji
TOOL_XYZ_ANGLE_AXIS oraz TOOL_AS_XYZ_EULER_ZY . . . . .
9
86
Implementacja aplikacji wielorobotowej
91
9.1
Generator trajektorii w systemie MRROC++ . . . . . . . . . . . . . . . . . . .
91
9.2
Implementacja prostego generatora trajektorii wykonujacego
˛
ruch pozycyjno-
9.3
siłowy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
94
Aplikacja wielorobotowa . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
95
9.3.1
Wykorzystywane generatory . . . . . . . . . . . . . . . . . . . . . . .
95
9.3.2
Generatory wykorzystywane w aplikacji wielorobotowej . . . . . . . . 102
9.3.3
Interfejs użytkownika . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
9.3.4
Działanie aplikacji . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
10 Badania przeprowadzone w oparciu o nowa˛ aplikacj˛e wielorobotowa˛
107
10.1 Kalibracja robotów . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107
10.2 Badania poprawności działania kinematyk . . . . . . . . . . . . . . . . . . . . 110
10.2.1 Kwadrat . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111
10.2.2 Okr˛egi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 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 TREŚCI
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
1
WSTEP
˛
Wst˛ep
Celem pracy jest opracowanie sterownika systemu dwurobotowego realizujacego
˛
zadanie
uczenia oraz powielania nauczonego rysunku przez dwa roboty IRp-6, znajdujace
˛ si˛e w laboratorium Robotyki, Instytutu Automatyki i Informatyki Stosowanej, oraz przeprowadzenie badań
jego działania.
Oryginalny robot IRp-6 jest typowym manipulatorem przemysłowym i, ze wzgl˛edu na swoje tradycyjne zastosowania, przystosowany jest do realizacji zadania sterowania pozycyjnego. W wyniku wbudowania nowych mikroprocesorowych sterowników osi oraz wprowadzenia
otwartego systemu MRROC++, służacego
˛
do tworzenia programowych sterowników robotów,
możliwym stało si˛e uruchomienie sterowania pozycyjno–siłowego robota. Poszerzone możliwości pozwalaja˛ na prób˛e użycia robota IRp-6 jako robota usługowego.
Roboty usługowe, w przeciwieństwie do robotów przemysłowych, działaja˛ w środowisku
nieuporzadkowanym,
˛
dynamicznie si˛e zmieniajacym.
˛
W przyszłości, roboty usługowe b˛eda˛
pracowały w bezpośrednim otoczeniu czlowieka. W zwiazku
˛
z tym potrzebuja˛ wielu różnorodnych czujnków by efektywnie wykonywać swoje zadania. Jednym z podstawowych czynników
zapewnienia bezpieczeństwa i efektywnego działania robota w takim środowisku, jest precyzyjne sterowanie siła˛ wywierana˛ przez końcówk˛e robota.
Realizacja aplikacji wielorobotowej wykorzystujacej
˛ sterowanie pozycyjno–siłowe wymagała wyposażenia obu robotów w czujniki sił i momentów sił, które zamontowano w nadgarstkach. Konieczne było zaprojektowanie elektronicznego połaczenia
˛
nowego czujnika sił Gamma
FT6284 firmy ATI z karta˛ PCI-6034E firmy NI umieszczona˛ w komputerze, opracowanie sterownika tego czujnika oraz modyfikacja połaczenia
˛
czujnika FT3084 firmy ATI znajdujacego
˛
si˛e w laboratorium.
W ramach pracy zaplanowano realizacj˛e nast˛epujacych
˛
zadań:
• zaimplementowanie generatora trajektorii dla aplikacji dwurobotowej wykorzystujacej
˛
dwa czujniki siły w środowisku MRROC++,
• zbadanie możliwości 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, wyposażonego w narz˛edzie osiowosymetryczne,
• zaprojektowanie płytki umożliwiajacej
˛ transfer informacji pomi˛edzy czujnikiem zamontowanym na końcówce robota, a karta˛ PCI umieszczona˛ w komputerze.
Zamontowanie nowego czujnika siły na robocie IRp-6, znajdujacym
˛
si˛e w laboratorium
Robotyki IAiIS, oraz zrealizowanie powyższych zadań znacznie rozszerzyło możliwości wykorzystania 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 niezb˛ednym
do zrozumienia jego istoty oraz wykorzystania w robotyce.
2.1
Wprowadzenie
Wraz z rozwojem państw uprzemysłowionych oraz zmianami demograficznymi w nich zachodzacymi
˛
pojawia sie coraz wi˛eksza potrzeba automatyzacji produkcji oraz wzrasta zapotrzebownie na roboty usługowe. Wykorzystanie robotów pozwoli na utrzymanie produkcji na
dotychczasowym poziomie, a nawet jej wzrost, niezależnie od wyst˛epujacej
˛ tendencji do starzenia sie społeczeństw.
Dotychczas w przemyśle wykorzystywano głównie roboty przemysłowe, które pracowały
w środowisku uporzadkowanym
˛
i w zwiazku
˛
z tym nie wymagały dużej liczby czujników ani
nadmiernie dużej inteligencji, by efektywnie realizować swoje zadania. W przeciwieństwie do
nich nowa generacja robotów, tak zwanych robotów usługowych, musi doskonale działać w
otoczeniu człowieka, w środowisku, w którym ludzie żyja˛ na codzień. Jest to otocznie mało
uporzadkowane,
˛
zmieniajace
˛ si˛e dynamicznie. Aby sprostać wymaganiom stawianym robotom
usługowym, musza˛ si˛e one efektywnie w nim poruszać, co wymaga wykorzystania wielu różnorakich czujników. Roboty usługowe powinny również posiadać zdolność do szybkiego przetwarzania informacji.
Głównymi zmysłami wykorzystywanymi przez ludzi do wykonywania zadań manualnych
sa˛ wzrok i dotyk. Jeśli roboty usługowe maja˛ efektywnie działać w środowisku ludzkim, powinny posiadać podobne zmysły. Z tego powodu obecne badania koncentruja˛ si˛e na wykorzystaniu
czujników wizyjnych – kamer, oraz różnego rodzaju sensorów dotyku, czyli czujników siły.
Poniższa praca dotyczy sterowania siłowego robotami. W kolejnych podrozdziałach zostana˛
opisane podstawowe zagadnienia sterowania siłowego robotami.
2.2
2.2.1
Zasada działania czujnika siły
Opis mechaniczny
Zasady oddziaływania sił sformułował Izaak Newton. Trzecia zasada dynamiki mówi o wzajemności oddziaływań i jest cz˛esto nazywana zasada˛ akcji i reakcji.
Sformułowanie III zasady dynamiki:
Jeżeli ciało A działa na ciało B siła˛ FAB , to ciało B działa na ciało A siła˛ FBA , o takim
samym kierunku i wartości jak FAB , ale przeciwnym zwrocie.
Czujnik (inaczej przetwornik) reaguje na przyłożone 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
2
w osiach X,Y oraz Z.
Rysunek 2.1: Wektory sił i momentów przyłożonych do czujnika ([25]).
Siła przyłożona do przetwornika napr˛eża trzy symetrycznie umieszczone belki (pr˛ety wykonane z metalu) zgodnie z prawem Hooke’a:
σ = E
gdzie:
σ – napr˛eżenie belki (σ jest proporcjonalne do siły),
E – wartość bezwzgledna spr˛eżystości belki,
– siła przyłożona do belki.
Przetwornik jest monolityczna˛ struktura.˛ Belki sa˛ wykonane z metalu. To zmniejsza histerez˛e i zwi˛eksza sił˛e oraz odporność struktury na odkształcenia. Do belek dołaczone
˛
sa˛ półprzewodniki mierzace
˛ napr˛eżenia w belkach. Nazywane sa˛ one tensometrami. Oporność tensometrów zmienia si˛e wraz z przykładanym napr˛eżeniem. Wyraża to poniższy wzór:
∆ R = Sa Ro gdzie:
∆ R – zmiana rezystancji miernika siły,
Sa – współczynnik napr˛eżenia miernika siły,
Ro – rezystancja w stanie bez przyłożonej siły,
– siła przyłożona do miernika napr˛eżenia.
10
2
Urzadzenie
˛
elektroniczne dołaczone
˛
do czujnika siły mierzy zmian˛e 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 zainstalowane na komputerze.
dane
Czujnik siły
(przetwornik)
dane
Interfejs
elektroniczny
zasilanie
zasilanie
Urządzenie
przeprowadzające
akwizycję danych
Dane w postaci
cyfrowej
Karta akwizycji danych odbiera
Informację z przetwornika.
Odpowiednie oprogramowanie
przekształca otrzymany sygnał
do postaci sił i momentów sił
komputer
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óżnych miejscach. Z punktu widzenia sterowania siłowego najprzydatniejsze sa˛ pomiary sił wywieranych na końcówk˛e manipulatora. Siły oddziaływania moga˛ być mierzone pomi˛edzy kolejnymi członami manipulatora,
jednak w takim przypadku zamiast normalnie wykorzystywanych tensometrów, cz˛esto dokonuje si˛e pomiaru pośrednio, badajac
˛ prad
˛ płynacy
˛ przez silnik. Tego typu pomiar jest cz˛esto
wykorzystywany do wykrycia przecia˛żenia silnika.
Czujniki siły montowane w manipulatorach wykorzystywane sa˛ głównie do pomiaru siły
chwytu oraz siły z jaka˛ końcówka manipulatora oddziaływuje na otoczenie. Dzi˛eki temu możliwe 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
2.4
Pozycyjne i siłowe sterowanie robotami
Sterowanie pozycyjne wystarcza w przypadku, gdy manipulator służy do przemieszczania obiektów o precyzyjnie znanym położeniu. Polega ono na minimalizacji różnicy pomi˛edzy
zadawanym, a aktualnym położeniem wałów silników, czyli tak zwanym uchybem regulacji.
Położenie zadane jest określane w wyniku rozwiazania
˛
odwrotnego zagadnienia kinematyki
(dokładniej opisane w 8.3.1) dla kolejnych położeń końcówki manipulatora na zdyskretyzowanej czasowo trajektorii zadanej. Zadaniem regulatora jest minimalizacja uchybu pojawiajacego
˛
si˛e przy nada˛żaniu rzeczywistego położenia końcówki manipulatora za trajektoria˛ zadana.˛
W przypadku, gdy na torze ruchu końcówki manipulatora pojawia si˛e przeszkoda, uchyb
regulacji narasta. Powoduje to zwi˛ekszenie siły z jaka˛ końcówka oddziaływuje na przeszkod˛e.
Wraz ze wzrostem siły przeszkoda lub sam manipulator może ulec zniszczeniu. Dlatego też w
manipulatorach montuje si˛e układy zabezpieczajace,
˛ wykrywajace
˛ przecia˛żenia, które wyłacza˛
ja˛ silniki w celu unikni˛ecia przecia˛żenia i spalenia. Tego typu rozwiazanie
˛
chroni manipulator,
ale uniemożliwia wykonanie zadania. Sterowanie siłowe umożliwia zrealizowanie zadania nawet po natrafieniu na przeszkod˛e. W oparciu o pomiar, ruch manipulatora może zostać zmodyfikowany lub siła wywierana na przeszkod˛e może być ustawiona na taka,˛ która nie spowoduje
uszkodzenia zarówno obiektu b˛edacego
˛
przeszkoda,˛ jak i manipulatora.
Sterowanie pozycyjne jest cz˛esto wykorzystywane w przypadku, gdy robot porusza si˛e w
środowisku bez przeszkód lub gdy położenie przeszkód jest znane. Sterowanie siłowe umożliwia pchanie lub ciagni˛
˛ ecie przedmiotów z zadana˛ siła.˛ Połaczenie
˛
tych dwóch sposobów sterowania umożliwia realizacj˛e wielu zadań, na przykład realizacj˛e zadań rysowania.
2.5
Klasyfikacja sposobów sterowania siła˛
Sterowanie siłowe może być realizowane różnymi metodami. W pracy [15] przedstawiono
nastepujac
˛ a˛ klasyfikacj˛e metod i algorytmów sterowania siłowego rozróżnianych na podstawie
trzech kryteriów:
1. Ze wzgl˛edu na charakter sterowania w poszczególnych kierunkach:
– metody jednolite, w których sterowanie we wszystkich kierunkach operacyjnych ma jednolity charakter.
– hybrydowe, w których rozróżnia si˛e kierunki sterowania pozycyjnego i siłowego,
2. Ze wzgl˛edu na sposób sterowania siła:
˛
– impedancyjne, w których utrzymywana jest relacja dynamiczna mi˛edzy uchybem położenia, a siła,˛
12
2
– admitancyjne, w których regulowana jest bezpośrednio siła.
3. Ze wzgl˛edu 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 wyrażone jest w przestrzeni zadaniowej.
Niezależnie od przyj˛etej klasyfikacji można stosować dodatkowe kryteria podziału metod
np. ze wzgl˛edu na układ współrz˛ednych, w którym jest wyznaczane sterowanie. Regulator siły
i położenia może być realizowany w układzie współrz˛ednych kartezjańskich lub w układzie
współrz˛ednych wewn˛etrznych (przegubowych).
Istnieja˛ również tzw. zaawansowane metody sterowania siłowego obejmujace:
˛
1. techniki adaptacyjne:
a) pośrednie metody adaptacyjne:
– pośrednie adaptacyjne sterowanie impedancyjne,
– adaptacyjne jawne sterowanie siłowe,
b) bezpośrednie metody adaptacyjne:
– bezpośrednie adaptacyjne sterowanie admitancyjne,
– bezpośrednie adaptacyjne sterowanie pozycyjno-siłowe.
2. metody odporne (krzepkie) sterowania siłowego,
3. algorytmy uczace
˛ si˛e.
2.5.1
Regulacja pozycyjno-siłowa
W badaniach przeprowadzonych w ramach niniejszej pracy wykorzystano regulacj˛e siłowo–
pozycyjna,˛ zaimplementowana˛ w systemie MRROC++. Poniżej zostana˛ podane informacje na
temat tego sposobu regulacji. Obszerniejsze informacje na temat różnych algorytmów sterowania siłowego można znaleźć w pracy [15].
Regulacja pozycyjno–siłowa jest przykładem hybrydowej metody sterowania siłowego. Regulator pozycyjno–siłowy składa si˛e z dwóch niezależnych p˛etli sterowania:
– pozycyjnej,
– siłowej.
Każda z tych p˛etli posiada indywidualne prawo sterowania. Na rysunku 2.3 pokazano schemat
hybrydowego regulatora pozycyjno-siłowego wykorzystywanego w systemie MRROC++.
13
generacja
trajektorii
i siły
zadanej
ECP
poleceń z ECP,
wysyłanie
odpowiedzi do
ECP
interpretacja
EDP_
MASTER
-
I -S
S
generacja
kroku
- regulatory
położenia
τ
EDP_SERVO
konwersja odczytu siły
kinematyki
zagadnienie
kinematyki
+ X c odwrotne θ +
d
XF +
Xp
EDP_FORCE
prawo sterowania
siłowego
Xmak
zagadnienie
proste
Rysunek 2.3: Hybrydowy regulator pozycyjno–siłowy ([7]).
Fd +
Xd
wariant 1
wariant 2
EDP_TRANS
EDP
Robot
θ
F
robot i
czujnik siły
2
14
2
W regulatorze pozycyjno–siłowym istnieje podział pomi˛edzy kierunkami, w których wywierana jest siła, a w których relizowany jest ruch. Podział dokonywany jest we współrz˛ednych
kartezjańskich, a realizowany jest poprzez zdefiniowanie diagonalnej macierzy S, nazywanej
macierza˛ selekcji. Macierz S jest macierza˛ kwadratowa˛ o wymiarze n równym liczbie stopni
swobody manipulatora. Elementy macierzy na diagonalii przyjmuja˛ wartości 0 lub 1, odpowiadajace
˛ przynależności do odpowiedniej p˛etli regulacji, siłowej lub pozycyjnej.
Każda p˛etla sterowania posiada własne prawo sterowania, cz˛esto bazujace
˛ na regulatorze
PID. Pozwala to na wyznaczenie przesuni˛eć (kroków):
XP – w p˛etli pozycyjnej,
XF – w p˛etli siłowej (na rys. 2.3 odpowiada generacji kroku),
XC – suma przesuni˛eć w p˛etli pozycyjnej i siłowej.
Wyznaczone przesuni˛ecie XC jest przekazywane do modułu obliczajacego
˛
odwrotne zagadnienie kinematyki. Dane otrzymane w wyniku obliczenia odwrotnego zadania kinematyki przekazywane sa˛ do regulatorów położenia, które przekształcaja˛ informacje otrzymane z kinematyki
na wymuszenie, które przekazane odpowiednim silnikom powoduje ruch robota (manipulatora).
Regulator pozycyjno–siłowy pozwala na wykonanie wielu zadań, w których kierunki regulacji siłowej i pozycyjnej sa˛ od siebie odseparowane. Przykłady takich zadań podano w rozdziale 2.4.
15
3
OPIS STANOWISKA BADAWCZEGO
3
Opis stanowiska badawczego
W tym rozdziale zostana˛ opisane wyposażenie stanowisk badawczych oraz budowa wyko-
rzystywanych robotów.
3.1
Budowa robota IRp6
Robot IRp-6 jest robotem przemysłowym, stosowanym do automatyzacji prac wykonywanych przez maszyny lub mogacym
˛
samodzielnie wykonywać pewne prace przy użyciu narz˛edzi
takich jak np.: spawarki łukowe, szlifierki.
W laboratorium Instytutu Automatyki i Informatyki Stosowanej znajduja˛ si˛e dwa roboty
IRp-6, z których jeden jest robotem umiejscowionym na postumencie, natomiast drugi, zamiast
postumentu (podstawy), posiada dodatkowo tor jezdny. W poniższej pracy, oba roboty b˛eda˛ posiadać t˛e sama˛ konfiguracj˛e mechaniczna,˛ a wi˛ec tor jezdny nie b˛edzie wykorzystywany.
Robot IRp-6 składa si˛e z cz˛eści manipulacyjnej i szafy sterowniczej. W skład cz˛eści manipulacyjnej, bez toru, wchodza˛ nast˛epujace
˛ podzespoły (3.1):
1. przegub (kiść),
2. rami˛e górne (przedrami˛e),
3. rami˛e dolne (rami˛e),
4. korpus obrotowy,
5. podstawa,
6. przekładnia śrubowa toczna θ,
7. przekładnia śrubowa toczna α,
8. nap˛ed ruchu v,
9. nap˛ed ruchu t,
10. nap˛ed ruchu θ,
11. nap˛ed ruchu α,
12. nap˛ed ruchu φ (wewnatrz
˛ korpusu).
16
3
Rysunek 3.1: Manipulator IRp-6 w wersji na postumencie.([26])
Manipulator pokazany na rys.3.1 ma pi˛eć obrotowych stopni swobody:
• obrót korpusu wzgl˛edem podstawy (φ),
• obrót ramienia (ramienia dolnego) (θ),
• obrót przedramienia (ramienia górnego) (α),
• pochylanie kiści (przegubu) (t),
• skr˛ecanie kiści (przegubu) (v).
Oś każdego stopnia swobody jest nap˛edzana silnikiem elektrycznym pradu
˛ stałego z przekładnia˛ i układem przeniesienia nap˛edu. Każda jednostka nap˛edowa jest wyposażona w elementy pomiarowe pr˛edkości obrotowej (pradnica
˛
tachometryczna) i położenia katowego
˛
(rezolwer). W szafie sterowniczej znajduja˛ si˛e układy sterowników mocy, wyposażonych w regulatory pr˛edkości typu PID z tachometrycznym sprz˛eżeniem zwrotnym oraz sterowniki położenia
osi, wykorzystujace
˛ sygnały z rezolwerów. Wartości zadane dla sterowników położenia sa˛ dostarczane przez mikroprocesorowy sterownik obsługujacy
˛ również pozostałe podzespoły szafy
sterowniczej.
Roboty IRp-6 znajdujace
˛ si˛e w laboratorium Instytutu Automatyki i Informatyki Stosowanej
zostały znaczaco
˛ zmodyfikowane. Sterowanie oboma manipulatorami odbywa si˛e przy użyciu
systemu MRROC++, umożliwiajacego
˛
mi˛edzy innymi implementowanie aplikacji wielorobotowych oraz sterowanie siłowe robotami.
17
3
Roboty zostały ostatnio rozbudowane o dodatkowe elementy zwi˛ekszajace
˛ ilość stopni swobody z sześciu do siedmiu plus stopień rozwarcia chwytaka w przypadku robota na torze oraz z
pi˛eciu do sześciu plus stopień rozwarcia chwytaka w przypadku robota na postumencie. Poniżej
na rysunku 3.2 przedstawiono schemat robota IRp-6 postument po 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 sześć obrotowych stopni swobody i dodatkowo możliwość zwierania i rozwierania szcz˛ek:
• obrót korpusu wzgl˛edem podstawy (φ),
• obrót ramienia (ramienia dolnego) (θ),
• obrót przedramienia (ramienia górnego) (α),
• pochylanie kiści (przegubu) (t),
18
3
• skr˛ecanie kiści (przegubu) (v),
• skr˛ecanie nowego 6 stopnia swobody (v 0 ),
• zaciskanie i rozwieranie szcz˛ek (chwytak) (h).
Dokładne schematy pokazujace
˛ wpływ zmian mechanicznych na wykorzystywane w niniejszej pracy modele kinematyki, wraz z ich opisem, znajduja˛ si˛e w rozdziale 8.3.
Obecne prace i modyfikacje robotów prowadzone w laboratorium maja˛ w przyszłości umożliwić implementacj˛e zadania układania kostki Rubika przez dwa manipulatory IRp-6. Poniższa
praca dotyczac
˛ a˛ mi˛edzy innymi implementacji nowego czujnika siły, niezb˛ednego przy układaniu kostki Rubika, jest ważnym elementem tych prac.
3.2
Stanowisko z dotychczasowym czujnikiem siły
Stanowisko badawcze w laboratorium Instytutu Automatyki i Informatyki Stosowanej wyposażone jest w nast˛epujace
˛ elementy:
czujnik siły
manipulator
sieć ethernet
komputer
operatora systemu
komputer sterujący
robotem
sprzęg robota
sprzęg czujnika
sieć ethernet
końcówka
manipulatora
(kiść)
mikrokomputerowy
sterownik
manipulatora
mikrokomputerowy
sterownik
czujnika siły
czujnik siły
sieć ethernet
komputer
operatora systemu
manipulator
Rysunek 3.3: Schemat
stanowiska badawczego z poprzednim czujnikiem siły.
komputer sterujący
robotem
końcówka
manipulatora
(kiść)
• komputer operatora systemu – tutaj za pośrednictwem interfejsu użytkownika (UI i SRP)
oraz pośrednio procesu MP operator sprawuje kontrol˛e nad pracujacym
˛
systemem
mikrokomputerowy
MRROC++ i w konsekwencji
manipulatorem.
Komputer operatora systemu wysyła, za
sprzęg robota
sterownik
manipulatora
pośrednictwem sieci ethernet,
polecenia do
komputera sterujacego
˛
robotem,
karta wejść - wyjść
przetwornik
• komputer sterujacy
˛ robotem – uruchomione w nim procesy: EDP, VSP i ECP sprawuja˛
bezpośrednia˛ kontrol˛e nad robotem i czujnikiem. Do komunikacji służa˛ dwie karty: I_R,
oraz karta 48 we-wy cyfrowych Advantech PCI-1751,
19
3
• karta I_R (sprz˛eg robota) – komunikuje si˛e ze sprz˛etowym interfejsem manipulatora.
• karta we-wy (sprz˛eg czujnika) – odbiera dane od sprz˛etowego interfejsu czujnika.
• port szeregowy – służy w pierwszej fazie do wysyłania i odbioru danych z mikrokomputerowego sterownika czujnika, a nast˛epnie jedynie do wysyłania tych danych,
• mikrokomputerowy sterownik manipulatora – z jednej strony ustawia wypełnienie sygnału PWM zasilajacego
˛
silniki manipulatora i zbiera odczyty położenia poszczególnych
stawów, z drugiej komunikuje si˛e z karta˛ I_R zainstalowana˛ w komputerze PC,
• mikrokomputerowy sterownik czujnika – zbiera dane z czujnika siły i wst˛epnie je przetwarza (m.in. filtruje). Ponadto komunikuje si˛e z karta˛ we-wy oraz portem szeregowym
komputera sterujacego
˛
robotem,
czujnik siły
manipulator siły i trzech składowych
• czujnik siły – urzadzenie
˛ sieć ethernet
służace
˛ do pomiaru trzech składowych
momentu sił umieszczone
w kiści manipulatora,
komputer sterujący
komputer
operatora systemu
robotem
końcówka
manipulatora
(kiść)
• manipulator - robot IRp - 6 na torze jezdnym.
3.3
Stanowisko do podłaczenia
˛
nowego czujnika siły
sprzęg robota
mikrokomputerowy
sterownik
manipulatora
Stanowisko badawcze w laboratorium Instytutu Automatyki i Informatyki Stosowanej wysprzęg czujnika
posażone zostało w nast˛epujace
˛ elementy:
sieć ethernet
mikrokomputerowy
sterownik
czujnika siły
czujnik siły
manipulator
sieć ethernet
komputer
operatora systemu
komputer sterujący
robotem
końcówka
manipulatora
(kiść)
sprzęg robota
mikrokomputerowy
sterownik
manipulatora
przetwornik
Rysunek 3.4: Schemat stanowiska badawczego z nowym czujnikiem siły.
• komputer operatora systemu – za pośrednictwem interfejsu użytkownika (UI i SRP) oraz
pośrednio procesu MP operator sprawuje kontrol˛e nad pracujacym
˛
systemem MRROC++
20
3
oraz manipulatorem. Komputer operatora systemu wysyła, za pośrednictwem sieci ethernet, polecenia do komputera sterujacego
˛
robotem,
• komputer sterujacy robotem – uruchomione w nim procesy: EDP, VSP i ECP sprawuja˛
bezpośrednia˛ kontrol˛e nad robotem i czujnikiem. Do komunikacji służa˛ karty: I_R oraz
NI-6034E firmy National Instruments,
• karta I_R (sprz˛eg robota)– komunikuje si˛e ze sprz˛etowym interfejsem manipulatora,
• karta we-wy – karta firmy National Instruments, typ NI-6034E, odbierajaca
˛ dane od
sprz˛etowego interfejsu czujnika,
• mikrokomputerowy sterownik manipulatora,
– ustawia wypełnienie sygnału PWM zasilajacego
˛
silniki manipulatora i zbiera odczyty
położenia 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 robotem, 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 kiści manipulatora,
• manipulator – robot IRp-6.
21
4
SYSTEM MRROC++
4
System MRROC++
System MRROC++ jest obiektowa˛ biblioteka˛ modułów, zapisana˛ w j˛ezyku C++, wspoma-
gajac
˛ a˛ tworzenie sterowników dla systemów wielorobotowych. MRROC++ służy do konstruowania sterowników dedykowanych konkretnym zadaniom, które maja˛ być realizowane przez
system wielorobotowy, wyposażony w różnorodne czujniki i urzadzenia
˛
współpracujace.
˛ Pracuje pod nadzorem systemu operacyjnego czasu rzeczywistego QNX w wersji 6.3.0.
Struktura i działanie systemu MRROC++ zostały szczegółowo opisane w [2] oraz [3]. Poniżej przedstawiono informacje uznane za najważniejsze, niezb˛edne do zrozumienia opisu przeprowadzonych prac oraz odnośniki do bardziej szczegółowych opisów poszczególnych zagadnień. Rozdział ten zawiera opis struktury sterownika jakim jest system MRROC++ oraz poszczególnych jego procesów.
4.1
System robotyczny
W systemie robotycznym ([2]) wyróżniamy trzy cz˛eści:
• efektory e – 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 obrabiarki,
• receptory r – czujniki rzeczywiste (sprz˛etowe) - zbieraja˛ informacje o stanie otoczenia
(np.: kamery, czujniki zbliżeniowe, dalmierze, czujniki sił i momentów sił),
• podsystem sterujacy
˛ c – składa si˛e zarówno ze sprz˛etu obliczeniowego (komputera lub
sieci komputerów) jaki i oprogramowania.
Stan systemu robotycznego wyraża 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 sterowania
C – przestrzeń stanów podsystemu sterowania
Dane z czujników rzeczywistych musza˛ zostać przeksztalcone w odczyty czujników wirtualnych v = fv (r, c, e), v ∈ V , gdzie V oznacza przestrzeń odczytów czujników wirtualnych. W
22
4
SYSTEM MRROC++
sterowniku MRROC++ podsystem sterujacy
˛ odpowiada za obliczenie trajektorii ruchu efektorów (ramion robotów).
Stan całego systemu można 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) cz˛eści:
c =< c0 , c1 , . . . , cne >
Każdy z podsystemów cj , j = 1, . . . , ne jest odpowiedzialny za sterowanie pojedynczym
efektorem, zaś c0 koordynuje prac˛e wszystkich efektorów (robotów). Każdemu efektorowi (robotowi) ej , j = 1, . . . , ne przyporzadkowany
˛
jest proces ECPj (Effector Control Process),
którego stan jest wyrażany przez cj , j = 1, . . . , ne . Procesem koordynujacym
˛
jest proces MP
(Master Process), którego stan wyraża si˛e przez c0 .
Każdy z procesów ECP dzieli si˛e na dwa podprocesy: pierwszy to właściwy ECP, który
odpowiada za realizacj˛e zadania przez efektor, drugi to EDP (Effector Driver Process), który
odpowiada za rozwiazanie
˛
prostego i odwrotnego zadania kinematyki.
4.2
Struktura sterownika
Sterownik MRROC++ ma hierarchiczna˛ struktur˛e funkcjonalna.˛ Poszczególne jego funkcje
realizowane sa˛ przez moduły – odr˛ebne procesy działajace
˛ w w˛ezłach sieci lokalnej. Sterownik
MRROC++ jest zaimplementowany w j˛ezyku C++ i używa systemu operacyjnego QNX 6.3.0.
Na rysunku 4.1 przedstawiono struktur˛e sterownika MRROC++.
23
4
SYSTEM MRROC++
Rysunek 4.1: Schemat blokowy systemu MRROC++, z uwzgl˛ednieniem sposobu komunikacji
miedzywatkowej
˛
([7]).
Poszczególne elementy systemu MRROC++ pełnia˛ nast˛epujace
˛ role:
watki
˛ UI i SRP – zapewniaja˛ komunikacj˛e sterownika z operatorem,
proces MP – koordynuje prac˛e całego systemu,
procesy ECP – realizuja˛ algorytmy sterowania poszczególnymi efektorami,
procesy EDP – sa˛ właściwymi sterownikami konkretnego typu efektora,
watki
˛ EDP_Servo – sa˛ odpowiedzialne za kontakt z warstwa˛ sprz˛etowa˛ nap˛edów efektora,
procesy VSP – odczytuja˛ i przetwarzaja˛ dane z czujników rzeczywistych.
24
4
SYSTEM MRROC++
Na najniższa˛ z warstw, które wyróżniamy w układzie sterowania, składaja˛ si˛e procesy odwołujace
˛ si˛e bezpośrednio do urzadzeń
˛
rzeczywistych (efektorów, receptorów). Nazywamy je
sterownikami. W przypadku robotów, procesy EDP rozwiazuj
˛ a˛ proste i odwrotne zadanie kinematyki, watki
˛ EDP_Servo realizuja˛ natomiast serwomechanizmy poszczególnych osi. Proces
EDP został szczegółowo opisany w [2].
Procesy VSP zajmuja˛ si˛e odczytem i przetwarzaniem danych z czujników rzeczywistych.
Procesy VSP i EDP sa˛ niezależne od zadania, natomiast zależne od sprz˛etu. Aby dołaczyć
˛
nowy efektor lub czujnik do systemu, konieczna jest modyfikacja odpowiedniego procesu.
W warstwie wyższej, procesy ECP (Effector Control Process) zajmuja˛ si˛e sterowaniem poszczególnymi efektorami, a w warstwie nadrz˛ednej proces MP (Master Process) zajmuje si˛e
koordynacja˛ procesów, które steruja˛ poszczególnymi efektorami. Procesy MP oraz ECP sa˛ zależne od zadania, natomiast niezależne od sprz˛etu. W celu zdefiniowania nowego zadania należy
zmienić fragmenty procesów ECP lub EDP.
Generatory trajektorii, określajace
˛ sposób realizacji zadania, wyst˛epuja˛ zarówno na poziomie procesu MP jaki i procesów ECP, sterujacych
˛
poszczególnymi efektorami (robotami).
W warstwie komunikacji użytkownika ze sterownikiem, która jest niezależna od zadania i
sprz˛etu, wyróżniamy watki
˛ UI i SRP. Watek
˛ UI (User Interface) obsługuje zlecenia operatora systemu. SRP (System Response Process) odpowiada za odbieranie komunikatów od innych
procesów.
W trakcie komunikacji pomi˛edzy poszczególnymi procesami systemu MRROC++ używane sa˛ bufory komunikacyjne. Każdy 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 procesu MP
• 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 dla ściśle współpracujacych
˛
robotów.
Podstawowe zadania procesu ECP
• realizacja programu użytkownika 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, jeśli robot działa niezależnie lub luźno współpracuje z innymi robotami.
Każdy z czterech procesów: MP, ECP, VSP oraz EDP można podzielić na dwie cz˛eści (rysunek
4.2).
Powłoka procesu
(stała)
Zawiera:
kod do komunikacji z pozostałymi procesami
kod 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 każdego z tych procesów wykonuje rejestracj˛e procesu w systemie operacyjnym.
Do jej zadań należy również komunikacja z innymi procesami oraz obsługa bł˛edów.
Zadania powłoki procesu MP
• powoływanie (po zakończeniu 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 użytkowego zawartego w jadrze
˛
dostarczonym przez programist˛e,
gdzie zawarte sa˛ instrukcje ruchowe.
Zadania powłoki procesu ECP
• 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 użytkowego zawartego w jadrze
˛
dostarczonym przez programist˛e,
gdzie zawarte sa instrukcje ruchowe.
Szczegółowe informacje na temat procesów można znaleźć w pracy [3].
Zadania jadra
˛ sa˛ różne dla każdego procesu. Rejestracja w systemie, powoływanie procesów potomnych oraz inicjalizacja kanałów komunikacyjnych pomi˛edzy jadrem,
˛
a procesami
współpracujacymi
˛
odbywa sie w cz˛eści stałej procesu. Struktura oraz funkcje jadra
˛ sa˛ zależne
od rodzaju i złożoności zadania.
Struktury jadra
˛ procesu MP i jadra
˛ procesu ECP przedstawiaja˛ rysunki 4.3 i 4.4.
27
4
SYSTEM MRROC++
Rysunek 4.3: Wewn˛etrzna struktura jadra
˛ procesu MP ([2]).
28
4
SYSTEM MRROC++
Rysunek 4.4: Wewn˛etrzna 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. Należało również usprawnić poła˛
czenie pomi˛edzy 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
˛
si˛e w laboratorium mikrokomputerowego sterownika czujnika siły FT3084 (Schunk F/T 65/5) z karta˛ Advantech
PCI-1751,
druga – umożliwiła komunikacj˛e pomi˛edzy nowym czujnikem siły ATI Gamma F/T, a karta˛
PCI National Instruments NI-6034E.
5.1
Opis narz˛edzi programowych. Program Protel DXP
Płytki drukowane zaprojektowano przy użyciu programu Protel. Program Protel firmy Altium jest systemem do projektowania obwodów drukowanych, pozwalajacym
˛
użytkownikowi
opracowywać i integrować układy FPGA. Poza kompletnym zestawem narz˛edzi do projektowania na poziomie PCB, Protel posiada zestaw komponentów dla projektów FPGA i instrumentów
wirtualnych, wspomagajacych
˛
uruchamianie w sprz˛ecie.
Informacje niezb˛edne do poznania sposobu działania programu oraz projektowania płytek
zaczerpni˛eto 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
˛
łacz
˛ ace
˛ kart˛e Advantech PCI-1751 z mikrokomputerowym sterownikiem czujnika siły Schunk F/T 65/5 wymagało wymiany ze wzgl˛edu na zmian˛e połaczeń
ścieżek. Poprzednia wersja płytki była zmodyfikowana poprzez połaczenie
˛
niektórych ścieżek
przewodami. Takie rozwiazanie
˛
było zawodne, gdyż mogłoby dojść do łatwego rozłaczenia
˛
si˛e
przewodów i braku komunikacji z czujnikiem. W zwiazku
˛
z tym zdecydowano si˛e na wymian˛e
płytki.
W tabeli 5.1 pokazano sposób połaczenia
˛
oraz funkcje sygnałowe karty Advantech PCI1751 i mikrokomputerowego sterownika czujnika siły Schunk F/T 65/5. Połaczenia
˛
na płytce
zostały poprowadzone pomi˛edzy:
– złaczem
˛
K77-68S firmy KYCON (ULTRA SCSI) o 68 wejściach–wyjściach, po stronie karty
we-wy Advantech-1751,
30
5
– złaczem DB-50 (SCSI) o 50 wejściach–wyjściach, po stronie skrzynki sterownika czujnika
siły firmy Schunk.
31
5
Sygnał
ULTRA SCSI
SCSI50
wyjście karty we-wy
1
38
2
37
3
36
4
35
5
34
6
33
7
32
8
31
masa
9
1
wejście karty we-wy
10
46
11
45
12
44
13
43
14
42
15
41
16
40
17
39
ACK
19
20
OBF
23
19
IBF
24
49
35
11
36
12
37
13
38
14
39
15
40
16
41
17
42
18
44
3
45
4
46
5
47
6
48
7
49
8
50
9
51
10
53
50
GND
STB
Kierunek przepływu danych
Tabela 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
Na podstawie połaczeń
˛
zdefiniowanych w tabeli 5.1 zaprojektowano, przy użyciu programu
Protel DXP, płytk˛e pokazana˛ na rysunku 5.1. Płytka ta ma wymiary 9.4 na 8.3 centymetra i
wpasowuje si˛e w zakupiona˛ skrzynk˛e o wymiarach 11 na 9 na 4 centymetrów.
Rysunek 5.1: Schemat zaprojektowanej płytki dwustronnej, łacz
˛ acej
˛ kart˛e 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 wejściach–
wyjściach oraz SCSI–50 o 50 wejściach–wyjściach. Z powodu braku odpowiednich bibliotek
zawierajacych
˛
potrzebne złacza
˛ w programie Protel DXP, zaprojektowano je od nowa i umieszczono w oddzielnych bibliotekach.
5.3
Płytka umożliwiajaca
˛ komunikacj˛e z czujnikiem Gamma FT6284
Po zaprojektowaniu pierwszej płytki, zaprojektowano druga˛ płytk˛e 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 pomi˛edzy:
– przetwornica˛ TEN 3–0523 firmy TRACO POWER, przetwarzajacej
˛ napi˛ecie wejściowe z
karty PCI NI-6034E (0V,+5V), na wymagane w czujniku napi˛ecie (±15V),
33
5
– złaczem
˛
K77-68S firmy KYCON (ULTRA SCSI) o 68 wejściach–wyjściach (w kierunku
karty PCI NI-6034E),
– złaczem
˛
D-25 o 25 wejściach–wyjściach (w kierunku czujnika siły).
D-25
TEN-3
ULTRA SCSI
(w kierunku czujnika)
(przetwornica)
(w kierunku karty PCI NI-6034E)
22, 23
8
+5V zasilanie
2, 3
13
0V zasilanie
6
14
10
9, 16
11
11
Sygnał
+15V (+VAN A ) zailanie
56
AGnd/AlGnd
−15V (−VAN A ) zasilanie
1
68
SG0 wyjście
3
34
SG0 odniesienie
7
33
SG1 wyjście
12
66
SG1 odniesienie
17
65
SG2 wyjście
20
31
SG2 odniesienie
8
30
SG3 wyjście
4
63
SG3 odniesienie
13
28
SG4 wyjście
18
61
SG4 odniesienie
9
60
SG5 wyjście
5
26
SG5 odniesienie
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 łacz
˛ acych
˛
kart˛e PCI NI-6034E z
nowym czujnikiem siły Gamma F/T firmy ATI.
Na rysunku 5.2 znajduje si˛e schemat połaczenia
˛
płytki PCI NI-6034E z czujnikiem siły ATI
Gamma F/T. Pośrodku znajduje si˛e skrzynka przetwornik, w której zamontowano przetwornic˛e
zamieniajac
˛ a˛ wchodzace
˛ do płytki zasilanie 0,+5V na wymagane do działania czujnika ±15V.
Na schemacie zaznaczono zmian˛e końcówek w stosunku do przedstawionego schematu poła˛
czeń producenta.
34
5
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 użyciu programu
Protel DXP, płytk˛e pokazana˛ na rysunku 5.3. Płytka ta ma wymiary 9.4 na 8.3 centymetra i
wpasowuje si˛e w zakupiona˛ skrzynk˛e o wymiarach 11 na 9 na 4 centymetrów.
35
5
Rysunek 5.3: Schemat zaprojektowanej płytki dwustronnej, umożliwiajacej
˛ 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 wejściach–
wyjściach i DB-25 o 25 wejściach–wyjściach oraz przetwornica TEN 3-0523. Z powodu braku
odpowiednich bibliotek zawierajacych
˛
potrzebne złacza
˛
oraz przetwornicy w programie Protel 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̇ wcześniej przedstawiony w rozdziale
3.2. Zmodyfikowane i uporzadkowane
˛
przez nowozaprojektowana˛ płytk˛e połaczenia
˛
z czujnikiem siły zostały opisane w tabeli 5.1 w rozdziale 5.
W poniższym podrozdziale zostana˛ podane podstawowe informacje o implementacji sterownika i działaniu zastanego czujnika. Bardziej szczegółowe informacje na temat sterownika
istniejacego
˛
już czujnika siły można znaleźć w [8].
Komunikacja z mikrokomputerowym sterownikiem czujnika Schunk może odbywać si˛e 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 wejść–wyjść cyfrowych
typu Advantech PCI-1751 na magistrali PCI. Służy ona do odbierania danych od mikrokomputerowego sterownika czujnika. Wysyłanie rozkazów do czujnika odbywa si˛e 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óżkowe złacze
˛
typu ULTRA SCSI. Z
kolei mikrokomputerowy sterownik czujnika, 50 nóżkowe złacze
˛
do taśmy SCSI. Taśmy wychodzace
˛ z obu złacz
˛ skrzyżowano przy wykorzystaniu nowozaprojektowanej płytki, opisanej
w rozdziale 5. Sposób połaczenia
˛
nóżek obu złacz,
˛ oraz funkcj˛e 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 możliwe w czasie krótszym niż 2,4 ms. Badania praktyczne wykazały, że średni czas odczytu wynosi
2,65 ms.
6.2
Implementacja sterownika nowego czujnika siły Gamma FT6284 firmy ATI
Sposób podłaczenia
˛
czujnika siły FT6284 został już wcześniej 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 po37
6
mi˛edzy czujnikiem, a karta˛ PCI.
W poniższym podrozdziale zostanie opisany sposób zaimplementowania nowego sterownika czujnika siły.
6.2.1
Parametry karty National Instruments PCI-6034E oraz czujnika siły Gamma firmy 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 si˛e schemat
wspomnianego czujnika.
Dopuszczalne przeciażenie
˛
Fxy
± 1200 N
Fz
± 4100 N
Txy
± 79 N-m
Tz
± 82 N-m
Sztywność
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
Średnica
± 75.4 mm
Wysokość
± 33.3 mm
Tabela 6.1: Parametry czujnika siły Gamma.
38
6
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
Rozdzielczość
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 rozdzielczość 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 bitowy F/T), w systemie kalibracji SI-65-5. Dane o systemie oraz rodzaju kalibracji sa˛ zawarte w
pliku ft6284.cal dołaczonym
˛
do zakupionego czujnika.
39
Rysunek 6.1: Schemat czujnika Gamma firmy ATI ([25]).
6
40
6
Parametry karty PCI-6034E firmy National Instruments
Karta 6034E firmy National Instruments posiada złacze
˛
PCI. Ma 16 wejść analogowych
pojedynczych lub 8 różnicowych (sparowanych) oraz 8 wejść cyfrowych. Rozdzielczość na
wejściu wynosi 16 bitów, maksymalna osiagana
˛
cz˛estotliwość próbkowania to 200 kS/s. Zakres
wejściowy karty znajduje si˛e w przedziale od ±0.05 do ±10 V. Karta posiada 8 wyjść cyfrowych. W tabeli 6.3 pokazano precyzj˛e pomiaru w zależności od wybranego zakresu sygnałów
wejściowych.
Wzmocnienie
Zakres wejściowy
Rozdzielczość
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: Rozdzielczość pomiaru karty PCI-6034E.
Karta posiada ochron˛e przeciwprzepi˛eciowa.˛ Przy załaczaniu
˛
– do ±25V, przy wyłaczaniu
˛
– do ±15V. Kolejka FIFO, w której karta umieszcza dane o zmierzonych sygnałach, ma pojemność 512 próbek. Rekomendowne jest odczekanie 15 minut od właczenia
˛
karty do rozpocz˛ecia
pracy. Dodatkowe informacje o karcie PCI-6034E firmy National Instruments można znaleźć w
[23].
Karta PCI-6034E usprawnia działanie czujnika w stosunku do poprzedniej karty Advantech.
Dzi˛eki nowemu sposobowi przeliczania danych otrzymywanych z czujnika, znacznie uproszczona została komunikacja z czujnikiem, co wpłyn˛eło na wzrost niezawodności działania tego
urzadzenia.
˛
W nowym rozwiazaniu
˛
nie jest wykorzystywana, niezb˛edna w poprzednim przypadku, skrzynka akwizycji danych firmy Schunk. Skrzynka ta przygotowuje dane odebrane z
czujnika, a karta Advantech komunikuje si˛e 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łości na komputerze, w którym znajduje si˛e karta PCI-6034E.
6.2.2
Metody implementacji sterownika czujnika siły w systemie QNX
W pierwszej fazie implementacji nowego sterownika zaznajomiono si˛e z możliwymi sposobami obsługi czujnika siły firmy ATI Gamma przez kart˛e PCI-6034E firmy National Instruments. Program obsługujacy
˛ czujnik siły powinien odpowiednio ustawić parametry karty PCI6034E w celu zebrania danych z czujnika, a nast˛epnie zebrane dane przetworzyć na informacj˛e
w postaci sił i momentów sił wskazywanych przez czujnik. Do realizacji przeliczenia sygnału
napi˛eciowego z karty PCI na sił˛e i momenty wymagane w systemie MRROC++ wykorzystano
41
6
bibliotek˛e ATI DAQ F/T Library (v.1.0.5). Bilioteka ta przelicza dostarczony sześcioelementowy wektor, zawierajacy
˛ napi˛ecia 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 cz˛eści sterownika obsługujacej
˛ kart˛e PCI-6034E, skorzystano z pakietu Measurement Hardware Development Kit (MH DDK), udost˛epnianego przez firm˛e National
Instruments. Pakiet ten jest zbiorem funkcji ułatwiajacych
˛
komunikacj˛e z karta˛ PCI oraz szablonów funkcji umożliwiajacych
˛
dost˛ep do sprz˛etu. Szablony te sa˛ wypełniane przez programist˛e,
w zależności od docelowego systemu operacyjnego, w którym ma działać karta.
W poczatkowym
˛
etapie prac, rozważano wykorzystanie do komunikacji z karta˛ PCI-6034E
biblioteki COMEDI [18]. Biblioteka ta jest jednak przeznaczona dla systemów linuksowych, z
tego wzgl˛edu bardziej odpowiednim do wykorzystania w systemie QNX 6.3.0 okazał si˛e pakiet
MH DDK firmy National Instruments.
6.2.3
Wst˛epna implementacja sterownika czujnika siły poza systemem
MRROC++
Pierwszym etapem niniejszej pracy było zaimplementowanie sterownika czujnika Gamma
firmy ATI w środowisku QNX 6.3.0. Ze wzgl˛edu na przewidywana˛ duża˛ ilość testów oraz niezb˛edne dokładne zbadanie właściwości karty oraz czujnika siły, zdecydowano si˛e na wst˛epna˛
implementacj˛e sterownika czujnika w systemie QNX 6.3.0 poza systemem MRROC++. Dzi˛eki
temu można było szybko sprawdzić wybrane rozwiazanie,
˛
bez potrzeby czasochłonnego przenoszenia 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 systemem MRROC++ został dołaczo˛
ny do niniejszej pracy i może służyć jako program do kontroli stanu czujnika lub spełniajacy
˛
funkcj˛e precyzyjnej wagi, gdy czujnik nie jest zamontowany na ramieniu robota.
Implementacja sterownika czujnika
Implementacj˛e sterownika poza systemem MRROC++ rozpocz˛eto od zebrania danych z
czujnika i przedstawienia ich w postaci jednostek napi˛ecia – woltów [V]. W tym celu napisano,
w oparciu o pakiet Measurement Hardware Development Kit (MH DDK) firmy National Instruments, program uruchamiajacy
˛ i konfigurujacy
˛ kart˛e, zbierajacy
˛ dane z czujnika i nast˛epnie
przetwarzajacy
˛ te dane do postaci wyjściowej w woltach.
W trakcie pisania programu zdecydowano si˛e na ustawienie rejestrów karty w taki sposób,
by zbierała ona 6 pomiarów z czujnika, w odst˛epie 10µs każdy, w zakresie ±10V , który odpowiada zakresowi sygnałów otrzymywanych z czujnika. W ten sposób wykorzystano pełne
możliwości szybkości zbierania danych przez kart˛e. Karta zbiera kolejno 6 pomiarów, z któ42
6
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 systemie QNX
6.3.0 oraz z programu firmowego ATI w środowisku Windows, potwierdziło poprawność zaimplementowanego rozwiazania
˛
– odczyty w woltach były identyczne. W kolejnym kroku skoncentrowano si˛e 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 bibliotek˛e ATI DAQ
F/T Library (v.1.0.5). Biblioteka ta pozwala na dołaczanie
˛
nowych czujników firmy ATI. Dane o kalibracji oraz możliwościach czujnika sa˛ wczytywane w trakcie jego inicjalizacji. Dzi˛eki
temu uzyskana została uniwersalność rozwiazania
˛
(łatwe podłaczenie
˛
innych czujników firmy
ATI) oraz możliwość uwzgl˛ednienia przypadków przecia˛żenia czujnika. Możliwe jest również
ustawianie rodzaju jednostek miar i sił, w jakich sa˛ podawane dane niezb˛edne do działania
programu oraz wyświetlane wyniki. Poniżej przedstawiono zawartosć wczytywanego w czasie inicjalizacji pliku konfiguracyjnego dla czujnika Gamma firmy ATI o oznaczeniu seryjnym
FT6284.
43
Rysunek 6.2: Plik z informacja˛ kalibracyjna˛ czujnika Gamma FT6284 firmy ATI.
<?xml version="1.0" encoding="utf-8"?>





<FTSensor Serial="FT6284" BodyStyle="Gamma" Family="DAQ" NumGages="6" CalFileVersion="1.0">
<Calibration PartNumber="SI-65-5" CalDate="7/21/2005" ForceUnits="N" TorqueUnits="N-m" DistUnits="m" OutputMode="Ground Referenced
Differential" OutputRange="20" HWTempComp="True" GainMultiplier="1" CableLossDetection="False" OutputBipolar="True">
<Axis Name="Fx" values=" -0.40709 -0.27318 0.34868 -33.58156 -0.32609 33.54162 " max="65" scale="4.5511972116989"/>
<Axis Name="Fy" values=" 0.35472 38.22730 -0.41173 -19.49156 0.49550 -19.15271 " max="65" scale="4.5511972116989"/>
<Axis Name="Fz" values=" 18.72635 -0.59676 19.27843 -0.56931 18.69352 -0.67633 " max="200" scale="1.41244051397552"/>
<Axis Name="Tx" values=" -0.40836 -0.95908 -33.37957 1.38537 32.52522 -0.51156 " max="5" scale="84.8843245576086"/>
<Axis Name="Ty" values=" 37.13715 -1.02875 -20.00474 -0.27959 -19.34135 1.42577 " max="5" scale="84.8843245576086"/>
<Axis Name="Tz" values=" -0.15775 -18.16831 -0.00133 -18.78961 0.31895 -18.38586 " max="5" scale="80.9472037525247"/>
<BasicTransform Dx="0" Dy="0" Dz="0.0134355078" Rx="0" Ry="0" Rz="0"/>
</Calibration>
</FTSensor>
6
44
6
Powyższy kod pochodzacy
˛ z pliku konfiguracyjnego dla czujnika Gamma FT6284 przekazuje do programu informacje na temat jednostek, kalibracji oraz zakresu obcia˛żeń czujnika.
Po przekształceniu danych z czujnika z postaci wyrażonej w woltach do postaci wyrażonej w jednostkach sił – Niutonach oraz momentów sił – Niutonometrach, ponownie przeprowadzono seri˛e badań porównujacych
˛
działanie programu napisanego w systemie QNX 6.3.0 i
programu firmy ATI uruchamianego w środowisku Windows. Tym razem porównywano odczyty przedstawiane w Niutonach i Niutonometrach. Do badań wykorzystano stalowy odważnik o
masie 800g. Wskazania wygenerowane w napisanym programie oraz programie firmowym były identyczne, co świadczy o poprawności zaimplementowanego algorytmu pobierania danych
z czujnika i dalszej ich obróbki, do postaci wyświetlanej w jednostakach sił i momentów sił.
Ze wzgl˛edu na poczatkowe
˛
niezerowe wskazania czujnika siły, wynikajace
˛ z ci˛eżaru urza˛
dzenia i jego orientacji w przestrzeni, wprowadzono dodatkowy mechanizm zerowania odczytu
(tzw. bias). Dzi˛eki temu odczyt nie jest zniekształcony przez mas˛e i położenie czujnika.
Po sprawdzeniu poprawności działania programu, zdecydowano si˛e na prób˛e usprawnienia
odczytu danych z czujnika, realizowanego przez kart˛e PCI-6034 firmy National Instruments,
poprzez wykorzystanie przerwań generowanych przez kart˛e. W pierwotnej wersji odczyt danych z czujnika odbywał si˛e w p˛etli. Po uruchomieniu programu nawiazywano
˛
komunikacj˛e z
karta˛ i ustawiano odpowiednie rejestry karty w celu poprawnego jej działania, a nast˛epnie karta
wykonywała odczyt danych z czujnika. Zakończenie odczytu i gotowość danych do odczytu były sygnalizowane poprzez ustawienie odpowiedniego bitu w jednym z rejestrów karty. Program
odczytywał ten rejestr w p˛etli i sprawdzał czy dane z czujnika sa˛ już gotowe do odczytu. Jednocześnie wprowadzono mechanizm uniemożliwiajacy
˛ zawieszenie si˛e w tej p˛etli. Jeśli 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 szybkości działania programu. W trakcie badań stwierdzono, że kolejne czynności: obsługa przerwania z karty i określenie, że dotyczy ono pojawienia si˛e pojedynczego pomiaru w kolejce FIFO,
dost˛ep do i odczyt pojedynczego pomiaru z rejestru kolejki FIFO, wyzerowanie rejestru informujacego
˛
o pojawieniu si˛e pomiaru; wykonywane sześciokrotnie w celu uzyskania kompletu
pomiarów z czujnika, powoduja,˛ że znacznie wydłuża si˛e czas uzyskania żadanych
˛
pomiarów
z karty. Znacznie efektywniejsze i niezawodne okazało si˛e rozwiazanie,
˛
w którym czeka si˛e
0.0007s na zebranie sześciu pomiarów z czujnika przez kart˛e, a nastepnie realizowane sa˛ jednorazowy dost˛ep do rejestru kolejki FIFO i odczyt do momentu opróżnienia kolejki z wcześniej
określonej w sposobie akwizycji liczby pomiarów. W celu uzyskania niezawodności takiego
rozwiazania
˛
(wykluczenia zawieszenia) wprowadzono mechanizm przerwania odpytywania rejestru FIFO karty po 0.002 sekundy, w przypadku braku pojawienia si˛e nowego pomiaru w
45
6
rejestrze kolejki FIFO. Takie rozwiazanie uniemożliwia zawieszenie w przypadku wadliwego
działania karty.
Wprowadzone rozwiazanie
˛
zostało wielokrotnie przetestowane i działało niezawodnie. Ze
wzgl˛edu na wymaganie szybkiego przetwarzania danych w systemie MRROC++ zdecydowano
si˛e na wykorzystanie rozwiazania
˛
z odpytywaniem. Rozwiazanie
˛
takie nie obcia˛ża w dużym
stopniu systemu, ponieważ czas realizacji odczytu jest bardzo krótki.
W programie pozostawiono jednak rozwiaznie
˛
opierajace
˛ si˛e na obsłudze przerwania z karty, które może być wybrane za pomoca˛ ustawienia odpowiedniej wartości w pliku nagłówkowym display.h.
Rozwiaznie
˛
opierajace
˛ si˛e na obsłudze przerwania z karty nie okazało si˛e lepsze od rozwia˛
znia z odpytywaniem, ze wzgl˛edu na niewielki wybór przerwań generowanych na podstawie
zapełnienia kolejki FIFO karty. Do wyboru sa˛ nast˛epujace:
˛ kolejka pusta, zapełniona w połowie i pełna. Pełna pojemność kolejki FIFO karty PCI-6034E to 512 pomiarów. Nie udało si˛e
uzyskać przerwania wskazujacego
˛
na pojawienie si˛e kompletu 6 danych odczytanych z czujnika
siły.
Badanie właściwości nowego czujnika siły
Po zaimplementowaniu sterownika czujnika siły w systemie QNX 6.3.0 przeprowadzono
seri˛e badań i porównań jego działania z programem firmy ATI w środowisku Windows. Nowy
program wskazywał te same odczyty co program firmowy, czujnik siły ATI Gamma działał poprawnie.
W zwiazku
˛
z tym zdecydowano si˛e na przejście do kolejnego etapu pracy, czyli do przeniesienia sterownika czujnika do systemu MRROC++.
6.2.4
Implementacja czujnika siły w systemie MRROC++
Czujnik siły w systemie MRROC++
W systemie MRROC++ (opisanym dokładniej w rozdziale 4) implementacja czujników powinna być standardowo powiazana
˛
z procesem VSP (Virtual Sensor Process). W procesie VSP
wykonywanoby pomiary siły, ECP zawierałoby regulator siłowy, a EDP pozycyjny. Tego typu
architektura była wygodna w implementacji i dawała si˛e łatwo dostosować do różnorodnych
zadań. Niestety miała istotne wady. Utworzony w oparciu o nia˛ regulator pozycyjno–siłowy był
wolny, a obcia˛żenie komputera, na którym uruchomiony był system, było znaczne. W zwiazku
˛
z tym zdecydowano si˛e na modyfikacje (opisane w [8]), które przyspieszyły działanie regulatora pozycyjno–siłowego.
Regulator pozycyjno–siłowy został wbudowany w całości, wraz z pomiarami siły, w proces
EDP. Rozszerzyło to wewn˛etrzny stan robota o wektor sił i momentów sił rejestrowanych w
jego końcówce, a czujnik siły stał si˛e proprioreceptorem.
46
6
Wykonywanie odczytów siły w procesie EDP pociagn˛
˛ eło za soba˛ stworzenie połaczenia
˛
pomi˛edzy EDP i VSP. Dzi˛eki temu VSP może przetwarzać pomiary siły i nadal pełnić rol˛e
„eksteroreceptora siłowego”. VSP uzyskuje nie tylko informacje o sile, ale także pozycji robota. Podobnie ECP może dodatkowo otrzymać pomiar siły od EDP i także go przetworzyć.
Przyj˛eto, że proces VSP bedzie wykorzystywany do wykrywania złożonych zdarzeń wyst˛epuja˛
cych w systemie, natomiast ECP do bieżacej
˛ modyfikacji parametrów (np.: wartości zadanych)
regulatorów pozycyjno–siłowych w EDP. Dzi˛eki tej redundancji możliwe stało si˛e pisanie różnorodnych, wydajnych obliczeniowo aplikacji.
System MRROC++ opiera si˛e na kilku procesach, które moga˛ składać sie z kilku watków.
Zaimplementowany regulator pozycyjno–siłowy wykorzystuje bezpośrednio 4 watki
˛ procesu
EDP i otrzymuje wartość zadana˛ siły i położenia od procesu ECP. Przepływ informacji na tle
struktury regulatora przedstawia rys. 2.3 na stronie 14. Dokładny opis poszczególnych watków
˛
można znależć w [8].
Implementacj˛e sterownika 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.
˛
Dołaczenie
˛
sterownika nowego czujnika do systemu MRROC++
Sprawdzony w systemie QNX 6.3.0 sterownik nowego czujnika ATI Gamma FT6284 przeniesiono do systemu MRROC++ i dowiazano
˛
do watku
˛
EDP_SERVO. W celu umożliwienia
łatwego ewentualnego późniejszego przeniesienia sterownika czujnika do procesu VSP, zachowano struktur˛e funkcji zawartych w szablonie VSP. Dotychczasowy program został zmodyfikowany i podzielony na nast˛epujace
˛ 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 zakończenia odczytu
Zmodyfikowano również struktur˛e źródeł systemu MRROC++ tworzac
˛ oddzielna˛ bibliotek˛e czujnika ATI Gamma FT6284, która jest kompilowana i dołaczana
˛
na etapie kompilacji do
pozostałej cz˛eści systemu. Podobnie zmodyfikowany został sposób dołaczania
˛
plików źródłowych dla znajdujacego
˛
si˛e już w laboratorium działajacego
˛
czujnika siły.
W ten sposób uległa zmianie hierarchia plików źródłowych dla czujników siły, przez co
uzyskano możliwość bardziej elastycznego dołaczania
˛
poszczególnych czujników siły (starego
i nowego) do odpowiednich robotów ( on_track lub postument). Wybór dowiazania
˛
odpowiedniego czujnika siły do wybranego robota odbywa si˛e poprzez zmodyfikowanie jednej linii w
47
6
plikach Makefile znajdujacych
˛
si˛e w katalogach irp6_on_track oraz irp6_postument.
Po modyfikacji hierarchii źródeł ujednolicono w systemie MRROC++ nazwy dotyczace
˛ sterowania siłowego z schunk na force. Dzi˛eki powyższym zmianom usystematyzowano i uproszczono nazewnictwo odnoszace
˛ si˛e do czujników siły.
W wyniku powyższych zmian otrzymano całkowicie zintegrowany z systemem MRROC++
sterownik nowego czujnika siły ATI Gamma FT6284 oraz ujednolicona˛ hierarchi˛e źródeł dla
nowego czujnika i poprzedniego czujnika ATI FT3084.
W trakcie przenoszenia sterownika nowego czujnika siły do systemu MRROC++ napotykano na wiele problemów zwiazanych
˛
z zależnościami czasowymi dotyczacymi
˛
zbierania i
udost˛epniania danych przez kart˛e PCI-6034E. Z tego wzgl˛edu wprowadzono kilka opóźnień,
bez których dane zebrane przez kart˛e nie byłyby poprawne. Sa˛ to:
• START_TO_READ_TIME_INTERVAL=0.0007s – określajace
˛ czas od żadania
˛
pomiarów do rozpocz˛ecia ich odczytu,
• INTERRUPT_INTERVAL=0.00007s – określajace
˛ czas oczekiwania zanim uaktywnione
zostanie przerwanie świadczace
˛ o pojawieniu si˛e danych w rejestrze,
• START_TO_READ_FAILURE=0.002s – określajace
˛ czas sp˛edzany w p˛etli oczekujacej
˛
na pojawienie si˛e danych w rejestrze.
Wszystkie powyższe opóźnienia zostały wprowadzone dla przyspieszenia odczytu danych
z karty. W trakcie badań okazało si˛e, że znacznie szybsze jest jednorazowe odebranie kompletu pomiarów z karty, niż na przykład sześciokrotne wykonywanie odczytu danych z rejestrów
karty w odpowiedzi na przerwanie z karty, informujace
˛ o pojawieniu si˛e pomiaru w rejestrze
kolejki FIFO karty.
Pierwsze opóźnienie START_TO_READ_TIME_INTERVAL wynika z możliwości przetwarzania przez kart˛e zebranych sygnałów i czasu niezb˛ednego do powiadomienia o zebraniu
danych, czyli modyfikacji odpowiedniego bitu rejestru karty. Drugie z opóźnień jest wymagane
w przypadku wykorzystania przerwania generowanego przez kart˛e, świadczacego o tym, że w
kolejce FIFO jest gotowy do odebrania komplet sześciu danych pomiarowych. Trzecie uniemożliwia zawieszenie w p˛etli odpytujacej
˛ rejestr karty o gotowe do odebrania wyniki. Jest to
niezb˛edne w przypadku, gdy karta z jakiegoś powodu nie działa poprawnie. Wszystkie powyższe opóźnienia zostały zdefiniowane po to, by przyspieszyć odczyt danych z karty oraz by ewentualne nieprawidłowe działanie tej cz˛eści systemu nie wpłyn˛eło na cały system MRROC++.
W trakcie dołaczania
˛
sterownika nowego czujnika siły do systemu MRROC++, zdefiniowano wiele komunikatów, które wyświetlaja˛ si˛e w oknie operatora systemu, informujac
˛ o stanie
pracy czujnika. Sa˛ to m. in. informacja o nadmiernym obcia˛żeniu czujnika oraz powrocie do
stanu normalnej pracy.
48
6
W przypadku przecia˛żenia czujnika, generowany jest komunikat o przecia˛żeniu, a do systemu jest dostarczany ostatni poprawny pomiar. Dzieje si˛e tak do momentu wyeliminowania
przecia˛żenia.
Po dołaczeniu
˛
czujnika przeprowadzono wiele badań w trybie testowym MRROC++. Nast˛epnie uruchomiono i zbadano działanie czujnika w rzeczywistości. Czujnik działał poprawnie,
aplikacja wielorobotowa (w wersji przed modernizacja˛ robotów na poczatku 2006r.), wykorzystujaca
˛ dwa manipulatory ze starym (FT3084) i nowym (FT6284) czujnikiem oraz taśmociag,
˛
uruchomiona pod koniec 2005 roku, potwierdziła poprawne działanie zaimplementowanego
sterownika czujnika siły.
49
7
7
OPIS FUNKCJI I KOD STEROWNIKA NOWEGO CZUJNIKA SIŁY GAMMA FT6284 FIRMY ATI
Opis funkcji i kod sterownika nowego czujnika siły Gamma FT6284 firmy ATI
W tym podrozdziale zostana˛ opisane wybrane, najważniejsze funkcje znajdujace
˛ si˛e w ko-
dzie nowego sterownika czujnika siły oraz lokalizacja nowych plików żródłowych i nagłówkowych, dotyczacych
˛
zaimplementowanego sterownika czujnika siły.
Program sterownika czujnika siły Gamma firmy ATI został napisany w jezyku C++. Do
napisania sterownika wykorzystano bibliotek˛e ATI DAQ F/T Library (v.1.0.5) oraz pakiet Measurement 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 j˛ezyku C. W przypadku
czujnika Gamma FT6284 jest to plik ft6284.cal, którego nazwa jest oznaczeniem seryjnym
zakupionego czujnika Gamma firmy ATI. Dodatkowo biblioteka ta zawiera funkcje umożliwiajace
˛ przeliczenie dostarczonych pomiarów w postaci woltów, otrzymanych z systemu akwizycji
danych, na siły i momenty sił. Biblioteka nie zawiera obsługi wejść–wyjść (I/O) sprz˛etowych.
Wszystkie funkcje zawarte w bibliotece można wykorzystywać bez ograniczeń prawnych. Biblioteka została zmodyfikowana tak, by wszystkie funkcje działały w j˛ezyku C++.
Pakiet Measurement Hardware Development Kit (MH DDK) jest kolekcja˛ przykładowego
kodu, dokumentacji sprz˛etowej oraz klas w j˛ezyku C++, które odpowiadaja˛ układom scalonym znajdujacym
˛
si˛e na kartach akwizycji danych firmy National Instruments. Zawarte sa˛ w
nim funkcje znacznie upraszczajace
˛ dost˛ep do sprz˛etu, dzi˛eki czemu można prościej zaimplementować obsług˛e kart akwizycji danych, w systemach nie wspieranych bezpośrednio przez
producenta.
Wspomniane pakiet i biblioteka zostały połaczone,
˛
a nast˛epnie znacznie zmodyfikowane i
rozbudowane, tak by jak najlepiej funkcjonowały w systemie QNX.
W zwiazku
˛
z wyraźnym podziałem plików na cz˛eść dotyczac
˛ a˛ odbierania danych z czujnika
ATI i ich obróbki oraz cz˛eści dotyczacej
˛ obsługi karty PCI-6034E firmy NI, zdecydowano si˛e
na podzielenie plików nagłówkowych i umieszczenie ich w dwóch katalogach, odpowiednio ati
oraz ni. Z tego samego wzgl˛edu, w plikach *.cc, bezpośrednio dotyczacych
˛
nowego sterownika
czujnika siły, zaznaczono, z która˛ z cz˛eści NI lub ATI jest zwiazany
˛
dany plik.
Poniżej podano lokalizacj˛e plików sterownika nowego czujnika siły zaimplementowanego
w systemie MRROC++ wraz z krótkim opisem spełnianych przez nich funkcji.
Pliki źródłowe, wchodzace
˛ w skład sterownika nowego czujnika siły, można podzielić na
dwie grupy.
Pliki źródłowe zawierajace
˛ funkcje niezb˛edne do parsowania pliku XML ft6284.cal, z zapisanymi danymi konfiguracyjnymi czujnika siły:
50
7
../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 źródłowe zawierajace
˛ funkcje realizujace
˛ obsług˛e karty akwizycji danych oraz zbieranie i przetwarzanie danych pomiarowych:
../mrrocpp/src/edp/
irp6s/
force.cc – zawiera funkcj˛e realizujac
˛ a˛ watek
˛ siłowy, niezb˛edny do inicjalizacji oraz
akwizycji danych z czujnika
ati6284/
edp_s.cc – główny plik sterownika czujnika zawierajacy
˛ funkcje ustawiajace
˛ parametry karty akwizycji danych oraz służace
˛ do komunikacji z karta˛ PCI-6034E,
ftconfig.cc – funkcje ustawiajace
˛ jednostki miar i wag używane do przeliczania
wejściowych napi˛eć na siły i momenty sił,
ftconvert.cc – funkcje główne, obsługujace
˛ przeliczanie wejściowych napi˛eć (w
woltach) na siły i momenty (w Niutonach i Niutionometrach) oraz wyświetlanie
postaci pliku kalibracyjnego,
ftrt.cc – funkcje przeliczajace
˛ wejściowe napi˛ecia na siły i momenty,
osiBus.cc – funkcje ustawiajace
˛ rodzaj magistrali PCI wykorzystywanej przez kart˛e,
osiUserCode.cc – uruchomienie karty akwizycji danych w systemie QNX, znalezienie 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, używanych do sprawdzenia
poprawności wyników, można tu również ustawić sposób odbierania danych z
karty, wykorzystujacy
˛ odpytywanie lub przerwanie.
ft6284.cal – zawiera tekst zapisany w postaci XML, opisujacy
˛ parametry konkretnego 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 stronie 44.
51
7
Pliki nagłówkowe umieszczono nast˛epujaco:
˛
../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 definicj˛e klasy edp_ATI6284_force_sensor, dziedziczacej
˛ po klasie
edp_force_sensor, która jest główna˛ klasa˛ nowego sterownika czujnika siły.
W dalszej cz˛eści rozdziału zostana˛ szczegółowo opisane najważniejsze cz˛eści kodu sterownika nowego czujnika siły.
7.1
Watek
˛ force_thread realizujacy
˛ 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_thread jest zdefiniowana w pliku force.cc w katalogu ../mrrocpp/src/edp/irp6s/. Poniżej
pokazano najbardziej znaczac
˛ a˛ cz˛eść kodu watku
˛ siłowego force_thread, obsługujacego
˛
czujnik
siły, zawarta˛ w pliku force.cc.
while(!TERMINATE){
try{
if (force_sensor_do_configure == TRUE; //!< polecenie konfiguracji czujnika
vs->configure_sensor();
//!< wst˛
epna 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 aż pomiar b˛
edzie 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
7
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: Watek
˛ force_thread komunikujacy
˛ si˛e z czujnikiem siły
Komunikacja z czujnikiem, realizowana w watku
˛ force_thread, odbywa si˛e za pomoca˛ funkcji configure_sensor(), wait_for_event() oraz initiate_reading(). Funkcje te sa˛ zdefiniowane w
pliku edp_s.cc.
7.2
Klasa edp_ATI6284_force_sensor : public edp_force_sensor realizuja˛
ca sterownik czujnika siły FT6284
Klasa edp_ATI6284_force_sensor jest klasa˛ konkretna˛ i dziedziczy po bazowej klasie abstrakcyjnej 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łówkowym edp_s.h w katalogu include/edp_ecp/ati6284/.
class edp_ATI6284_force_sensor : public edp_force_sensor{
private:
unsigned
uCount;
//!<
unsigned
uStatus;
//!< wskazanie zapełnienia kolejki
zmienna indeksujaca
˛
float last_correct[6];
short int overload ;
//!< wskazanie przecia˛żenia
short int show_no_result; //!< czy komunikat wyświetlony
unsigned Samples_Acquired, Total_Number_of_Samples;
short int
uValues[6]; //!<
binarne napi˛
ecie 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;
//!< ścieżka do pliku zawierajacego
˛
parametry konfiguracyjne czujnika
short ERROR_CODE;
53
7
edp_ATI6284_force_sensor();
//!< konstruktor uruchamiajacy
˛
czujnik i ustawiajacuy
˛
bias
virtual ~edp_ATI6284_force_sensor();
//!< destruktor odłaczaj
˛
acy
˛
kart˛
e 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łuż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 wyzwalajace
˛
void Number_of_Scans(void);
//!< ustawienie ilości 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 przerwań
void AI_Arming(void);
//!< uruchomienie wejść analogowych karty
void Interrupt_Service_Routine(void);
//!<
void AI_Start_The_Acquisition(void);
//!< start akwizycji
void InitMite(void);
//!<
void Input_to_Volts(void);
}; //!<
Interrupt service routine
Inicjalizacja chipu Mite
//!< przekształcenie wejściowych 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: Klasa edp_ATI6284_force_sensor realizujaca
˛ sterownik nowego czujnika siły.
Funkcje klasy, realizujace
˛ komunikacj˛e z czujnikiem siły, zorganizowano w taki sposób, by
możliwe było łatwe przeniesienie sterownika nowego czujnika siły do procesu VSP. Obecnie
sterownik czujnika siły jest umieszczony w procesie EDP i uruchamiany w watku
˛ force_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 zaimplementowanego sterownika czujnika Gamma FT6284. Zamieszczono tu metody i funkcje, które sa˛
przydatne dla użytkownika 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
7
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 struktury cal, przechowujacej dane o czujniku, wykorzystywanej w dalszych 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 kart˛e akwizycji danych, dotyczace
˛ zapełnienia kolejki FIFO, w której znajduja˛ si˛e pomiary.
Rezultat: Metoda zwraca NULL jeśli przerwanie nie zostało wygenerowane przez kart˛e, 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 manipulatora. Jeśli program działa w trybie testowym, obsługuje przerwanie generowane z
zegara komputera, na którym jest uruchomiony program.
Rezultat: Metoda zwraca NULL jeśli przerwanie nie zostało wygenerowane przez kart˛e. W
przeciwnym razie obsługuje przerwanie i uruchamia dalsze działanie programu.
Bł˛edy: Metoda nie zgłasza wyjatków.
˛
——————————————————————————————————————
55
7
Definicja: edp_ATI6284_force_sensor::edp_ATI6284_force_sensor(void)
Opis: Konstruktor czujnika realizuje wst˛epna˛ inicjalizacj˛e czujnika, wczytuje plik konfiguracyjny czujnika ft6284.cal, podłacza
˛
kart˛e do magistrali PCI i ustawia jej parametry.
Po ustawieniu karty konstruktor uruchamia obsług˛e przerwania generowanego z szafy
w przypadku trybu rzeczywistego lub z zegara komputera, na którym jest uruchomiony
program, w trybie testowym.
Rezultat: Metoda nie zwraca żadnej wartości (jest konstruktrem obiektu).
Bł˛edy: W przypadku braku możliwości podłaczenia
˛
przerwania od szafy lub od zegara komputera, 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łaczaj
˛
acy
˛ kart˛e z magistrali PCI oraz odmapowujacy
˛ pami˛eć karty. Odła˛
czane jest przerwanie generowane przez kart˛e.
Rezultat: Metoda nie zwraca żadnej wartości (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
˛ magistral˛e PCI, oraz podłaczaj
˛
aca
˛ kart˛e akwizycji danych do magistrali PCI. Po podłaczeniu
˛
karty do magistrali mapowana jest pamieć karty i ustawiane
sa˛ zmienne, niezb˛edne dla poprawnego działania funkcji z pakietu MH DDK, umożliwiajacych
˛
konfiguracj˛e karty oraz dost˛ep do rejestrów karty.
Rezultat: W przypadku pomyślnej inicjalizacji karty, zwracana jest struktura bus zawierajaca
˛
dane umożliwiajace
˛ ustawianie i odwoływanie si˛e do karty.
56
7
Bł˛edy: Funkcja zwraca wyjatki
˛ zwiazane
˛
z brakiem szukanej karty PCI, bł˛edem mapowania
pami˛eci karty oraz bł˛edami zwiazanymi
˛
z ewentualnie uruchamionymi przerwaniami generowanymi przez kart˛e akwizycji danych.
Listing:
iBus* acquireBoard(const u32 devicePCI_ID)
{
u32 devBAR0,BAR0sz,devBAR1,BAR1sz;
iBus *bus;
//!< Znalezienie zakresów pami˛
eci 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 pami˛
eci (BAR) w celu dost˛
epu do pami˛
eci 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 urzadzenia
˛
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
7
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 pami˛
eci
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: Funkcja iBus* acquireBoard(const u32 devicePCI_ID) uruchamiajaca
˛ kart˛e akwizycji danych NI-6034E w systemie QNX.
——————————————————————————————————————
58
7
Definicja: int ftconvert(float SampleReading[6],float SampleBias[6],float FT[6])
Lokalizacja: src/edp/ati6284/ftconvert.cc
Opis: Funkcja ta ustawia parametry narz˛edzia, wczytuje bias czujnika, a nast˛epnie uruchamia
funkcj˛e przeliczajaca
˛ dane z postaci woltów na jednostki sił i momentów sił.
Rezultat: Zwracany jest sześcioelementowy 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 narz˛edzia, wykorzystywana˛
w przypadku przesuni˛ecia i zmiany orientacji narz˛edzia robota wzgl˛edem końcówki. Siła
i momenty sił sa˛ odpowiednio przeliczone tak, by mierzone były siły i momenty sił na
końcówce narz˛edzia.
Rezultat: Uzyskiwana jest macierz ze zdefiniowanym narz˛edziem, 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 wejść karty.
Rezultat: Uruchomienie liczników wejść analogowych.
˛
——————————————————————————————————————
59
7
Definicja: void edp_ATI6284_force_sensor::AI_Board_Personalize(void)
Opis: Funkcja z pakietu MH DDK, umożliwiajaca
˛ ustawienie własnych parametrów karty
akwizycji danych. Mi˛edzy innymi sposobu konwersji sygnału i jego przetwarzania.
Rezultat: Karta zostaje skonfigurowana według potrzeb użytkownika.
˛
——————————————————————————————————————
Definicja: void edp_ATI6284_force_sensor::AI_End_of_Scan(void)
˛ wybranie sposobu zakończenia akwizycji
danych z czujnika siły.
Rezultat: Ustawienie sposobu zakończenia akwizycji pomiarów.
˛
——————————————————————————————————————
Definicja: void edp_ATI6284_force_sensor::AI_Interrupt_Enable(void)
Opis: Funkcja z pakietu MH DDK aktywujaca
˛ obsług˛e przerwania generowanego z karty.
Możliwy jest wybór zdarzenia generujacego
˛
przerwanie.
Rezultat: Ustawienie wymaganego sposobu generacji przerwania.
˛
——————————————————————————————————————
60
7
Definicja: void edp_ATI6284_force_sensor::AI_Scan_Start(void)
Opis: Funkcja z pakietu MH DDK, ustawiajaca
˛ sygnał rozpocz˛ecia akwizycji pomiarów przeprowadzanych przez kart˛e, dla poprzednio określonych wejść karty oraz ustalonej ilości
pomiarów.
Rezultat: Ustawienie sposobu rozpocz˛ecia akwizycji danych.
˛
——————————————————————————————————————
Definicja: void edp_ATI6284_force_sensor::AI_Start_The_Acquisition(void)
Opis: Funkcja z pakietu MH DDK uruchamiajaca
˛ akwizycj˛e danych realizowana˛ przez kart˛e.
Rezultat: Karta zbiera pomiary.
˛
——————————————————————————————————————
Definicja: void edp_ATI6284_force_sensor::AI_Reset_All(void)
Opis: Funkcja z pakiety MH DDK, zatrzymujaca
˛ wszystkie czynności jakie wykonuje obecnie
karta.
Rezultat: Zastopowanie wszelkich czynności wykonywanych przez kart˛e.
˛
——————————————————————————————————————
Definicja: void edp_ATI6284_force_sensor::Clear_FIFO(void)
Opis: Funkcja z pakietu MH DDK, usuwajaca
˛ pomiary z kolejki FIFO.
61
7
Rezultat: Kolejka FIFO pomiarów wykonanych przez kart˛e akwizycji danych jest pusta.
˛
——————————————————————————————————————
Definicja: void edp_ATI6284_force_sensor::Configure_Board(void)
Opis: Funkcja z pakietu MH DDK umożliwiajaca
˛ ustawienie parametrów wejść karty akwizycji danych, m. in. wzmocnienia na poszczególnych wejściach, ustawienia wejść różnicowych oraz sposobu wyrażenia pomiaru wykonanego na danym wejściu (np.: unipolarne,
bipolarne, czyli ze znakiem lub bez znaku).
Rezultat: Po uruchomieniu tej funkcji uzyskiwane jest ustawienie dwunastu analogowych wejść
karty akwizycji danych, do których podłaczone
˛
sa˛ sygnały. Sygnały te sa˛ sparowane przez
co otrzymywane sa˛ sześć wejść różnicowych, umożliwiajacych
˛
dokładniejszy pomiar.
˛
——————————————————————————————————————
Definicja: void edp_ATI6284_force_sensor::configure_sensor (void)
Opis: Metoda konfiguruje kart˛e akwizycji danych, a nast˛epnie dokonuje pojedynczej sekwencji sześciu pomiarów. Pomiary te sa˛ pierwszymi pomiarami z czujnika, które zostaja˛ przetworzone z postaci bitowej na wolty, a nast˛epnie na siły i momenty sił. Pomiar zostaje
zapisany jako bias czujnika, dzi˛eki czemu zwi˛ekszona zostanie dokładność kolejnych pomiarów.
Rezultat: Otrzymujemy skonfigurowane do odczytów kart˛e i czujnik. Zapisany zostaje bias
niezb˛edny do wyeliminowania wpływu pozycji czujnika w przestrzeni na pomiar. Jeśli pomiar jest prawidłowy, generowany jest komunikat o poprawnym skonfigurowaniu
czujnika.
Bł˛edy: W przypadku przecia˛żenia czujnika podczas pomiaru, generowany jest komunikat o
bł˛edzie i pomiar zostaje powtórzony.
——————————————————————————————————————
62
7
Definicja: void edp_ATI6284_force_sensor::get_reading (void)
Opis: Szkielet funkcji, niezb˛edny dla zachowania kompatybilności 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 kart˛e akwizycji danych, a nast˛epnie przetwarza
je z postaci bitowej do woltów, i nast˛epnie do jednostek sił i momentów. Przetworzone
dane sa˛ wysyłane do procesu VSP
Rezultat: Jeśli pomiar został wykonany poprawnie zmienna from_vsp.vsp_report przyjmuje
wartość VSP_REPLY_OK.
Bł˛edy: W przypadku przecia˛żenia czujnika podczas odczytu, generowany jest komunikat o
przecia˛żeniu pojawiajacy
˛ si˛e w UI użytkownika. Do systemu dostarczany jest ostatni poprawny pomiar. Po usuni˛eciu przecia˛żenia generowany jest komunikat o przywróceniu
poprawnego
działania
czujnika.
Metoda
zgłasza
również
wyjatki
˛
SENSOR_NOT_CONFIGURED jeśli czujnik nie został wcześniej skonfigurowany (parametr is_sensor_configured TRUE) oraz READING_NOT_READY jeśli parametr
is_reading_ready nie został wcześniej 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
˛ si˛e na karcie
akwizycji danych. Ustawiany jest adres fizyczny karty i dost˛ep do rejestrów karty.
Rezultat: Umożliwiony jest dost˛ep do rejestrów karty.
63
7
˛
——————————————————————————————————————
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 sześcioelementowy wektor sVolt z danymi wyrażonymi w woltach.
˛
——————————————————————————————————————
Definicja: void edp_ATI6284_force_sensor::Interrupt_Service_Routine(void)
Opis: Funkcja z pakietu MH DDK, reagujaca
˛ na pojawienie si˛e pomiaru w kolejce FIFO karty.
Rezultat: Do wektora uValues wstawiana jest kolejna wartość z kolejki FIFO pomiarów z czujnika siły.
˛
——————————————————————————————————————
Definicja: void edp_ATI6284_force_sensor::MSC_Clock_Configure(void)
Opis: Funkcja z pakietu MH DDK umożliwiajaca
˛ konfiguracj˛e opcji czasowych systemu akwizycji danych.
Rezultat: Ustawiany jest czas taktowania karty.
˛
64
7
——————————————————————————————————————
Definicja: void edp_ATI6284_force_sensor::Number_of_Scans(void)
˛ ustawienie ilości wymaganych pomiarów.
W przypadku zdefiniowania wykorzystania sześciu różnicowych sygnałów analogowych,
w pojedynczym pomiarze ("skanie") zostanie zmierzonych sześć różnicowych wejść karty.
Rezultat: Uzyskane zostaje ustawienie karty do wykonania ustalonej ilości pomiarów.
˛
——————————————————————————————————————
Definicja: void edp_ATI6284_force_sensor::terminate(void)
Opis: Szkielet funkcji, niezb˛edny dla zachowania kompatybilności 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. Nast˛epnie metoda oczekuje na przerwanie do momentu upłyni˛ecia czasu niezb˛ednego dla zebrania pomiarów. W tym czasie moga˛ si˛e wykonywać inne cz˛eści systemu
MRROC++.
Rezultat: Po upłyni˛eciu odpowiedniego czasu zmienna is_reading_ready ustawiana jest na
TRUE. W przeciwnym razie zmienna ta przyjmuje wartość FALSE.
Bł˛edy: Metoda zgłasza wyjatek
˛ jeśli sensor nie został uprzednio skonfigurowany
(SENSOR_NOT_CONFIGURED).
65
7
——————————————————————————————————————
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 wejściowych danych w postaci woltów
na jednostki sił i momentów sił. W tej funkcji realizowane jest przeliczenie wcześniej
zdefiniowanych macierzy, zawierajacych
˛
informacje o typie czujnika, jednostkach sił,
wymaganych wyjściowych jednostkach pomiaru oraz o zdefiniowanym narz˛edziu, przy
uwzgl˛ednieniu wektora zawierajacego
˛
dane pomiarowe, odczytane z czujnika siły, w woltach.
Rezultat: Wynikiem jest macierz z danymi wyrażonymi 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) {
wymnożenie dwóch macierzy
//!<
//!< a - macierz zawierajaca
˛
odpowiednio przetworzone dane o czujniku
//!< b - wektor zawierajacych
˛
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: Funkcja 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) wykonujaca
˛
przeliczenie wejściowych pomiarów w woltach na jednostki sił i momentów sił.
——————————————————————————————————————
66
7
Definicja: void releaseBoard(iBus *&bus)
Lokalizacja: src/edp/ati6284/osiUserCode.cc
Opis: Funkcja odmapowywujaca
˛ pami˛eć przydzielona˛ do obsługi karty i zwalniajaca
˛ wszelkie
przydzielone wcześniej zasoby systemu. Odłaczane
˛
jest przerwanie generowane przez
kart˛e. 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 pami˛eci przydzielonej karcie.
Listing:
void
releaseBoard(iBus *&bus)
{
//!< odmapowanie pami˛
eci 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: Funkcja void releaseBoard(iBus *&bus) odłaczajaca
˛
kart˛e akwizycji danych NI6034E w systemie QNX.
——————————————————————————————————————
67
7
Definicja: void RTConvertToFT(RTCoefs *coefs, float voltages[],float result[],BOOL tempcomp)
Lokalizacja: src/edp/ati6284/ftrt.cc
Opis: Funkcja z pakietu ATI realizujaca
˛ przeliczenie wejściowych danych w postaci woltów na
jednostki sił i momentów sił. W tej funkcji realizowane jest przemnożenie wcześniej zdefiniowanych macierzy, zawierajacych
˛
informacje o typie czujnika, jednostkach sił, wymaganych wyjściowych jednostkach pomiaru oraz o zdefiniowanym narz˛edziu, przez wektor
zawierajacy
˛ dane odczytane z czujnika w postaci woltów.
Rezultat: Wynikiem jest sześcioelementowy wektor z siłami i momentami sił.
˛
Listing:
void RTConvertToFT(RTCoefs *coefs, float voltages[],float result[],BOOL tempcomp) {
//!< uwzgl˛
ednij kompensacj˛
e 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: Funkcja void RTConvertToFT(RTCoefs *coefs, float voltages[],float result[],BOOL
tempcomp) przeliczajaca
˛ podany sześcioelementowy wektor napi˛eć na odpowiadajace
˛ im siły i
momenty sił.
——————————————————————————————————————
68
7
Wczytanie pliku kalibracyjnego
czujnika
createCalibration(calfilepath,index)
Uzyskanie dostępu do sprzętu
acquireBoard(const u32 devicePCI_ID)
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();
Ustawienie przerwań
Rysunek 7.1: Sieć działań metody edp_ATI6284_force_sensor::edp_ATI6284_force_sensor(void).
69
7
Ustawienie karty akwizycji danych
Clear_FIFO(); AI_Interrupt_Enable(); AI_Arming();
AI_Start_The_Acquisition();
Interrupt_Wait()
InterruptWait(NULL, NULL);
Odczyt zmierzonych danych z rejestru
kolejki FIFO karty akwizycji danych
TAK
Liczba odczytanych
pomiarów < 6
TAK
NIE
Komunikat o skonfigurowaniu
czujnika
show_no_result=0;
czas <
START_TO_READ_FAILURE
NIE
Komunikat o błędzie
konfiguracji czujnika
show_no_result=1;
Przeliczenie danych z postaci bitowej na
wolty
Input_to_Volts();
Clear_FIFO(); Samples_Acquired=0;
Przeliczenie woltów na jednostki sił i
momentów sił
NIE
Sprawdzenie poprawności
pomiaru (brak przeciążenia)
ftconvert(sVolt,sBias,force_torque);
TAK
Rysunek 7.2: Sieć działań metody void edp_ATI6284_force_sensor::configure_sensor (void).
70
7
Odczyt pomiaru z rejestru kolejki FIFO karty
akwizycji danych
TAK
TAK
Liczba odczytanych pomiarów < 6
czas < START_TO_READ_FAILURE
NIE
NIE
Poprawny odczyt danych
no_result=0;
Invalid_value=0;
Błąd odczytu czujnika
no_result=1;
Invalid_value=1;
Przeliczenie danych z postaci bitowej na
wolty
Input_to_Volts();
Clear_FIFO(); Samples_Acquired=0;
Przeliczenie woltów na jednostki sił i
momentów sił
ftconvert(sVolt,sBias,force_torque);
Rysunek 7.3: Sieć działań metody void edp_ATI6284_force_sensor::initiate_reading (void),
cz˛eść I.
71
0
Pomiar poprawny
last_correct[i]=force_torque[i];
Komunikat
Sensor initiate_reading OVERLOAD REMOVED
overload=0;
1
1
Komunikat
Sensor initiate_reading - wynik
otrzymany
show_no_result=0;
show_no_result = ?
overload = ?
0
0
Rysunek 7.4: Sieć działań metody void edp_ATI6284_force_sensor::initiate_reading (void), cz˛eść II.
from_vsp.vsp_report=VSP_REPLY_OK;
is_reading_ready=TRUE;
Pomiar niepoprawny
(wczytanie ostatniego poprawnego)
force_torque[i]=last_correct[i];
1
invalid_value = ?
overload = ?
0
Komunikat błędu
overload=1;
sensor_status=EDP_FORCE_SENSOR
_OVERLOAD
0
no_result = ?
Komunikat błędu
show_no_result=1;
sensor_status=EDP_FORCE_SENSOR
_READING_ERROR;
1
1
7
72
7
Ustawienie karty akwizycji danych i rozpoczęcie akwizycji
danych
Clear_FIFO(); AI_Interrupt_Enable(); AI_Arming();
AI_Start_The_Acquisition();
InterruptWait(NULL, NULL);
NIE
Czas < START_TO_READ_TIME_INTERVAL
TAK
is_reading_ready=TRUE;
Rysunek 7.5: Sieć działań metody void edp_ATI6284_force_sensor::wait_for_event().
73
7
7.4
Stałe modyfikujace
˛ działanie programu
W pliku display.h znajdujacym
˛
si˛e w katalogu src/edp/ati6284/ zdefiniowano stałe, które
służa˛ 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 pośrednich
#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 modernizacj˛e robotów, polegajac
˛ a˛ 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
si˛e przeprowadzenie modyfikacji w oprogramowaniu MRROC++ oraz zmodyfikowanie modeli
kinematyki robotów.
Poniższy rozdział opisuje prace wykonane w zwiazku
˛
z modyfikacjami wprowadzonymi
w laboratorium, stanowisko badawcze, na którym przeprowadzono badania oraz zmiany jakie
wprowadzono w systemie MRROC++ oraz kinematyce pi˛ecioosiowej, która została uruchomiona na zmodyfikowanym robocie postument. Pokazano również schematy nowej kinematyki
sześcioosiowej, wykorzystywanej w robocie on_track, która została gruntownie przetestowana
i cz˛eściowo zmodyfikowana.
8.2
Schemat stanowiska badawczego
Poniżej, na rysunku 8.1, zaprezentowano schemat stanowiska badawczego, na którym testowano nowa˛ aplikacj˛e wielorobotowa.˛ Poszczególne elementy połaczonych
˛
stanowisk robotów IRp-6 postument oraz on_track opisano w rozdziale 3. Należy zaznaczyć, że robot IRp-6
on_track jest pierwotnie wyposażony w tor jezdny, który w konfiguracji do poniższej pracy nie
b˛edzie wykorzystywany. W zwiazku
˛
z tym na rysunku 8.1 tor robota on_track nie jest uwzgl˛edniony. Warto zwrócić uwag˛e na zaznaczone nowe dołaczone
˛
elementy manipulatora i zmian˛e
umieszczenia czujników w stosunku do konfiguracji opisanych w rozdziale 3.
75
sprzęg robota
komputer sterujący
robotem
mikrokomputerowy
sterownik
manipulatora
przetwornik
manipulator
mikrokomputerowy
sterownik
czujnika siły
mikrokomputerowy
sterownik
manipulatora
manipulator
końcówka
manipulatora
(kiść)
końcówka
manipulatora
(kiść)
czujnik siły
Rysunek 8.1: Stanowisko do badania aplikacji wielorobotowej z dwoma robotami IRp-6. Kolorem zaznaczono nowe rozwiazanie
˛
komputer
operatora systemu
sieć ethernet
sieć ethernet
sprzęg czujnika
sprzęg robota
komputer sterujący
robotem
sieć ethernet
czujnik siły
8
76
8
8.3
Zmiany zwiazane
˛
z modelem kinematyki robotów
W tym podrozdziale zostana˛ opisane dwie kinematyki wykorzystane przy realizacji aplikacji dwurobotowej oraz dodane elementy obsługi narz˛edzia osiowosymetrycznego, o które
rozbudowano system MRROC++.
Robot IRp-6 on_track wykorzystuje kinematyk˛e sześcioosiowa,˛ natomiast robot postument
wykorzystuje kinematyk˛e pi˛ecioosiowa˛ dla narz˛edzia osiowosymetrycznego.
W trakcie pracy istotnie zmodyfikowano sposób zadawania i uruchamiania kinematyki w
systemie MRROC++. Poczatkowo
˛
przełaczenie
˛
kinematyki robota postument z pi˛ecioosiowej
na kinematyk˛e sześcioosiowa˛ realizowano poprzez modyfikacj˛e jednej linii w pliku Makefile
dotyczacym
˛
robota on_track lub postument.
W wyniku modyfikacji uruchomiono obsług˛e zmiany kinematyki wewnatrz
˛
systemu
MRROC++ poprzez wysłanie odpowiedniej instrukcji z generatora trajektorii z poziomu MP
lub ECP. Nowy sposób przełaczania
˛
kinematyki został zaimplementowany zarówno dla robota
postument jak i robota on_track. Dla robota on_track możliwy jest wybór pomi˛edzy:
• kinematyka˛ sześcioosiowa˛ bez toru jezdnego, z obsługa˛ nowodołaczonego
˛
członu i chwytaka;
• poprzednia˛ kinematyka˛ sześcioosiowa˛ z obsługa˛ toru jezdnego bez obsługi nowodołaczo˛
nego stopnia swobody i chwytaka.
Dla robota postument możliwy jest wybór pomi˛edzy:
• kinematyka˛ sześcioosiowa˛ obsługujaca
˛ nowa˛ konfiguracj˛e robota z szóstym stopniem
swobody i chwytakiem;
• kinematyka˛ piecioosiowa˛ wykorzystujac
˛ a˛ narz˛edzie osiowosymetryczne, nie uwzgl˛edniajaca˛ nowodołaczonego
˛
szóstego stopnia swobody i chwytaka.
W trakcie inicjalizacji systemu MRROC++ domyślnie ładowana jest kinematyka sześcioosiowa bez toru jezdnego dla robota on_track oraz kinematyka sześcioosiowa dla robota postument. Zmiana wykorzystywanej kinematyki jest możliwa w generatorze na poziomie MP lub
ECP.
Dzi˛eki powyżej opisanym modyfikacjom znacznie uproszczono i uporzadkowano
˛
hierarchi˛e
plików zwiazanych
˛
z modelami kinematyki w systemie MRROC++. Obecnie modele kinematyk dla robota on_track sa˛ zdefiniowane w pliku irp6_kin_ot.cc, a dla robota postument w pliku
irp6_kin_p.cc. Do wyboru wykorzystywanej kinematyki służy parametr kinematic_model, który domyślnie przyjmuje wartość 0 i wczytuje domyślne, sześcioosiowe modele kinematyk.
W plikach nagłówkowych irp6.h (różnych dla obu robotów), dołaczanych
˛
do plików zawierajacych
˛
modele kinematyk, sa˛ zdefiniowane parametry GEOM_SYNCH_POS_MODEL_*_*
77
8
modyfikujace
˛ i kalibrujace
˛ kinematyki.
W zwiazku
˛
z modernizacja˛ robotów na poczatku
˛
2006 roku należało zmodyfikować poprzednia˛ wersj˛e kinematyki piecioosiowej oraz sprawdzić nowa˛ kinematyk˛e sześcioosiowa.˛
Konieczne było dodanie obsługi nowych elementów manipulatorów dołaczonych
˛
do robotów
IRp-6 postument i on_track. Elementami dodanymi do każdego z robotów sa˛ jeden dodatkowy
stopień swobody oraz chwytak (gripper). Parametry nowych członów, pomimo iż nie sa˛ bezpośrednio wykorzystywane w kinematyce pi˛ecioosiowej do wyliczania prostego i odwrotnego
zadania kinematyki, sa˛ niezb˛edne do prawidłowego działania robota jako całości.
8.3.1
Kinematyka robotów
Manipulator składa si˛e z ogniw (członów) sztywnych połaczonych
˛
przegubami. Ogniwa
połaczone
˛
przegubami tworza˛ struktur˛e kinematyczna.˛
Zagadnienie kinematyki robotów sprowadza si˛e do odpowiedniego obliczenia wymaganego
położenia (a wi˛ec ustawienia silników poruszajacych
˛
poszczególnymi członami robota) tak, by
uzyskać zadana˛ orientacj˛e końcówki robota w przestrzeni.
Rozróżnia si˛e dwa zadania kinematyki:
proste – sprowadzajace
˛ si˛e do obliczenia, na podstawie podanych katów
˛
pomi˛edzy poszczególnymi członami oraz długości poszczególnych członów, pozycji końcówki robota wzgl˛edem układu bazowego,
odwrotnego – polegajacego
˛
na obliczeniu, przy zadanej pozycji końcówki robota w przestrzeni
3D, takiego ułożenia poszczególnych członów robota, by końcówka osiagn˛
˛ eła zadana˛ pozycj˛e. Zagadnienie to jest znacznie trudniejsze, dla szeregowych łańcuchów kinematycznych jakimi sa˛ wykorzystywane w labolatorium manipulatory, od prostego zadania kinematyki, ponieważ wymaga rozpatrzenia wielu możliwości osiagni˛
˛ ecia zadanego punktu w
przestrzeni trójwymiarowej przez końcówk˛e robota, jednocześnie uwzgl˛edniajac
˛ obszar
roboczy robota, jego ogranicznia i położenia osobliwe.
Istnieje wiele sposobów zadawania orientacji końcówki robota lub narz˛edzia w przestrzeni
trójwymiarowej. Zagadnienia kinematyki oraz notacje określania orientacji w przestrzeni zostały szczegółowo opisane w [1].
8.3.2
Kinematyka pi˛ecioosiowa
Zaimplementowana przed modernizacja˛ robotów w laboratorium Instytutu Automatyki i Informatyki Stosowanej kinematyka dla robota o pi˛eciu stopniach swobody wymagała przebudowania w zwiazku
˛
z nowa˛ konfiguracja˛ robotów. Informacje na temat kinematyki pi˛ecioosiowej
zaczerpni˛eto z pozycji [9]. Nast˛epnie rozpocz˛eto uaktualnianie starej kinematyki pi˛ecioosiowej,
78
8
tak by mogła działać na zmodyfikowanym robocie IRp-6 postument.
W założeniu poniższej pracy, robot postument ma nadal działać według kinematyki pi˛ecioosiowej, ale dołaczone
˛
elementy manipulatora musza˛ zostać odpowiednio obsłużone, ponieważ
istnieje możliwość zmiany ich położenia niezależnie od kinematyki pi˛ecioosiowej np.: poprzez
interfejs użytkownika UI.
W zwiazku
˛
z tym dokonano modyfikacji m.in. funkcji check_joints oraz dodano przepisywanie aktualnych pozycji nowodołaczonych
˛
elementów manipulatora.
Prace nad modyfikacja˛ kinematyki zakończone zostały badaniami, które potwierdziły prawidłowe dołaczenie
˛
zmodyfikowanej kinematyki pi˛ecioosiowej do systemu MRROC++. Na rysunku 8.2 pokazany został schemat przedstawiajacy
˛ robota IRp-6 postument z poszczególnymi
układami odniesienia, w szczególności z układem współrz˛ednych umiejscowionym w kołnierzu, którego poznanie jest istotne dla poprawnego zdefiniowania orientacji narz˛edzia osiowosymetrycznego, które jest opisane w rozdziale 8.3.4.
θ3′
3
3
y
x
a3
θ 4′
a2
θ4
5
z
4
x
5
x
θ5
4y
6
1
z
2
x
z
x
θ3
1
θ2
2
0z
6
x
y
d1
θ1 0 x
Rysunek 8.2: Robot IRp-6 postument w uj˛eciu kinematyki pi˛ecioosiowej.
79
8
8.3.3
Kinematyka sześcioosiowa
W zwiazku
˛
z dołaczeniem
˛
nowego stopnia swobody zwiazanego
˛
z chwytakiem, konieczne
stało si˛e opracowanie nowej kinematyki sześcioosiowej. Niniejsza praca wykorzystuje nowa˛
kinematyk˛e sześcioosiowa,˛ której postać została szczegółowo opisana w oddzielnej pracy.
Kinematyka sześcioosiowa jest wykorzystywana w robocie on_track. Poniżej na rysunku
8.3 zamieszczono schemat wykorzystywanej kinematyki sześcioosiowej, a na rysunku 8.4 zamieszczono powi˛ekszony schemat końcówki robota z zaimplementowana˛ kinematyka˛ sześcioosiowa,˛ która w pełni wykorzystuje nowe elementy dołaczone
˛
do robota IRp-6 po przeprowadonej modernizacji.
80
θ3
0
z
θ2
2
2
x
y
θ1 0 x
d1
1z
1
x
y
3
x
a3
5
x'
5
θ5
x
θ 4′
4
y d5
4
x
θ6
5
z
6
6
x
z
7
7
z
Rysunek 8.3: Robot IRp-6 on_track w uj˛eciu kinematyki sześcioosiowej z dołaczonym
˛
nowym stopniem swobody i chwytakiem.
θ4
a2
3
θ 3′
x
8
81
8
x
4
a3
x
θ5
5
7
z
θ 4′
x'
4
y d5
6
5
x
θ6
z
6
5
7
x
x
z
Rysunek 8.4: Robot IRp-6 on_track w uj˛eciu kinematyki sześcioosiowej – końcówka robota.
Zrozumienie schematu przedstawiajacego
˛
układy odniesienia robota IRp-6 on_track w uj˛eciu nowej kinematyki sześcioosiowej jest istotne dla poprawnego zdefiniowania orientacji narz˛edzia osiowosymetrycznego, które jest opisane w rozdziale 8.3.4.
8.3.4
Narz˛edzie osiowosymetryczne
W zwiazku
˛
z planowanym wykorzystaniem definicji narz˛edzia osiowosymetrycznego w
zmodyfikowanej kinematyce pi˛ecioosiowej oraz nowej kinematyce sześcioosiowej, dodano obsług˛e zadawania parametrów takiego narz˛edzia w notacji TOOL_XYZ_ANGLE_AXIS oraz TOOL_AS_XYZ_EULER_ZY. Na rysunkach 8.5 oraz 8.6 pokazano istot˛e wymienionych dwóch
sposobów zadawania orientacji narz˛edzia w przestrzeni. Wektor pokazany przerywana˛ linia˛
wskazuje kirunek orientacji narz˛edzia zakończonego w punkcie przesunietym o wektor Q wzgl˛edem kołnierza robota.
82
Yr
Zr
Q = [Qx, Qy, Qz]T
Vy
narzędzie
Vz
Rysunek 8.5: Narz˛edzie zdefiniowane w notacji TOOL_XYZ_ANGLE_AXIS.
końcówka robota
Xr
Vx
V = [Vx, Vy, Vz]T
8
83
r
r
x
y
z
T
r
ψ
r
’’
r
’
r
Rysunek 8.6: Narz˛edzie zdefiniowane w notacji TOOL_AS_XYZ_EULER_ZY.
r
’’
r
’
r
ϕ
x
r
’
’’
r
r
y
z
T
8
84
8
W przypadku wykorzystywanej kinematyki pi˛ecioosiowej dla robota postument, pi˛eć stopni
swobody oznacza, że pełne możliwości maipulacyjne (tzn. osiagni˛
˛ ecie dowolnej pozycji zadanej w przestrzeni roboczej) moga˛ być osiagni˛
˛ ete jedynie przez narz˛edzia osiowosymetryczne.
W przypadku innych narz˛edzi, każda zadana pozycja jest nierealizowalna z prawdopodobieństwem zbliżonym do jedności.
Definiowanie narz˛edzi, tzn. określanie pozycji końcówki roboczej narz˛edzia w układzie
współrz˛ednych kołnierza, jest mało intuicyjne. Najbardziej przejrzystym sposobem zadawania orientacji narz˛edzia jest notacja TOOL_XYZ_ANGLE_AXIS, w której sześć elementów
definiujacych
˛
położenie końcówki narz˛edzia oznacza odpowiednio:
pierwsze trzy Q = [Qx , Qy , Qz ]T – położenie końcówki w układzie kołnierza,
kolejne trzy V = [Vx , Vy , Vz ]T – wektor (przeliczany nast˛epnie do wersora) określajacy
˛ kierunek i zwrot osi narz˛edzia w układzie kołnierza.
W zależności od wykorzystywanej kinematyki możliwe jest również obliczenie, na podstawie
otrzymanych parametrów, kata
˛ obrotu α wokół osi narz˛edzia. W przypadku kinematyki pi˛ecioosiowej kat
˛ obrotu α nie jest obliczany, ale jest zmienny w czasie ruchu narz˛edzia. Jest to
wynikiem niewystarczajacej
˛ ilości stopni swobody (5). Dlatego właśnie dla manipulatorów z
pi˛ecioma osiami swobody tylko narz˛edzie osiowosymetryczne daje pełne możliwości manipulacyjne.
W przypadku zadawania pozycji narz˛edzia w postaci TOOL_AS_XYZ_EULER_ZY wymagane jest podanie pi˛eciu parametrów:
pierwsze trzy Q = [Qx , Qy , Qz ]T – określajacych
˛
położenie końcówki w układzie kołnierza,
kat
˛ nutacji φ – określajacy
˛ obrót wokół osi Z, mi˛edzy X, a X 0 ,
kat
˛ precesji ψ – określajacy
˛ obrót wokół osi Y 0 , mi˛edzy X 0 , a X 00 .
Na podstawie podanych parametrów kata
˛ nutacji i precesji, w wyniku odpowiednich przeliczeń,
otrzymywany jest wersor osi narz˛edzia V . Wersor V wraz z wektorem Q przesuni˛ecia końcówki narz˛edzia wzgl˛edem końcówki robota, opisuja˛ położenie narz˛edzia osiowosymetrycznego
wzgl˛edem końcówki robota.Ten sposób zadawania narz˛edzia osiowosymetrycznego jest jednak
znacznie trudniejszy od poprzednio opisanego.
Po zdefiniowaniu nowych sposobów definicji narz˛edzi dodano obsług˛e zadawania parametrów narz˛edzi poprzez interfejs użytkownika UI. Dzi˛eki temu, możliwe stało si˛e przetestowanie
działania nowych kinematyk wraz z zadanymi narz˛edziami bez uruchamiania robotów w trybie
rzeczywistym. Dodana możliwość okazała si˛e bardzo przydatna i wydatnie pomogła w analizie
w poczatkowej,
˛
testowej fazie badania działania robotów.
85
8
Końcówka narz˛edzia powinna być zadawana w odniesieniu do układu współrz˛ednych, umieszczonego w końcówce ramienia robota. W przypadku robota postument w konfiguracji z pi˛ecioma stopniami swobody jest to kiść, natomiast dla robota on_track z sześcioma stopniami swobody (bez toru) jest to punkt poczatku
˛
układu współrz˛ednych X6 Z6 , pokazanego na rysunku
8.3.
Układ współrz˛ednych do definicji narz˛edzia zwiazany
˛
z końcówka˛ dla robota postument w
kinematyce pi˛ecioosiowej różni si˛e od układu współrz˛ednych dla robota on_track w kinematyce
sześcioosiowej. Należy o tym pami˛etać przy definicji narz˛edzia w odpowiedniej kinematyce.
8.3.5
Kod funkcji realizujacych
˛
przeliczanie narz˛edzia w notacji
TOOL_XYZ_ANGLE_AXIS oraz TOOL_AS_XYZ_EULER_ZY
W tym podrozdziale zamieszczono kod funkcji realizujacych
˛
przeliczanie zadanej orientacji
w postaci TOOL_XYZ_ANGLE_AXIS oraz TOOL_AS_XYZ_EULER_ZY do postaci trójścianu
używanego w systemie MRROC++ i w przeciwna˛ stron˛e.
Funkcje te znajduja˛ si˛e w pliku edp_irp6s.cc w katalogu ../mrrocpp/src/edp/irp6s.
Funkcja command_buffer::tool_xyz_aa_2_frame
Funkcja przeliczajaca
˛ polecenie z postaci XYZ_ANGLE_AXIS do postaci trójścianu używanego wewnatrz
˛ systemu MRROC++.
/*--------------------------------------------------------------------------*/
void command_buffer::tool_xyz_aa_2_frame (c_buffer *instruction) {
// Przekształcenie definicji narz˛
edzia z postaci
// TOOL_XYZ_ANGLE_AXIS do postaci TOOL_FRAME oraz przepisanie wyniku
// przekształcenia do wewn˛
etrznych struktur danych
// TRANSFORMATORa
double alfa;
double x, y, z;
double kx, ky, kz;
// kat
˛ obrotu
// wspołrz˛
edne wektora przesuni˛
ecia
// 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 wartości pomnożone sa˛ przez kat
˛ 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 kata
˛
obrotu alfa i wartości funkcji trygonometrycznych
alfa = sqrt(kx*kx + ky*ky + kz*kz);
// korekta wartosci x, y, z
if((alfa
< dalfa) && (alfa > -dalfa))
{
86
8
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 poprawności macierzy
homog_matrix A_B_TOOL (tool);
if (!(A_B_TOOL.is_valid())) {
// bład: niewłaściwy 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: Funkcja tool_xyz_aa_2_frame realizujaca
˛ przeliczenie współrz˛ednych narz˛edzia z
postaci TOOL_XYZ_ANGLE_AXIS do trójścianu.
—————————————————————————————————————————————————————————
Funkcja reply_buffer::tool_frame_2_xyz_aa
Funkcja przygotowujaca
˛ odpowiedź w postaci XYZ_ANGLE_AXIS na podstawie trójścianu używanego wewnatrz
˛ systemu MRROC++.
/*--------------------------------------------------------------------------*/
void reply_buffer::tool_frame_2_xyz_aa (void) {
edzia z postaci
// 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
8
break;
default: // bład: z reply_type wynika, że odpowiedź nie zawiera narz˛
edzia
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: Funkcja tool_frame_2_xyz_aa realizujaca
˛ przeliczenie współrz˛ednych narz˛edzia z
postaci trójścianu do TOOL_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ójścianu używanego wewnatrz
˛ systemu MRROC++.
/*--------------------------------------------------------------------------*/
void command_buffer::tool_axially_symmetrical_xyz_eul_zy_2_frame (c_buffer *instruction) {
edzia z postaci
// 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;
//wspolrz˛
ecia
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;
//kat
˛ obrotu
double x, y, z;
double kx, ky, kz;
//wspołrz˛
ecia
//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 wartości pomnożone sa˛ przez kat alfa
88
8
kx = v6[0];
ky = v6[1];
kz = v6[2];
//obliczenie kata
˛
obrotu alfa i wartości 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: Funkcja tool_axially_symmetrical_xyz_eul_zy_2_frame realizujaca
˛ przeliczenie
współrz˛ednych narz˛edzia z postaci TOOL_AS_XYZ_EULER_ZY do ramki .
—————————————————————————————————————————————————————————
Funkcja reply_buffer::tool_axially_symmetrical_frame_2_xyz_eul_zy
Funkcja przygotowujaca
˛ odpowiedź w postaci XYZ_EULER_ZY na podstawie trójścianu
używanego wewnatrz
˛ systemu MRROC++.
/*--------------------------------------------------------------------------*/
void reply_buffer::tool_axially_symmetrical_frame_2_xyz_eul_zy (void) {
edzia z postaci
// 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
8
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: Funkcja tool_axially_symmetrical_xyz_eul_zy_2_frame realizujaca
˛ przeliczenie
współrz˛ednych narz˛edzia z postaci trójścianu do TOOL_AS_XYZ_EULER_ZY.
—————————————————————————————————————————————————————————
90
9
IMPLEMENTACJA APLIKACJI WIELOROBOTOWEJ
9
Implementacja aplikacji wielorobotowej
W tym rozdziale opisano implementacj˛e nowej aplikacji wielorobotwej dla nowej konfigu-
racji robotów, realizujacej
˛ zadanie rysowania przez dwa roboty IRp-6, znajdujace
˛ si˛e w laboratorium IAiIS. Badania oraz efekt prac zostana˛ zaprezentowane w kolejnym rozdziale. Nowa
aplikacja wielorobotowa wykorzystuje generatory, które istniały w systemie MRROC++, ale
wymagały modyfikacji w zwiazku
˛
ze zmiana˛ konfiguracji robotów, oraz nowoutworzony generator mp_gen_kd_generator..
9.1
Generator trajektorii w systemie MRROC++
W systemie MRROC++ ruch robota realizowany jest przy użyciu dwóch instrukcji ruchowych (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 końcowego,
Instrukcja Wait – monitoruje warunek poczatkowy.
˛
91
9
Generator trajektorii jest ostatnim argumentem procedury Move. Określa on warunek końcowy oraz sposób sterowania ruchem efektorów. Spełnienie warunku końcowego jest równoznaczne z wykonaniem instrukcji i powoduje przerwanie ruchu.
Przemieszczenie z punktu startowego do docelowego zostało zdefiniowane w systemie
MRROC++ jako sekwencja makrokroków. Makrokrok to pojedyncze zlecenie ruchu dla sterownika robota. Zawiera si˛e w nim pojedynczy przedział interpolacji. W jednym makrokroku
nast˛epuje przejście z jednego w˛ezła interpolacji do nast˛epnego. Przedział interpolacji podzielony jest na kroki ruchu, które sa˛ przyrostami położenia ramienia robota, realizowanymi w
pojedynczym okresie próbkowania, który dla obecnego okresu pracy algorytmu regulacji wynosi 2 ms. W pojedynczym kroku ruchu odbywa si˛e prosta liniowa interpolacja dla każdego
stopnia swobody. W kolejnych makrokrokach interpolacj˛e mi˛edzy w˛ezłami realizuje konkretny generator ruchu. Na rysunku 9.2 pokazane zostały kroki, makrokroki oraz sposób realizacji
makrokroku zadanego w procesie MP.
92
9
Proces MP
generator.first_step
generator.next_step
Zadany przyrost położenia / orientacji pomiędzy
dwoma punktami węzłowymi
Proces ECP
generator.first_step
generator.next_step
makrokrok = przedział interpolacji
kroki makrokroku
Proces EDP
Realizacja makrokroku
węzeł interpolacji
Servo_group
Rysunek 9.2: Makrokroki i kroki ruchu w systemie MRROC++
Utworzenie nowego generatora wymaga zdefiniowania nowej klasy, pochodnej od klasy bazowej (procesy MP lub ECP) i napisania dwóch jej metod : first_step oraz next_step. Dwie
różne procedury zostały wprowadzone ze wzgl˛edu na różnice pomi˛edzy obliczeniami w pierwszym i kolejnych krokach realizacji trajektorii. Obie metody moga˛ zlecać dowolne polecenia
do realizacji przez sterownik efektora – proces EDP. Sa˛ to zlecenia ruchu lub odczytu aktualnego położenia ramienia w dowolnej reprezentacji, zmiany parametrów lub korekty modelu
kinematycznego, zmiany stosowanych algorytmów regulacji lub przełaczenia
˛
wyjść i odczytu
wejść binarnych. Ogólny schemat realizacji ruchu efektorów w systemie MRROC++ pokazano
na rysunku 9.3.
93
9
polecenie MP dla
czujników wirtualnych
Proces MP
generator trajektorii na
poziomie MP
instrukcje ruchowe
Move( , , )
Wait( , , )
polecenia MP dla ECPj
Proces VSPl
first_step
next_step
wewnętrzne struktury
danych
Odczyty czujników
wirtualnych
odpowiedzi ECPj dla MP
polecenie ECP dla
czujników wirtualnych
Proces ECPj
generator trajektorii na
poziomie ECP
instrukcje ruchowe
Move( , , )
Wait( , , )
polecenia ECPj dla EDPj
Proces EDPj
first_step
next_step
danych
Proces VSPl
Odczyty czujników
wirtualnych
polecenia EDPj dla ECPj
proste i odwrotne zagadnienie kinematyki
danych
Wątek EDP_Servoj
obsługa serwomechanizmu danej osi
Rysunek 9.3: Schemat realizacji ruchu efektorów w systemie MRROC++.
W przypadku wykorzystywania generatorów na poziomie MP, na poziomie ECP uruchamiany jest generator transparentny. Jego rola sprowadza si˛e do przekazywania poleceń z procesu MP do znajdujacych
˛
si˛e niżej w hierarchii procesów EDP.
Szczegółowe informacje na temat generatorów i ich implementacji można znaleźć w [10].
9.2
Implementacja prostego generatora trajektorii wykonujacego
˛
ruch
pozycyjno-siłowy
Po zakończonym sukcesem dołaczeniu
˛
nowego czujnika siły do systemu MRROC++, rozpocz˛eto prac˛e 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 nast˛epnie ponowny ruch
w gór˛e. Generator ten był "wprawka"
˛ przed napisaniem aplikacji dwurobotwej, która pokaza94
9
ła poprawność działania czujnika siły oraz możliwości wykorzystania kinematyki dla robota z
zamontowanym narz˛edziem osiowosymetrycznym.
9.3
Aplikacja wielorobotowa
W tym podrozdziale opisano implementacj˛e zmodyfikowanego generatora trajektorii na poziomie MP, wykorzystujacego
˛
informacje otrzymywane zarówno z czujnika FT3084 oraz nowego czujnika siły Gamma FT6284.
Poprzez zaimplementowanie tego generatora sprawdzono poprawność działania nowego
czujnika siły i przetestowano rozległe zmiany wprowadzone w systemie MRROC++ po modyfikacji konfiguracji manipulatorów, znajdujacych
˛
si˛e w laboratorium Instytutu Automatyki i
Informatyki Stosowanej, opisanych w rozdziale 3.1.
Aplikacja wielorobotowa, sterujaca
˛ dwoma manipulatorami IRp-6, wykorzystuje dwie kinematyki: pi˛ecioosiowa˛ (z założenia wykorzystywana˛ dla narz˛edzia osiowosymetrycznego) dla
robota postument i nowa˛ kinematyk˛e sześcioosiowa˛ dla robota on_track (bez wykorzystania
toru jezdnego).
9.3.1
Wykorzystywane generatory
W systemie MRROC++ jest zdefiniowana klasa bazowa (klasa abstrakcyjna) generator, dla
generatorów trajektorii. Wszystkie generatory dla procesu MP, wykorzystywane przez instrukcj˛e Move, musza˛ być wywiedzione z tej klasy. Służy ona do wyznaczenia nast˛epnej wartości
zadanej oraz sprawdzenia spełnienia warunku końcowego. W programie użytkowym procesu
MP metody konkretnego generatora sprawdzaja˛ warunek końcowy, na podstawie informacji,
która zawarta jest w liście czujników i robotów. Jeżeli nie b˛edzie on spełniony, to wygeneruja˛ nowe wartości zadane, które zapisane zostana˛ w wewn˛etrznych strukturach danych robotów
konkretnych (obrazów robotów), umieszczonych na liście. W przypadku ścisłej współpracy robotów, wyst˛epujacej
˛ w opisanej w tym podrozdziale aplikacji wielorobotowej, generator na
poziomie MP, poza pobudzaniem do działania generatorów na poziomie ECP, ma obowiazek
˛
obliczenia kolejnych wartości zadanych dla wszystkich robotów bioracych
˛
udział w ruchu. W
tym przypadku, generatory na poziomie ECP ograniczaja˛ si˛e jedynie do przekazywania odebranych od MP przesyłek do procesów EDP.
Główna˛ cz˛eścia˛ każdego generatora sa˛ jego dwie metody first_step i next_step. Wprowadzone zostały dwie różne procedury, ponieważ obliczenia w pierwszym kroku realizacji trajektorii
sa˛ zazwyczaj różne od tych, wykonywanych w każdym nast˛epnym kroku. Obie metody moga˛
zlecać dowolne polecenia dla ECP, który przekazuje je procesowi EDP. Metoda first_step generuje pierwszy makrokrok ruchu, a next_step każdy nast˛epny. Dwie kolejne metody copy_data
oraz copy_generator_command służa˛ do kopiowania danych mi˛edzy buforami przesyłowymi,
95
9
a obrazami robotów w procesie MP.
Z bazowej klasy generator wywiedziona jest klasa abstrakcyjna robot_generator, która
dziedziczy wszystkie pola z klasy macierzystej generator. W klasie robot_generator zdefiniowane sa˛ ciała dwóch funkcji copy_data oraz copy_generator_command, które były wirtualne w
klasie generator.
Metoda copy_data kopiuje dane z buforów przesyłowych, otrzymanych z procesów ECP, do
odpowiednich obrazów robotów. Metoda copy_generator_command kopiuje polecenia stworzone przez generator, a umieszczone w obrazach robotów, do buforów przesyłowych odpowiednich procesów ECP.
W oparciu o klas˛e robot_generator został zaimplementowany nowy generator trajektorii,
wykorzystywany w nowej aplikacji wielorobotowej. Pozostałe, istniejace
˛ już wcześniej, generatory wymagaly modyfikacji w zwiazku
˛
ze zmiana˛ konfiguracji robotów. Poniżej zamieszczono kod źródłowy, znajdujacy
˛ si˛e w pliku nagłówkowym mp.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 sześcioosiowej (domyślna) na pi˛ecioosiowa,˛
96
9
• ustawienie parametrów narz˛edzia osiowosymetrycznego dla każdego z robotów;
• ustawienie rozwarcia chwytaka trzymajacego
˛
narz˛edzie osiowosymetryczne,
• zapami˛etanie uczonej, poprzez wodzenie końcówki robota on_track, trajektorii ruchu w
przestrzeni trójwymiarowej;
• równoczesne odtworzenie zapami˛etanej trajektorii zadanej przez dwa roboty postument
oraz on_track.
Kod aplikacji wielorobotowej został umieszczony w dwóch plikach:
mp_m_kd.cc – zawierajacy
˛ główna˛ cz˛eść procesu MP, realizujacego
˛
powielanie rysunku;
mp_gen_force_kd.cc – zawierajacy
˛ generatory wykorzystywane przy realizacji aplikacji wielorobotowej.
Do realizowanego przez aplikacj˛e wielorobotowa˛ zadania powielania rysunku wykorzystywane
sa˛ nast˛epujace
˛ generatory:
mp_kd_generator
ustawianie rozwarcia szcz˛ek, definiowanie wykorzystywanych narz˛edzi, przełaczenie
˛
kinematyk,
mp_drawing_teach_in_force_generator
uczenie trajektorii poprzez r˛eczne rysowanie trajektorii robotem on_track, powielanie zapami˛etanych trajektorii jednocześnie przez oba roboty.
mp_nose_run_force_generator
siłowe wodzenie za "nos" robota, czyli wykorzystanie wskazań czujnika siły do poruszania robotem i przesuwania końcówki robota do żadanej
˛
pozycji.
mp_short_move_up
krótki ruch w gór˛e po zakończeniu 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 generatorach jest przerywany na skutek spełnienia warunku końcowego (prawidłowe zakończenie),
wtedy zmienna break_state przyjmuje wartość FALSE. Przerywana˛ linia˛ zaznaczono przebieg
działań w przypadku przerwania działania generatora na skutek polecenia operatora (STOP).
Zmienna break_state przyjmuje wtedy wartość TRUE.
Program ma postać automatu skończonego, którego kolejne stany odwołuja˛ si˛e do generatorów ruchu, które z jednej strony wykorzystuja˛ informacje o sile i położeniu pochodzacym
˛
z
97
9
procesu EDP, a z drugiej przygotowuja˛ wartość zadana˛ dla wybranych składowych położenia i
siły, przesyłana˛ do procesu EDP.
98
B
C
Wysłanie sygnału start do wszystkich ECP
start_all (*robot_list_head);
TAK
Oczekiwanie na start od UI
wait_for_start ();
NIE
Rysunek 9.4: Sieć działań programu realizujacego
˛
zadanie rysowania, umieszczonego w pliku mp_m_kd.cc, cz˛eść 1.
A
1
Ustawienie narzędzia,
rozwarcia szczęk, wybór kinematyki
Move (robot_list_head, slhead, mp_kd_gen)
0
Inicjalizacja generatorów
mp_kd_generator mp_kd_gen(mp_msg, 8);
mp_nose_run_force_generator mp_nrf_gen(mp_msg, 8);
mp_drawing_teach_in_force_generator mp_dtif_gen(mp_msg, 8);
Utworzenie listy robotów
Wczytanie na podstawie pliku konfiguracyjnego
dostępnych robotów
Inicjalizacja komunikacji,ustawienie przerwań
signal(SIGTERM, &catch_signal);
mp_initialize_communication(config);
9
99
E
D
F
1
G
Wodzenie za nos
Move (robot_list_head, slhead, mp_nrf_gen)
break_state = TRUE lub FALSE;
Krótki ruch w górę
Uczenie trajektorii
mp_dtif_gen.teach_or_move=YG_TEACH;
Move (robot_list_head, slhead, mp_dtif_gen)
Wodzenie za nos
2
Wybór opcji
1 - Load drawing,
2 - Learn drawing
Wodzenie za nos
Ustawienie czujników
configure_sensor();
C
Wczytanie trajektorii
mp_load_file (mp_dtif_gen, mp_msg);
H
I
˛
B
A
9
100
E
F
NIE
FALSE
TAK
Zapisać trajektorię?
H
NIE
FALSE
Zakończenie procesu MP?
TRUE
TRUE
Zapisanie trajektorii
mp_save_file (mp_dtif_gen, POSE_FORCE_LINEAR, mp_msg);
FALSE
break_state?
TRUE
break_state?
Zakończnie programu
wait_for_stop (MP_THROW);
terminate_all (*robot_list_head);
Krótki ruch w górę
Odtwarzanie nauczonej trajektorii
mp_dtif_gen.teach_or_move=YG_MOVE;
Move (robot_list_head, slhead, mp_dtif_gen)
Wodzenie za nos
TAK
Narysować zapamiętany
rysunek?
G
I
˛
D
9
101
9
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ór˛e, realizowany na przykład po zakończeniu rysowania rysunku. Metoda ta została utworzona ze wzgl˛edu na cz˛este wykorzystywanie tego
typu ruchu. Żadana
˛
pozycja robota jest zadawana w notacji XYZ_EULER_ZYZ.
Rezultat: Robot unosi si˛e o 0.5 cm w gór˛e 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 generatora mp_kd_generator. Ustawienie kinematyk dla robota postument (pi˛ecioosiowa) oraz on_track (sześcioosiowa).
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 generatora mp_kd_generator. Ustawienie narz˛edzi osiowosymetrycznych
dla robotów postument oraz on_track. Definicje narz˛edzi różnia˛ si˛e z powodu wykorzystania różnych kinematyk dla poszczególnych robotów. Zadawane sa˛ szerokości rozwarcia chwytaków robotów.
Rezultat: Ustawienie narz˛edzi oraz rozwarcia szcz˛ek robotów.
——————————————————————————————————————
102
9
Definicja: BOOLEAN mp_nose_run_force_generator::first_step (mlst<sensor>* sensor_list,
mlst<mp_robot>* robot_list)
Opis: Pierwszy krok generatora mp_nose_run_force_generator, realizujacego
˛
wodzenie końcówka˛ robota. Robot jest podatny na sił˛e – operator wodzac
˛ końcówka˛ robota oddziałuje
na nia˛ siła,˛ która˛ odczytuje czujnik siły. Robot reaguje na odczyt przesuwajac
˛ si˛e w kierunkach zgodnych z przyłożona˛ siła.˛
Rezultat: Ustawienie parametrów niezb˛ednych 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 generatora mp_nose_run_force_generator realizujacego
˛
wodzenie końcówka˛ robota. Ta metoda realizuje ruch robota w zależności od sił mierzonych przez
czujnik siły.
Rezultat: Realizacja wodzenia końcówki robota.
——————————————————————————————————————
Definicja: BOOLEAN mp_drawing_teach_in_force_generator::first_step (mlst<sensor>* sensor_list, mlst<mp_robot>* robot_list)
Opis: Metoda realizujaca
˛ pierwszy krok generatora mp_drawing_teach_in_force_generator.
W zależności od parametru teach_or_move roboty działaja˛ w trybie nauki rysowanej
trajektorii (teach_or_move=YG_TEACH) lub powielania wcześniej nauczonego rysunku
(teach_or_move=YG_MOVE). W zależności od trybu pracy robot postument jest przygotowywany do ruchu lub nie. W trybie uczenia trajektorii wykorzystywany jest tylko robot
on_track
Rezultat: Roboty postument i on_track sa˛ ustawione do powielania rysunku lub robot on_track
jest przygotowany do nauki rysunku.
——————————————————————————————————————
103
9
Definicja: BOOLEAN mp_drawing_teach_in_force_generator::next_step (mlst<sensor>* sensor_list, mlst<mp_robot>* robot_list)
Opis: Metoda realizujaca
˛ kolejne kroki generatora mp_drawing_teach_in_force_generator. W
zależności od parametru teach_or_move roboty realizuja˛ rysowanie nauczonego rysunku
(teach_or_move=YG_MOVE) lub zapami˛etywana jest trajektoria zakreślana przez robota
on_track (teach_or_move=YG_TEACH). W trakcie uczenia zapami˛etywane sa˛ zdarzenia
zwiazane
˛
z oderwaniem pisaka od kartki i jego ponownym zetkni˛eciem. Dzi˛eki temu istnieje możliwość rysowania z odrywaniem pisaka od kartki. Komunikaty informujace
˛ o
kolejnych etapach rysowania: Uniesienie, Opuszczanie, Powierzchnia, Zetkni˛ecie, Unoszenie, sa˛ przekazywane do interfejsu użytkownika UI. W procedurze rysowania ruch w
osiach X i Y odbywa si˛e na podstawie wcześniej zapami˛etanej trajektorii, a w osi Z proces EDP da˛ży do utrzymania stałej siły docisku na odcinkach trajektorii zapamietanych
jako Powierzchnia oraz przesuwania końcówki pisaka na odpowiedniej wysokości nad
powierzchnia˛ pisania na odcinkach trajektorii zapami˛etanych jako Uniesienie. Dla procedury uczenia ruch w osiach X i Y odbywa si˛e na podstawie sił wywieranych przez operatora na końcówk˛e manipulatora, a w osi Z proces EDP sprawdza odczyty siły czujnika
i określa czy pisak jest uniesiony, czy dotyka powierzchni. Jednocześnie zapami˛etywana
jest trajektoria w płaszczyźnie X,Y.
Rezultat: Zapisywana jest trajektoria kreślona przez pisak zamontowany na robocie on_track
lub realizowane jest równoczesne rysowanie zapami˛etanej trajektorii przez dwa roboty.
Jeśli końcówki pisaka nie znajduja˛ si˛e na tej samej wysokości, to rysowanie rozpocznie
si˛e dopiero po wykryciu zetkni˛ecia si˛e z kartka˛ pisaków zamontowanych na obu robotach.
——————————————————————————————————————
9.3.3
Interfejs użytkownika
Interfejs użytkownika umożliwia poczatkow
˛
a˛ synchronizacj˛e systemu, a nastepnie informuje operatora o stanie, w którym znajduje sie system. Poprzez interfejs można wybrać startowa,˛
wcześniej zdefiniowana˛ pozycj˛e robota lub ustawić go wykorzystujac
˛ jedna˛ z dost˛epnych notacji
zadawania
orientacji
końcówki
robota,
na
przykład
XYZ_EULER_ZYZ,
XYZ_ANGLE_AXIS, współrz˛ednych wewn˛etrznych lub inkrementów wzgl˛edem położenia
synchronizacji.
Aplikacja wielorobotowa jest uruchamiana jako proces MP, którego parametry sa˛ zdefiniowane we wczytywanym przy uruchamianiu systemu MRROC++ pliku konfiguracyjnym kd.ini,
znajdujacym
˛
si˛e w katalogu ../configs/. Plik konfiguracyjny może być również załadowany po
104
9
uruchomieniu systemu. Polecenia operatora sterujace
˛ działaniem aplikacji sa˛ wydawane poprzez wysyłanie pulsów systemu QNX 6.3.0.
9.3.4
Działanie aplikacji
Proces uczenia i rysowania zapami˛etanych rysunków składa si˛e z kilku etapów, mogacych
˛
wyst˛epować 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 może zostać wcześniej zdefiniowana
w pliku konfiguracyjnym (tutaj plik kd.ini). Dzi˛eki temu można szybko przejść do zapami˛etanej
wcześniej pozycji, z której rozpocz˛ete zostanie zadanie rysowania lub nauki rysunku.
Wodzenie manipulatora
Końcówka manipulatora jest przemieszczana przez nauczyciela do wymaganej pozycji. Wodzenie odbywa si˛e poprzez zamian˛e siły wywieranej na końcówk˛e manipulatora we wszystkich
trzech kierunkach na przyrosty położenia koncówki manipulatora w tych samych kierunkach.
Uchyb siły przetwarzany jest na przyrost położenia poprzez czynnik proporcjonalny.
Uczenie trajektorii
Rysowanie jest podobne do etapu wodzenia manipulatora. Dodatkowo zapami˛etywana jest
trajektoria na płaszczyźnie kartki (dwie składowe ruchu), a regulator 3 składowej (pionowej),
utrzymuje stała˛ sił˛e docisku do powierzchni papieru. W etapie uczenia możliwe jest odrywanie
i ponowne dociśni˛ecie pisaka. Zdarzenia te wykrywane sa˛ w automacie skończonym, zaszytym
w procesie VSP. Podczas uczenia wykorzystywane sa˛ nast˛epujace
˛ stany automatu:
• powierzchnia (rysowanie po powierzchni),
• unoszenie (pisaka),
• uniesienie (wodzenie manipulatorem bez kontaktu pisaka z kartka),
˛
105
9
• opuszczanie (pisaka),
• zetkni˛ecie (stan pomi˛edzy opuszczaniem, a powierzchnia,˛ potrzebny do unikni˛ecia odbicia od powierzchni).
Przejście pomi˛edzy stanami nast˛epuje tylko w jednym kierunku (na przykład po "opuszczaniu”
jest "zetkni˛ecie”). Przejście pomi˛edzy stanami uzależnione jest od progowego kryterium wartości siły w osi pionowej. Uniesienie i ponowne zetkni˛ecie z powierzchnia˛ pisaka składa si˛e z
kolejnych kroków: "unoszenia”, "uniesienia", "opuszczania", "zetkni˛ecia" i "powierzchni". Ze
wzgl˛edu na zastosowanie progowego kryterium wartości siły w osi pionowej, operator powinien
gładko przesuwać pisak podczas uczenia rysunku, ponieważ szarpni˛ecia moga˛ zostać potraktowane jako chwilowe uniesienie pisaka.
Nauka rysunku rozpoczyna si˛e od "uniesienia" pisaka. Rysunek jest zapami˛etywany od
pierwszego zetkni˛ecia pisaka z powierzchnia˛ papieru.
Powielanie rysunku
Ten etap rozpoczyna si˛e z pisakiem uniesionym w powietrzu. Końcówka manipulatora
opuszcza si˛e do momentu osiagni˛ecia powierzchni papieru, wykrywanego przez proces VSP.
Nast˛epnie odtwarzany jest pierwszy zapami˛etany odcinek trajektorii z regulacja˛ docisku flamastra do kartki. Jeśli pojawi si˛e informacja, że danemu punktowi trajektorii towarzyszy uniesienie
pisaka, wtedy wykonywany jest krótki ruch w gór˛e. Kolejna faza to przemieszczenie manipulatora w powietrzu aż do momentu wykrycia, że kolejnemu, zapami˛etanemu punktowi trajektorii towarzyszy informacja o osiagni˛
˛ eciu powierzchni kartki. Wówczas pisak jest opuszczany
pionowo w dół do momentu wykrycia zetkni˛ecia z powierzchnia˛ kartki. Sekwencja ruchu jest
powtarzana do osiagni˛
˛ ecia końca zapami˛etanej trajektorii.
106
10
BADANIA PRZEPROWADZONE W OPARCIU O NOWA˛ APLIKACJE˛ WIELOROBOTOWA˛
10
Badania przeprowadzone w oparciu o nowa˛ aplikacj˛e wielorobotowa˛
W tym rozdziale opisano badania przeprowadzone przy wykorzystaniu aplikacji wieloro-
botowej realizujacej
˛ zadanie uczenia i powielania rysunku, opisanej w poprzednim rozdziale.
Wyposażenie 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 taśmociagu
˛ znajdujacym
˛
si˛e w obszarze
działania robotów. Oba roboty w poczatkowej
˛
fazie rysowania sa˛ umieszczone prostopadle do
taśmociagu
˛ i równolegle do siebie.
Narz˛edziami osiowosymetrycznymi, wykorzystywanymi w aplikacji wielorobotowej sa˛ flamastry zamontowane w szcz˛ekach chwytaka za pomoca˛ przygotowanych drewnianych kostek.
Kostki sa˛ niezb˛edne dla prawidłowego umocowania flamastrów w szcz˛ekach chwytaka, które
zostały zaprojektowane jako uchwyty dla kostki Rubika. Flamastry sa˛ umieszczone po środku
chwytaka (szcz˛ek) i skierowane pionowo w dół.
10.1
Kalibracja robotów
Oba roboty postument oraz on_track maja˛ taka˛ sama˛ konfiguracj˛e mechaniczna.˛ Wyst˛epuja˛
jednak różnice w odczytach z enkoderów w piatym
˛
stopniu swobody, czyli kiści. Z tego wzgl˛edu na poczatku
˛
badań należało przeprowadzić kalibracj˛e kinematyki robotów, tak by rzeczywisty układ współrz˛ednych odpowiadał układowi współrz˛ednych założonemu w kinematykach
(rysunek 8.2 strona 79 oraz rysunek 8.3 strona 81). Z tego wzgl˛edu zdefiniowano parametry
GEOM_SYNCH_POS_*MODEL_*, które modyfikuja˛ obliczanie położenia poszczególnych
stopni swobody przez zagadnienia kinematyki w taki sposób, by układy współrz˛edne rzeczywisty i "wewn˛etrzny" systemu MRROC++ były tożsame. Poniżej zamieszczono wartości parametrów jakie otrzymano w wyniku przeprowadzonych badań.
Parametry korygujace
˛ kinematyk˛e sześcioosiowa˛ robota on_track:
GEOM_SYNCH_POS_0_MODEL_0_OT
SYNCH_POS_0
-13.819
-5.012
-4.219
155.997
360.0
769.7
4830
˛ kinematyk˛e sześcioosiowa˛ robota postument:
GEOM_SYNCH_POS_0_MODEL_0_P
SYNCH_POS_0
-15.9
-5
107
10
-8.527
153.31
314.665
793.0
4830
˛ kinematyk˛e pi˛ecioosiowa˛ robota on_track:
SYNCH_POS_0
-15.9
-5
-8.529
153.37
435.2
793.0
4830
Powyższe ustawienia, wyrażone w inkrementach, pozwoliły na skalibrowanie kinematyki w
stopniu umożliwiajacym
˛
poprawne wykonania zadania rysowania (z dokładnościa˛ do około
1mm). Obecnie prowadzone sa˛ badania, b˛edace
˛ tematem oddzielnej pracy, które maja˛ znacznie
poprawić dokładność kinematyk dla poszczególnych robotów.
Przy badaniach kalibracji robotów, skorzystano z wbudowanego w system MRROC++ mechanizmu do zbierania danych pomiarówych tzw. readera. Umożliwia on zbieranie danych pomiarowych w dowolnym momencie w trakcie działania robotów. Na rysunkach 10.1, 10.2, 10.3
pokazano wyniki kalibracji dla kinematyki sześcioosiowej dla robota on_track, oraz pi˛ecio- i
sześcioosiowej dla robota postument.
Pomiary zostały opracowane w programie Matlab. W celu porównania otrzymanych pomiarów opracowano skrypt matlabowy, który przelicza macierze wyników w taki sposób, by
wybrany punkt wykresu dla poszczególnych kinematyk pokrył si˛e. W oparciu o wyliczone
przesuni˛ecia wgl˛edem wybranego punktu charakterystyki jednej z kinematyk, korygowane sa˛
punkty pozostałych kinematyk. W ten sposób uzyskiwane jest nałożenie si˛e trzech trajektorii
uzyskanych w pomiarach dla poszczególnych kinematyk. Dzi˛eki temu można zaobserwować
czy otrzymane wykresy pokrywaja˛ si˛e podczas całego ruchu robota oraz czy poszczególne odcinki ruchu maja˛ taka˛ sama˛ długość.
Przy prezentowaniu wykresów przyj˛eto nast˛epujace
˛ oznaczenia:
kin6 ot – oznacza wykres dla robota on_track z kinematyka sześcioosiowa,˛
kin6 p – oznacza wykres dla robota postument z kinematyka sześcioosiowa,˛
kin5 p – oznacza wykres dla robota postument z kinematyka pi˛ecioosiowa.˛
108
10
6kin p
5kin p
6kin ot
wspolrzedna z [m]
1.02
1
0.98
0.96
0.94
0.92
0.05
0
wspolrzedna y [m]
−0.05
0.72
0.74
0.76
0.78
0.8
0.82
wspolrzedna x [m]
Rysunek 10.1: Wykres ruchu w trzech kierunkach w wykorzystywanych kinematykach sześcio-
polozenie koncowki manipulatora dla kartezjanskiej wsp.y [m]
i pi˛ecioosiowej dla robotów postument i on_track.
0.06
0.04
6kin p
5kin p
6kin ot
0.02
0
−0.02
−0.04
−0.06
0.7
0.72
0.74
0.76
0.78
0.8
0.82
polozenie koncowki manipulatora dla kartezjanskiej wsp.x [m]
Rysunek 10.2: Wykres ruchu w osiach x oraz y dla wykorzystywanych kinematyk.
109
polozenie koncowki manipulatora dla kartezjanskiej wsp.z [m]
10
1.04
1.02
6kin p
5kin p
6kin ot
1
0.98
0.96
0.94
0.92
0.9
0.7
0.72
0.74
0.76
0.78
0.8
0.82
Rysunek 10.3: Wykres ruchu w osiach x oraz z dla wykorzystywanych kinematyk.
Przeprowadzone po kalibracji badania ruchu robotami w kinematyce sześcioosiowej dla robotów postument i on_track oraz pi˛ecioosiowej dla robota postument, zadawane poprzez zmian˛e
współrz˛ednych w notacji XYZ_EULER_ZYZ, potwierdziły poprawne skalibrowanie robotów.
Osie x,y,z sa˛ pod katem
˛
prostym wzgl˛edem siebie, linie zakreślone w trakcie ruchu w poszczególnych osiach nakładaja˛ si˛e, a bład
˛ nie jest wi˛ekszy niż 0.1 mm.
10.2
Badania poprawności działania kinematyk
Kolejne badania polegajace
˛ na odwzorowywaniu nauczonej trajektorii pozwoliły na sprawdzenie, 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 programie Matlab.
W kolejnych podrozdziałach zostana˛ przedstawione wyniki badań dla czterech nauczonych
trajektorii.
Przy prezentowaniu wykresów przyj˛eto nast˛epujace
˛ oznaczenia:
teach ot – oznacza wykres dla robota on_track w trybie uczenia,
draw ot – oznacza wykres dla robota on_track w trybie powielania,
draw p – oznacza wykres dla robota postument w trybie powielania.
110
10
10.2.1
Kwadrat
Na rysunkach 10.4, 10.5, 10.6 przedstawiono naniesione na siebie pomiary wykonane dla
kinematyki sześcioosiowej robota on_track w trybie uczenia oraz kinematyki sześcioosiowej
dla robota on_track i pi˛ecioosiowej dla robota postument w trybie powielania nauczonego rysunku kwadratu.
teach ot
draw ot
draw p
wspolrzedna z [m]
0.96
0.94
0.92
0.9
0.88
0.02
0
0.82
0.81
−0.02
wspolrzedna y [m]
0.8
0.79
−0.04
0.78
wspolrzedna x [m]
Rysunek 10.4: Trójwymiarowy wykres trajektorii realizowanych w trakcie uczenia i powielania
rysunku przez roboty postument i on_track.
111
0.005
0
teach ot
draw ot
draw p
−0.005
−0.01
−0.015
−0.02
−0.025
−0.03
−0.035
−0.04
0.77
0.78
0.79
0.8
0.81
0.82
0.83
Rysunek 10.5: Wykres ruchu w osiach x oraz y uczonej i powielanych trajektorii.
10
0.99
0.98
0.97
teach ot
draw ot
draw p
0.96
0.95
0.94
0.93
0.92
0.91
0.9
0.89
0.77
0.78
0.79
0.8
0.81
0.82
0.83
Rysunek 10.6: Wykres ruchu w osiach x oraz z uczonej i powielanych trajektorii.
112
10
10.2.2
Okr˛egi
Na rysunkach 10.7, 10.8, 10.9 przedstawiono naniesione na siebie pomiary wykonane dla
kinematyki sześcioosiowej robota on_track w trybie uczenia oraz kinematyki sześcioosiowej
dla robota on_track i pi˛ecioosiowej dla robota postument w trybie powielania nauczonego rysunku okr˛egów.
teach ot
draw ot
draw p
0.92
wspolrzedna z [m]
0.915
0.91
0.905
0.9
0.895
0.89
0
0.86
0.84
−0.02
0.82
wspolrzedna y [m]
−0.04 0.8
wspolrzedna x [m]
Rysunek 10.7: Trójwymiarowy wykres trajektorii realizowanych w trakcie uczenia i powielania
rysunku przez roboty postument i on_track.
113
0.02
0.01
teach ot
draw ot
draw p
0
−0.01
−0.02
−0.03
−0.04
0.8
0.81
0.82
0.83
0.84
0.85
0.86
0.87
10
0.935
0.93
0.925
teach ot
draw ot
draw p
0.92
0.915
0.91
0.905
0.9
0.895
0.89
0.885
0.8
0.81
0.82
0.83
0.84
0.85
0.86
0.87
114
10
10.2.3
Gwiazda
Na rysunkach 10.10, 10.11, 10.12 przedstawiono naniesione na siebie pomiary wykonane
dla kinematyki sześcioosiowej robota on_track w trybie uczenia oraz kinematyki sześcioosiowej dla robota on_track i pi˛ecioosiowej dla robota postument w trybie powielania nauczonego
wspolrzedna z [m]
rysunku gwiazdy.
teach ot
draw ot
draw p
0.94
0.93
0.92
0.91
0.9
0.89
−0.02
−0.04
−0.06
−0.08
wspolrzedna y [m]
−0.1
0.82
0.84
0.86
0.88
0.9
wspolrzedna x [m]
Rysunek 10.10: Trójwymiarowy wykres trajektorii realizowanych w trakcie uczenia i powielania rysunku przez roboty postument i on_track.
115
−0.02
−0.03
teach ot
draw ot
draw p
−0.04
−0.05
−0.06
−0.07
−0.08
−0.09
−0.1
0.81 0.82 0.83 0.84 0.85 0.86 0.87 0.88 0.89
10
0.98
0.97
0.96
teach ot
draw ot
draw p
0.95
0.94
0.93
0.92
0.91
0.9
0.89
0.88
0.81 0.82 0.83 0.84 0.85 0.86 0.87 0.88 0.89
116
10
10.2.4
Dom
Na rysunkach 10.13, 10.14, 10.15 przedstawiono naniesione na siebie pomiary wykonane
dla kinematyki sześcioosiowej robota on_track w trybie uczenia oraz kinematyki sześcioosiowej dla robota on_track i pi˛ecioosiowej dla robota postument w trybie powielania nauczonego
rysunku domu.
teach ot
draw ot
draw p
wspolrzedna z [m]
0.96
0.94
0.92
0.9
0.88
0.05
0.95
0
0.9
wspolrzedna y [m]
−0.05
0.85
wspolrzedna x [m]
Rysunek 10.13: Trójwymiarowy wykres trajektorii realizowanych w trakcie uczenia i powielania rysunku przez roboty postument i on_track.
117
10
0.04
0.03
0.02
teach ot
draw ot
draw p
0.01
0
−0.01
−0.02
−0.03
−0.04
−0.05
−0.06
0.84
0.86
0.88
0.9
0.92
0.94
0.99
0.98
0.97
teach ot
draw ot
draw p
0.96
0.95
0.94
0.93
0.92
0.91
0.9
0.89
0.84
0.86
0.88
0.9
0.92
0.94
10.2.5
Wnioski
Przeprowadzone badania potwierdziły prawidłowe działanie kinematyk. Rysunki przedstawione na wykresach zostały przekształcone w sposób umożliwiajacy
˛ porównanie kreślonych
trajektorii. Można na nich zaobserwować niewielkie odchylenia, około 0.1 mm, pomi˛edzy na118
10
uczona,˛ a odtwarzanymi trajektoriami. Niedokładności te wynikaja˛ z obliczeń wykonywanych
przez wykorzystywane kinematyki. Wpływ na dokładność rysowania ma również przyj˛ety sposób kalibracji.
Charakterystyczne różnice pomi˛edzy rysunkami odpowiadajacymi
˛
fazie uczenia i powielania sa˛ widoczne w torze unoszenia i opuszczania pisaka. Operator wykonuje "płynne" ruchy w
gór˛e i dół, a odcinek pokonywany ponad kartka˛ jest pofalowany i nieregularny w płaszczyźnie
pionowej. Trajektorie zakreślone przez robota, w trybie powielania rysunku, w trakcie ruchu w
gór˛e i dół sa˛ pionowe, o takiej samej wysokości, a odcinek pokonywany ponad kartka˛ leży w
płaszczyźnie poziomej.
10.3
Wykresy siły dla trajektorii nauczonej przez operatora
Na poniższych rysunkach pokazano wyniki pomiaru sił odczytywanych przez czujnik siły
FT3084 zamontowany na robocie postument oraz 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 nast˛epujace
˛ oznaczenia:
* – zetkni˛ecie pisaka z podłożem,
a – ruch po powierzchni,
b – podnoszenie pisaka,
c – ruch ponad powierzchna˛ kartki,
d – opuszczanie pisaka.
119
10
Linia przerywana w osi X,Y, sila w osi Z dla robota on_track,
kinematyka 6 osiowa, uczenie
15
10
sila [N]
5
0
−5
−10
−15
0.06
0.9
0.04
0.02
wspolrzedna Y [m]
0.89
0
−0.02
−0.04 0.87
0.88
wpsolrzedna X [m]
Rysunek 10.16: Rysowanie linii przerywanej w przestrzeni dwuwymiarowej x,y. W osi z zaznaczono odczytywana˛ sił˛e.
Linia przerywana narysowana w osi Y,sila w osi Z dla robota on_track,
kinematyka 6 osiowa, uczenie
15
10
d
sila [N]
5
0
−5
a
−10
−15
−0.04
c
b
−0.03
−0.02
−0.01
0
0.01
0.02
wspolrzedna Y [m]
0.03
0.04
0.05
0.06
Rysunek 10.17: Odczyt siły w trakcie uczenia rysunku (robot on_track).
120
kinematyka 6 osiowa, pisanie
4
3.5
3
*
2.5
sila [N]
2
1.5
1
0.5
0
−0.5
a
c
d
−1
b
−1.5
−0.05 −0.04 −0.03 −0.02 −0.01
0
0.01
0.02
wspolrzedna Y [m]
0.03
0.04
0.05
Rysunek 10.18: Odczyt siły w trakcie powielania rysunku (robot on_track).
Linia przerywana narysowana w osi Y,sila w osi Z dla robota postument,
3
*
2
1
sila [N]
10
0
−1
a
−2
−0.05
c
b
−0.04
−0.03
−0.02
−0.01
0
0.01
wspolrzedna Y [m]
d
0.02
0.03
0.04
0.05
Rysunek 10.19: Odczyt siły w trakcie powielania rysunku (robot postument).
121
10
10.3.1
Wnioski
W trybie powielania nauczonego rysunku, podczas ruchu po powierzchni kartki, regulator stara si˛e 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 jako d, realizowany jest jedynie ruch pozycyjny.
W trakcie badań zauważono silne oscylacje wyst˛epujace
˛ podczas rysowania po powierzchni. Jest to prawdopodobnie spowodowane nieoptymalnym doborem zastosowanego obecnie w
systemie MRROC++ 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 programie Matlab
Badania sił przeprowadzone w oparciu o trajektorie nauczone przez operatora sa˛ zakłócone
poprzez odchylenia trajektorii, jakie pojawiały si˛e w trakcie uczenia. W celu wyeliminowania
nieliniowości w uczonej trajektorii, do celów badania sił, zdecydowano si˛e na wygenerowanie
idealnych trajektorii przy użyciu programu Matlab. Trajektorie te musza˛ mieć określona˛ postać,
tak by zostały poprawnie odczytane w systemie MRROC++.
Poniżej pokazano pierwsze trzy linie pliku idealnalinia.trj przechowujacego
˛
informacje opisujace
˛ zapami˛etana˛ trajektori˛e 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 zapami˛etana została trajektoria. W drugiej
linii znajduje si˛e liczba zapisanych danych o położeniu. Kolejne linie zawieraja˛ koordynaty wyrażone we współrzednych kartezjańskich lub katach
˛
wewn˛etrznych manipulatora. Każda z linii
zawiera 10 parametrów. Pierwszy parametr nie jest wykorzystywany w generatorach siłowych.
W generatorach pozycyjnych określa on czas osiagni˛
˛ ecia pozycji opisanej w kolejnych ośmiu
parametrach wiersza. Liczba ośmiu parametrów jest potrzebna w przypadku wykorzystywania
trajektorii podanej w katach
˛
wewn˛etrznych robota. W napisanej aplikacji wielorobotowej wykorzystywane sa˛ druga, trzecia, czwarta oraz ostatnia pozycja z linii. Pierwsze trzy parametry
odpowiadaja˛ współrz˛ednym kartezjańskim zapisanym w trakcie nauki trajektorii. Ostatni parametr określa stan, w jakim znajduje si˛e aktualnie robot ( 2 – powierzchnia, 3 – unoszenie, 4 –
uniesienie, 5 – opuszczanie).
Na rysunkach zamieszczonych poniżej pokazano wyniki pomiaru sił odczytywanych przez
czujnik siły FT3084 zamontowany na robocie postument oraz czujnik siły FT6284 zamontowany na robocie on_track, w trakcie powielania trajektorii "idealnej" linii przerywanej oraz
122
10
"idealnego" koła, wygenerowanych w programie Matlab.
W opisie wykresów zastosowano nast˛epujace
˛ oznaczenia:
* – zetkni˛ecie pisaka z podłożem,
a – ruch po powierzchni,
b – podnoszenie pisaka,
c – ruch ponad powierzchna˛ kartki,
d – opuszczanie pisaka.
10.4.1
Idealna przerywana linia prosta
Zadana trajektoria idealnej linii przerywanej, rysowanej w plaszczyznie XY.
−3
x 10
5
wspolrzedna z [m]
4
3
2
1
0
−1
0.08
0.06
1
0.5
0.04
0
0.02
wspolrzedna y [m]
−0.5
0
−1
wspolrzedna x [m]
Rysunek 10.20: Wygenerowana w programie Matlab trajektoria o postaci linii przerywanej.
123
10
Wykres trójwymiarowy linii X,Y,Z dla robota on_track, kinematyka 6 osiowa, pisanie
wspolrzedna Z [m]
−0.05
−0.055
−0.06
−0.065
−0.07
−0.075
0.02
0
0.9885
−0.02
0.988
−0.04
0.9875
−0.06
wspolrzedna Y [m]
0.987
wspolrzedna X [m]
Rysunek 10.21: Trajektoria zrealizowana przez robota on_track w trakcie powielania rysunku.
Wykres trójwymiarowy linii X,Y,Z dla robota postument, kinematyka 5 osiowa, pisanie
wspolrzedna Z [m]
0.645
0.64
0.635
0.63
0.625
0.62
−0.02
−0.04
0.899
0.8985
−0.06
0.898
0.8975
−0.08
wspolrzzedna Y [m]
−0.1
0.897
0.8965
wspolrzedna X [m]
Rysunek 10.22: Trajektoria zrealizowana przez robota postument w trakcie powielania rysunku.
124
10
4.5
4
3.5
*
3
sila [N]
2.5
2
1.5
1
0.5
0
a
b
−0.5
−0.06
−0.05
−0.04
c
d
−0.03
−0.02
−0.01
wspolrzedna Y [m]
0
0.01
0.02
Rysunek 10.23: Odczyt siły w trakcie powielania idealnej linii przerywanej przez robota
on_track.
Linia przerywana narysowana w osi Y,sila w osi Z dla robota postument,
5
*
sila [N]
2.5
0
a
−2.5
−0.1
c
b
−0.09
−0.08
d
−0.07
−0.06
−0.05
wspolrzedna Y [m]
−0.04
−0.03
−0.02
Rysunek 10.24: Odczyt siły w trakcie powielania idealnej linii przerywanej przez robota postument.
125
10.4.2
Idealne koło
Wykres trójwymiarowy kola
−3
x 10
5
4
wspolrzedna z [m]
10
3
2
1
0
0.04
0.02
0.04
0.02
0
−0.02
wspolrzedna y [m]
−0.04 −0.04
0
−0.02
wspolrzedna x [m]
Rysunek 10.25: Wygenerowana w programie Matlab trajektoria o postaci koła.
126
10
Wykres trójwymiarowy kola X,Y,Z dla robota on_track, kinematyka 6 osiowa, pisanie
wspolrzedna Z [m]
−0.04
−0.05
−0.06
−0.07
−0.08
0
−0.02
0.92
−0.04
0.9
−0.06
0.88
−0.08
wspolrzedna Y [m]
0.86
wspolrzedna X [m]
Rysunek 10.26: Trajektoria zrealizowana przez robota on_track w trakcie powielania rysunku
koła.
Wykres trójwymiarowy kola X,Y,Z dla robota postument, kinematyka 5 osiowa, pisanie
wspolrzedna Z [m]
0.645
0.64
0.635
0.63
0.625
0.62
0.04
0.88
0.02
0.86
0.84
0
0.82
wspolrzzedna Y [m]
−0.02
0.8
wspolrzedna X [m]
Rysunek 10.27: Trajektoria zrealizowana przez robota postument w trakcie powielania rysunku
koła.
127
10
Kolo narysowane w osi Y,sila w osi Z dla robota on_track, kinematyka 6 osiowa, pisanie
3.5
b
3
2.5
sila [N]
2
1.5
1
0.5
0
a
a
−0.5
−0.065 −0.06 −0.055 −0.05 −0.045 −0.04 −0.035 −0.03 −0.025 −0.02 −0.015
wspolrzedna Y [m]
Rysunek 10.28: Odczyt siły w trakcie powielania idealnego koła przez robota on_track.
Kolo narysowane w osi Y,sila w osi Z dla robota postument, kinematyka 5 osiowa, pisanie
3.5
b
3
2.5
2
sila [N]
1.5
1
0.5
0
−0.5
a
a
−1
−1.5
−0.01 −0.005
0
0.005 0.01 0.015 0.02 0.025 0.03 0.035 0.04
wspolrzedna Y [m]
Rysunek 10.29: Odczyt siły w trakcie powielania idealnego koła przez robota postument.
10.4.3
Wnioski
Otrzymane trajektorie sa˛ dokładniejsze, bez odchyleń na odcinkach w osi Z. Tego typu trajektorie jest bardzo trudno otrzymać poprzez uczenie r˛eczne.
Na otrzymanych wykresach znacznie łatwiej jest zaobserwować kolejne fazy ruchu robotów i odpowiadajacych
˛
im sił, niż w przypadku wykresów powielanych na podstawie trajektorii
128
10
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 Niutona. W trakcie pisania wyst˛epuja˛ drgania zwiazane
˛
z tarciem statycznym i dynamicznym na
styku pisaka z kartka.˛
W trakcie badań zauważono zależność amplitudy drgań odczytów siły od wykorzystywanego pisaka. Zmiany były widoczne przy zamianie zużytego pisaka na nowy. Mało używany pisak
jest bardziej sztywny i z tego powodu drgania w trakcie pisania sa˛ znaczaco
˛ wi˛eksze. Można
to zaobserowować na rysunkach 10.28 oraz 10.29 (strona 128). W trakcie powielania idealnego koła robot on_track wykorzystywał nowy flamaster, natomiast flamaster zamontowany
w uchwycie robota postument był już wcześniej kilkukrotnie wykorzystywany. Na wykresach
10.21 oraz 10.22 (strona 124), przedstawiajacych
˛
trajektorie w przestrzeni trójwymiarowej dla
robotów on_track i postument, można zaobserwować odchylenia w osi X spowodowane niedokładnościami obliczeń poszczególnych kinematyk. Niedokładności te sa˛ rz˛edu około 0.0005 m,
czyli 0.5 mm. Prawdopodobnie wynikaja˛ one z błedów numerycznych oraz zaokragleń
˛
stosowanych w obliczeniach kinematycznych. Niedokładności te moga˛ być również spowodowane
luzami wyst˛epujacymi
˛
w mechanice wykorzystywanych robotów.
Badania przeprowadzone przy użyciu trajektorii wygenerowanych poprzez skrypt uruchomiony w programie Matlab, wskazuja˛ na duża˛ zależność wyników od tarcia pomi˛edzy pisakiem, a powierzchnia.˛ Zagadnienie tarcia oraz jego wpływ na odczyty czujnika siły jest trudne
do rozwiazania
˛
ze wzgl˛edu na jego silna˛ nieliniowość.
10.5
Podsumownie wyników badań
Przeprowadzone badania potwierdziły sprawne działanie nowego sterownika dołaczonego
˛
do systemu MRROC++ czujnika siły Gamma FT6284 oraz poprawne skalibrowanie kinematyk.
Zadania uczenia i powielania rysunku sa˛ realizowane z wystarczajac
˛ a˛ dokładnościa.˛ Obecnie
prowadzone badania nad kalibracja˛ robotów, b˛edace
˛ tematem oddzielnej pracy, moga˛ polepszyć dokładność obliczeń kinematycznych i wpłynać
˛ na dokładność rysowania.
Modyfikacje wprowadzone po modernizacji robotów umożliwiły sprawdzenie i porównanie
dwóch kinematyk sześcio- i pi˛ecioosiowej. W zaimplementowanej aplikacji wielorobotowej,
wykorzystujacej
˛ narz˛edzia osiowosymetryczne, obie kinematyki działaja˛ prawidłowo i wystarczajaco
˛ dokładnie (dokładność rz˛edu 1 mm). Uruchomiony mechanizm przełaczania
˛
kinematyk
działa poprawnie.
129
11
11
PODSUMOWANIE
Podsumowanie
Efektem
niniejszej
pracy
jest
aplikacja
wielorobotowa
realizujaca
˛
sterowanie
pozycyjno–siłowe. Aplikacja ta wykorzystuje dwa czujniki siły zamontowane na dwóch manipulatorach. W trakcie pracy zaprojektowano i wykonano płytki drukowane, niezb˛edne do
usprawnienia komunikacji z zastanym czujnikiem siły FT3084 oraz komunikacji z nowym czujnikiem siły FT6284 firmy ATI. Zaimplementowano nowy sterownik czujnika siły FT6284 firmy ATI, realizujacy
˛ odczytywanie danych pomiarowych poprzez kart˛e akwizycji danych PCI6034E firmy National Instruments i przetwarzajacy
˛ otrzymane dane do postaci wykorzystywanej przez system MRROC++.
Przy realizacji aplikacji wielorobotowej wprowadzono wiele istotnych zmian i poprawek,
które zwiazane
˛
były ze zmiana˛ konfiguracji robotów, wpływajacych
˛
na struktur˛e systemu
MRROC++, na przykład realizacj˛e buforów komunikacyjnych pomi˛edzy poszczególnymi procesami. Zmiany w konfiguracji robotów wymagały przebadania i wprowadzenia do systemu
MRROC++ nowych kinematyk robotów, obsłużenia 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 sześcioosiowej, natomiast drugi z piecioosiowej. Wykorzystanie kinematyki pi˛ecioosiowej wymagało uruchomienia możliwości zadawania narz˛edzia osiowosymetrycznego w systemie MRROC++. W trakcie realizacji zmian wprowadzono
możliwość zadawania parametrów narz˛edzia wykorzystywanego przez manipulatory poprzez
interfejs użytkownika UI. Ze wzgl˛edu na zmiany w konstrukcji robotów, niezb˛edne stało si˛e
przeprowadzenie kalibracji nowych kinematyk w taki sposób, by przyj˛ety wewnatrz
˛ systemu
MRROC++ układ końcówki narz˛edzia był tożsamy z układem końcówki w rzeczywistości. W
przeciwnym razie, z powodu rozbieżności robot działałby nieprawidłowo.
Zaimplementowana aplikacja wielorobotowa wykorzystuje szereg generatorów, umożliwiajacych
˛
ruch robotów, wybór odpowiedniej kinematyki, ustawienie parametrów narz˛edzi oraz
pozwalajacych
˛
na realizacj˛e zadania uczenia i powielania nauczonego rysunku.
Po uruchomieniu aplikacji wielorobotowej przeprowadzono wiele badań, w trakcie których
sprawdzono poprawność działania zaimplementowanego sterownika czujnika siły FT6284 oraz
pozostałych wprowadzonych zmian i poprawek. Wyniki badań wskazuja˛ na poprawne działanie
systemu MRROC++ rozbudowanego o dodane funkcjonalności.
Niniejsza praca została zakończona pomyślnie, 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 zadania układania kostki Rubika przez dwa manipulatory. Zapewnienie dodatkowego czujnika
siły oraz zmiany wprowadzone do systemu MRROC++ znacznie rozszerzyły możliwości wy130
11
PODSUMOWANIE
korzystania znajdujacych
˛
si˛e w laboratorium robotów.
131
LITERATURA
Literatura
[1] J.J. Craig, Introduction to Robotics, Mechanics and Control, Addison-Wesley Publishing
Company, 1986r.,
[2] C.Zieliński, W. Szynkiewicz, Podr˛ecznik programowania systemu MRROC++ dla robota
IRp-6 na torze jezdnym, Raport IAiIS nr 99-21, Warszawa, kwiecień 1999r.,
[3] C. Zieliński, W. Szynkiewicz, System MRROC++ dla robota IRp-6, Raport IAiIS nr 9920, Warszawa, maj 1999r.,
[4] C.Zieliński, W. Szynkiewicz, T. Zielińska, System MRROC++ dla robota RNT, raport
IAiIS nr 99-35, Warszawa, maj 1999r.,
[5] C. Zieliński, A Quasi-Gormal Approach to Structuring Multi-Robot System Controllers,
IAiIS, RoMoCo’01, 18-20 październik 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, Wst˛epna implementacja sterowania pozycyjno-siłowego w systemie
MRROC++, Raport IAiIS nr 04-14, Warszawa, grudzień 2004r.,
[9] A. Śluzek, C. Zieliński, W. Szynkiewicz, Model kinematyczny i generator trajektorii liniowej 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. Dul˛eba, 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, Wrocław 1992r.,
[13] Multi-Axis Force/Torque Sensor System, Installation and Operation Manual, Document # : 9610-05-1017-06, październik 2003r.,
[14] M.W.Spong, M. Vidyasagar, Dynamika i Sterowanie Robotów, WNT, Warszawa, 1997r.
[15] G. Zeng, A. Hemami Overview of Robot Force Control, Robotica, 1997, 15(5), str.473482,
132
LITERATURA
[16] Herman Bruyninckx, Specification of Force-Controlled Actions in the "Task Frame Formalism" - 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 Corporation, (http://www.ni.com), listopad 1998r.
[22] E/S/M Series Calibration Procedure for NI-DAQ mx, National Instruments Corporation,(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-101708,(http://www.ati-ia.com/), lipiec 2004r.
[26] Dokumentacja techniczno–ruchowa robotów IRb–6 i IRb–60,PIAP, Warszawa 1983.
133
SPIS RYSUNKÓW
Spis rysunków
2.1
Wektory sił i momentów przyłożonych 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 uwzgl˛ednieniem sposobu komunikacji miedzywatkowej
˛
([7]). . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
24
4.2
Struktura ogólna procesów MP, ECP, EDP oraz VSP ([2]). . . . . . . . . . . .
26
4.3
Wewn˛etrzna struktura jadra
˛ procesu MP ([2]). . . . . . . . . . . . . . . . . . .
28
4.4
Wewn˛etrzna struktura jadra
˛ procesu ECP ([2]). . . . . . . . . . . . . . . . . .
29
5.1
Schemat zaprojektowanej płytki dwustronnej, łacz
˛ acej
˛ kart˛e Advantech PCI1751 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, umożliwiajacej
˛ 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́ metody edp_ATI6284_force_sensor::edp_ATI6284_force_sensor(void). 69
7.2
Sieć działań metody void edp_ATI6284_force_sensor::configure_sensor (void).
7.3
Sieć działań metody void edp_ATI6284_force_sensor::initiate_reading (void),
cz˛eść I. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.4
70
71
Sieć działań metody void edp_ATI6284_force_sensor::initiate_reading (void),
cz˛eść II. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
72
7.5
Sieć działań metody void edp_ATI6284_force_sensor::wait_for_event(). . . . .
73
8.1
Stanowisko do badania aplikacji wielorobotowej z dwoma robotami IRp-6. Kolorem zaznaczono nowe rozwiazanie
˛
. . . . . . . . . . . . . . . . . . . . . . .
76
8.2
Robot IRp-6 postument w uj˛eciu kinematyki pi˛ecioosiowej. . . . . . . . . . . .
79
8.3
Robot IRp-6 on_track w uj˛eciu kinematyki sześcioosiowej z dołaczonym
˛
nowym stopniem swobody i chwytakiem. . . . . . . . . . . . . . . . . . . . . . .
81
8.4
Robot IRp-6 on_track w uj˛eciu kinematyki sześcioosiowej – końcówka robota.
82
8.5
Narz˛edzie zdefiniowane w notacji TOOL_XYZ_ANGLE_AXIS. . . . . . . . . .
83
8.6
Narz˛edzie zdefiniowane w notacji TOOL_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++. . . . . . . . . . .
9.4
Sieć działań programu realizujacego
˛
zadanie rysowania, umieszczonego w pliku mp_m_kd.cc, cz˛eść 1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9.5
94
99
˛
zadanie rysowania, umieszczonego w pliku mp_m_kd.cc, cz˛eść 2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
9.6
˛
zadanie rysowania, umieszczonego w pliku mp_m_kd.cc, cz˛eść 3. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
10.1 Wykres ruchu w trzech kierunkach w wykorzystywanych kinematykach sześcioi pi˛ecioosiowej dla robotów postument i 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 powielania rysunku przez roboty postument i 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 powielania rysunku przez roboty postument i on_track. . . . . . . . . . . . . . . . . . 113
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 powielania rysunku przez roboty postument i on_track. . . . . . . . . . . . . . . . . . 115
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 powielania rysunku przez roboty postument i on_track. . . . . . . . . . . . . . . . . . 117
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 zaznaczono odczytywana˛ sił˛e. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
10.17Odczyt siły w trakcie uczenia rysunku (robot on_track). . . . . . . . . . . . . . 120
10.18Odczyt siły w trakcie powielania rysunku (robot on_track). . . . . . . . . . . . 121
10.19Odczyt siły w trakcie powielania rysunku (robot postument). . . . . . . . . . . 121
10.20Wygenerowana w programie Matlab trajektoria o postaci linii przerywanej. . . 123
10.21Trajektoria zrealizowana przez robota on_track w trakcie powielania rysunku. . 124
10.22Trajektoria zrealizowana przez robota postument w trakcie powielania rysunku. 124
10.23Odczyt siły w trakcie powielania idealnej linii przerywanej przez robota on_track.125
135
SPIS RYSUNKÓW
10.24Odczyt siły w trakcie powielania idealnej linii przerywanej przez robota postument. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125
10.25Wygenerowana w programie Matlab trajektoria o postaci koła. . . . . . . . . . 126
10.26Trajektoria zrealizowana przez robota on_track w trakcie powielania rysunku
koła. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127
10.27Trajektoria zrealizowana przez robota postument w trakcie powielania rysunku
koła. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127
10.28Odczyt siły w trakcie powielania idealnego koła przez robota on_track. . . . . 128
10.29Odczyt siły w trakcie powielania idealnego koła przez robota postument. . . . . 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. . . . . . . . .
5.2
32
Sposób połaczenia
˛
oraz funkcje linii sygnałowych łacz
˛ acych
˛
kart˛e PCI NI6034E z nowym czujnikiem siły Gamma F/T firmy ATI. . . . . . . . . . . . .
34
6.1
Parametry czujnika siły Gamma. . . . . . . . . . . . . . . . . . . . . . . . . .
38
6.2
Parametry kalibracji i rozdzielczość czujnika siły Gamma. CON = Controller
6.3
F/T system, DAQ = 16 bit F/T System . . . . . . . . . . . . . . . . . . . . . .
39
Rozdzielczość pomiaru karty PCI-6034E. . . . . . . . . . . . . . . . . . . . .
41
137
SPIS ZAWARTEGO KODU
Spis zawartego kodu
1
Watek
˛ force_thread komunikujacy
˛ si˛e z czujnikiem siły . . . . . . . . . . . . .
53
2
Klasa edp_ATI6284_force_sensor realizujaca
˛ sterownik nowego czujnika siły. .
54
3
Funkcja iBus* acquireBoard(const u32 devicePCI_ID) uruchamiajaca
˛ kart˛e akwi58
zycji danych NI-6034E w systemie QNX. . . . . . . . . . . . . . . . . . . . .
4
Funkcja 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) wykonujaca
˛ przeliczenie wejściowych pomiarów w woltach na jednostki sił i momentów sił. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5
66
Funkcja void releaseBoard(iBus *&bus) odłaczajaca
˛
kart˛e akwizycji danych
NI-6034E w systemie QNX. . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
67
Funkcja void RTConvertToFT(RTCoefs *coefs, float voltages[],float result[],BOOL
tempcomp) przeliczajaca
˛ podany sześcioelementowy wektor napi˛eć na odpowiadajace
˛ im siły i momenty sił. . . . . . . . . . . . . . . . . . . . . . . . . .
68
7
Plik display.h. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
74
8
Funkcja tool_xyz_aa_2_frame realizujaca
˛ przeliczenie współrz˛ednych narz˛edzia z postaci TOOL_XYZ_ANGLE_AXIS do trójścianu. . . . . . . . . . . . . .
9
Funkcja tool_frame_2_xyz_aa realizujaca
˛ przeliczenie współrz˛ednych narz˛edzia z postaci trójścianu do TOOL_XYZ_ANGLE_AXIS. . . . . . . . . . . . . .
10
87
88
Funkcja tool_axially_symmetrical_xyz_eul_zy_2_frame realizujaca
˛ przeliczenie współrz˛ednych narz˛edzia z postaci TOOL_AS_XYZ_EULER_ZY do ramki
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
11
89
Funkcja tool_axially_symmetrical_xyz_eul_zy_2_frame realizujaca
˛ przeliczenie współrz˛ednych narz˛edzia z postaci trójścianu do TOOL_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˛ prac˛e w wersji elektronicznej,
• kod sterownika MRROC++ wraz z wprowadzonymi zmianami,
• kod sterownika czujnika siły poza systemem MRROC++.
139

X - Politechnika Warszawska

Transkrypt

Podobne dokumenty