INSTYTUT AUTOMATYKI I INFORMATYKI STOSOWANEJ

na kierunku Automatyka i Robotykaw specjalności Automatyka i Robotyka

Układ elektroniczny do akwizycji danych z jednostkiinercyjnej za pośrednictwem EtherCAT

Bartosz Joacutezef KaczorNumer albumu 241797

PromotorDr inż Tomasz Winiarski

Warszawa 2017

Streszczenie

Układ elektroniczny do akwizycji danych z jednostkiinercyjnej za pośrednictwem EtherCAT

Przedmiotem niniejszej pracy jest stworzenie oraz oprogramowanie układu elek-tronicznego umożliwiającego podłączenie jednostki inercyjnej ADIS16362 do magi-strali EtherCAT W pracy tej opisana jest struktura zaprojektowanego układu odmodułu głoacutewnego poprzez urządzenie komunikujące się z istniejącą infrastrukturąkomunikacyjną EtherCAT po podłączenie jednostki inercyjnej Znajdziemy roacutew-nież opis wytworzonego oprogramowania znajdującego się zaroacutewno na wytworzonymurządzeniu jak i urządzeniu nadrzędnym w komunikacji (komputerze PC) W ramachpracy został roacutewnież przeprowadzony przegląd dostępnych na rynku rozwiązań tegotypu oraz algorytmoacutew służących do korekcji odczytu siły Zostały roacutewnież zaimple-mentowane programy testowe i przeprowadzone na ich podstawie badania

Podjęcie tego tematu było motywowane wynikami eksperymentoacutew z jednostkąinercyjną komunikującą się za pomocą interfejsu USB Ujawniły one zmienne poacuteź-nienie w komunikacji Opoacuteźnienie to wahało się nawet do kilkudziesięciu milisekundDodatkowym powodem wyboru sposobu komunikacji była chęć ujednolicenia sposo-boacutew połączenia urządzeń w ramach całego robota

Słowa kluczowe EtherCAT IMU Anybus B40 IRp-6

2

Abstract

The electronic system for data acquisition from inertial unitvia EtherCAT

The subject of this thesis is to create software and electronic system that allowsconnecting the ADIS16362 inertial unit to the EtherCAT bus In this work thestructure of the designed system is described from the main hardware unit by thedevice communicating with existing EtherCAT infrastructure to hardware interfaceconnecting to IMU In this paper we also find a description of the produced softwarelocated on both the manufactured device and the master communication device(PC) As a part of the work was also carried out a review of commercially availablesolutions of this type and correction algorithms for force measurement There havealso been implemented test programs and executed experiments with its use

The choice of this topic was motivated by the result of experiments with iner-tial unit communicating via the USB interface They revealed a variable delay incommunication This delay ranged up to tens of milliseconds Another reason forchoosing this way of communication was to standardize the ways to link deviceswithin the entire robot

Keywords EtherCAT IMU Anybus B40 IRp-6

3

4

Spis treści

1 Wstęp 711 Założenia i problematyka pracy 712 Układ Pracy 7

2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwezastosowania układu 921 Definicje 922 Działanie jednostki inercyjnej 10

221 Akcelerometry MEMS 10222 Żyroskopy MEMS 11223 Żyroskopy światłowodowe 11

23 Działanie magistrali Ethercat 11231 COE 12232 Synchronizacja zegara 12

24 Jednostki inercyjne ze złączami I2C oraz SPI 1225 Jednostki inercyjne ze złączami przemysłowymi 1326 Możliwe zastosowania proponowanego układu 14

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym 14262 Korekcja odczytu siły z czujnika nadgarstkowego 14

3 Opis budowanego układu 1531 Opis Układu 15

311 Moduł głoacutewny 16312 Moduł jednostki inercyjnej (IMU) 16313 Anybus M40 EtherCAT 17

32 Opis złącz 17321 Złącze międzypłytkowe 17322 Złącze zasilania 18323 Złącze interfejsu EtherCAT 19324 Złącze Jtag 20325 Złącze Anybus B40-CPU 20

4 Opis oprogramowania 2341 Protokoły komunikacyjne 23

411 Standard EtherCAT 23412 Can over EtherCAT 23

42 Oprogramowanie MCU 24421 Konfiguracja peryferioacutew 24422 Sterownik Anybus 25423 Przerwania 30

43 Sterownik w strukturze OROCOS 31

Spis treści 5

5 Badania weryfikacyjne 3551 Opis stanowiska eksperymentalnego 3552 Projekt eksperymentoacutew 3553 Wyniki eksperymentoacutew 3754 Podsumowanie 42

A Schematy modułoacutew urządzenia 51

B Wykresy pobranych trajektorii 57

6 Spis treści

Rozdział 1

Wstęp

11 Założenia i problematyka pracy

Robotyka jest bardzo szeroką i ciekawą dziedziną Z czasem systemy robotyczne wy-posażone są w coraz większą ilość czujnikoacutew ktoacutere mierzą najroacuteżniejsze parametrypracy robota jak i roacutewnież środowiska w ktoacuterym pracuje

Założeniem pracy było stworzenie układu interfejsującego jednostkę inercyjnąADIS16362 Rozwiązanie musiało zapewnić niezawodną komunikacje z kompute-rem sterującym robotem Ponieważ systemy robotyczne muszą działać w reżimieczasu rzeczywistego roacutewnież komunikacja z projektowanym układem musiała dzia-łać w ścisłym reżimie czasowym Dodatkowo ważnym czynnikiem decydującym o uży-teczności w niektoacuterych systemach robotycznych jest nie tylko okres pomiaru aleroacutewnież ustalenie momentu pomiaru z jak największą dokładnością Dlatego wymo-giem okazał się pomiar synchronizowany do czego potrzebny jest interfejs umożli-wiający synchronizację zegara we wszystkich urządzeniach magistrali Dodatkowymograniczeniem był rozmiar ponieważ układ ma nadać się do montażu w ograni-czonej przestrzeni nadgarstka robota IRP6 oraz w potencjalnych innych robotachgdzie występują ograniczenia przestrzenne Układ został więc zbudowany z mięk-kim (z możliwością niewielkiego przekroczenia) ograniczeniem rozmiaru ustalonymna 50x50x50mm

12 Układ Pracy

Niniejsza praca składa się z pięciu rozdziałoacutew Rozdział 2 zawiera podstawy teore-tyczne działania jednostki inercyjnej oraz jej elementoacutew magistrali EtherCAT orazprzegląd dostępnych na rynku jednostek inercyjnych i przykłady zastosowania jed-nostek inercyjnych w robotyce Poroacutewnywane są w nim ich parametry oraz sposobykomunikacji ich wady i zalety Opisane są tam roacutewnież przykładowe zastosowaniajednostki inercyjnej w robotyce usługowej Rozdział 3 zawiera opis mechaniczny orazelektroniczny budowanego układu opis złącz oraz wyprowadzonych na ich styki sy-gnałoacutew W rozdziale 4 znajduje się opis oprogramowania wytworzonego w ramachpracy znajdującego się zaroacutewno w wytworzonym urządzeniu jak i tego pracującegona komputerze sterującym robota Wreszcie rozdział 5 w ktoacuterym znajdziemy pro-jekt oraz wyniki eksperymentoacutew przeprowadzonych z wykorzystaniem manipulatoraIrp6 oraz wytworzonego układu

Rozdział 1 Wstęp 7

8 Rozdział 1 Wstęp

Rozdział 2

Wstęp teoretyczny przegląddostępnych rozwiązań oraz możliwezastosowania układu

Jednostka inercyjna jest urządzeniem służącym do pomiaru przyspieszeń liniowychprędkości obrotowych oraz w niektoacuterych przypadkach orientacji w polu magnetycz-nym Zawiera ono w sobie troacutejosiowe akcelerometr oraz żyroskop (dodatkowo nie-ktoacutere modele zawierają troacutejosiowy magnetometr pozwalający na wyznaczenie orien-tacji względem pola magnetycznego) Urządzenia takie roacuteżnią się pomiędzy sobąparametrami oraz sposobem połączenia z urządzeniem nadrzędnym

W tym rozdziale czytelnik zostanie wprowadzony w podstawowe zagadnienia natemat działania jednostki inercyjnej Najpierw w sekcji 21 przedstawiony zosta-nie spis definicji oraz skroacutetoacutew Zasady działania jednostki inercyjnej zostały opi-sane w sekcji 22 W sekcji 23 przedstawione zostały podstawy działania magistraliEtherCAT oraz jej mechanizmoacutew

Jednostki inercyjne dzielą się przede wszystkim ze względu na obecność magne-tometru częstotliwość pomiaru (oraz dostarczania danych wyjściowych) oraz nasposoacuteb komunikacji Przykłady urządzeń komunikujących się za pomocą interfejsoacutewI2C oraz SPI opisane są w sekcji 24 a te komunikujące się za pomocą standardoacutewprzemysłowych opisane są w sekcji 25 Na końcu wzbogacony tą wiedzą w sekcji 26czytelnik pozna przykładowe zastosowania takiego urządzenia w zadaniach robotyki

21 Definicje

IMU - z Angielskiego Inertial Measurement Unit - zespoacuteł czujnikoacutew pozwalającyna pomiar Przyspieszeń ciała oraz prędkości kontowych z jakimi się porusza Cza-sami roacutewnież zawiera on Magnetometr pozwalający na pomiar orientacji urządzeniawzględem pola magnetycznego (zazwyczaj ziemi)

ETHERCAT - Ethernet for Control Automation Technology - jest standardemkomunikacji w urządzeniach automatyki Bazuje on na architekturze Ethernet Stan-dard ten został stworzony do zastosowań czasu rzeczywistego i pozwala na ustalenieczasu pomiaru z dokładnością do kilkudziesięciu mikrosekund

PDO - Process Data Object - Paczka danych procesowych służąca do przekazy-wania stanu urządzenia Ten typ obiektoacutew pozwala na ciągły odczyt danych

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 9

SDO - Service Data Object - Są to paczki danych pozwalające na konfiguracjeurządzenia podrzędnego

FMMU - jest urządzeniem odpowiedzialnym za mapowanie obiektoacutew PDO dopamięci urządzenia podrzędnego Pośredniczy ono w każdej komunikacji za pomocąmagistrali ethercat [7]

ROS - Robot Operating System - Jest to zbioacuter bibliotek oraz oprogramowaniezapewniające komunikacje międzyprocesowa spoacutejne środowisko uruchomienioweoraz środowisko do budowy oprogramowania

Węzeł - Jest to pojedyncza instancja programu uruchomionego w ramach opro-gramowania ROS

Temat - Jest to kanał komunikacji międzyprocesowej udostępniany przez opro-gramowanie ROS Topik posiada dwa typy Nodoacutew podłączonych do niego Publisherjet to node ktoacutery wysyła dane w danym topicu Subscriber jest nodem odbierającymdane z topicu Każdy kanał ma określony typ danych ktoacutere można na nim nadawaćoraz może mieć wielu nadawcoacutew (Publisher) i odbiorcoacutew (Subscriber)

OROCOS - Open RObot Control Software - jest otwartym oprogramowaniemsłużącym do sterowania systemami robotycznymi w czasie rzeczywistym Wszystkieakcje ktoacutere muszą być wykonywane w reżimie czasu rzeczywistego są uruchamianew ramach struktury OROCOSMCU - W skroacutecie mikrokontroler układ scalony zawierający w sobie rdzeń proce-sora oraz peryferia zewnętrzne W tym przypadku MCU to mikrokontrolerTM4C129ENCPDT wyposażony w rdzeń Cortex-M4F

MEMS - Od angielskiego microelectromechanical system Jest to zintegrowanyukład elektromechaniczny ktoacuterego co najmniej jeden wymiar znajduje się w skalimikro

22 Działanie jednostki inercyjnej

Jednostka inercyjna jest urządzeniem złożonym z kilku czujnikoacutew Najczęściej wszyst-kie zawarte w niej urządzenia wykonane sa w technologi MEMS Często spotykanąw ostatnich latach sytuacją jest że wszystkie te urządzenia znajdują się w jed-nym układzie scalonym Akcelerometry zawierające się w jednostkach inercyjnychwystępują praktycznie tylko w wersji MEMS a podstawy ich działania opisane sąw podsekcji 221 Żyroskopy roacutewnież w zdecydowanej większości występują jakowykonane w tej technologii elementy krzemowe ktoacuterych zasadę działania opisanow podsekcji 222 Jednak okazało się że można roacutewnież spotkać jednostki inercyjnezawierające żyroskopy światłowodowe przedstawione w podsekcji 223 Urządzeniate jednak najczęściej dedykowane są do zastosowań na dużych maszynach takich jakstatki morskie czy samoloty

221 Akcelerometry MEMS

Akcelerometry MEMSmożna dzielić na dwa typy Pierwszym z nich są akcelerometrypiezorezystancyjne [3] Akcelerometry te opierają się o dosyć prostą zasadę działania

10 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Masa odchylająca się pod wpływem siły bezwładności proacutebuje odkształcić piezorezy-story wmontowane w belkę na ktoacuterej jest zaczepiona Mierząc rezystancję wspomnia-nych piezorezystoroacutew można ustalić siłę działającą na masę oraz dzięki znajomościmasy wartość przyspieszenia ktoacuteremu poddawany jest układ Oczywiście wspoacutełcze-sne układy stosują zaawansowane techniki w celu zmniejszenia wpływu przyspieszeńw innych osiach niż badana oraz zabezpieczeniu przed uszkodzeniem wywołanymnadmiernymi przeciążeniami Jednak zasada działania pozostaje ta sama

Drugim rodzajem akcelerometroacutew MEMS są akcelerometry pojemnościowe Ak-celerometr pojemnościowy do swojego działania wymaga dwoacutech elementoacutew elek-trody stałej oraz elektrody połączonej z masą ktoacutera będzie poruszana siłą bezwład-ności Gdy masa zbliża się do elektrody stałej pojemność się zwiększa gdy natomiastsiła bezwładności działa w drugą stronę pojemność się zmniejsza Takie założeniapozwalają na pomiar przyspieszenia w akcelerometrze pojemnościowym

222 Żyroskopy MEMS

Obecnie produkowane żyroskopy MEMS to żyroskopy wibracyjne [3] Ich zasadadziałania jest nieco bardziej skomplikowana niż zasada działania akcelerometroacutewMEMS W układzie takim masę należy wprawić w wibracje wzdłuż jednej z osi (naprzykład x) leżącej na płaszczyźnie zawierającej dwie z nich (na przykład płaszczy-zna xy) W tej sytuacji pomiar możliwy będzie przy obrocie wokoacuteł trzeciej osi (z)Przy obrocie wokoacuteł osi Z czujnika siła Coriolisa przeniesie drgania z osi X na oś Ymierząc amplitudę tych drgań (na przykład za pomocą metody pojemnościowej jakw akcelerometrze pojemnościowym) można ustalić prędkość obrotu wokoacuteł danej osi

223 Żyroskopy światłowodowe

Działanie żyroskopoacutew światłowodowych oparte jest o efekt Sagnaca [12] jest toefekt polegający na przesunięciu w fazie dwuch rozszczepionych wiązek światła bie-gnących po pętli w przeciwnych kierunkach jeżeli układ się obraca Efekt ten jestspowodowany tym że w obracającym się układzie światło lecące w jedną stronę musipokonać dłuższą drogę niż światło lecące w stronę przeciwną Efekt ten stosuje sięw żyroskopach światłowodowych wielokrotnie zwiekszając drogę jaką musi pokonaćświatło poprzez zwiększenie ilości zwojoacutew światłowodu Żyroskopy takie mają bar-dzo wysoką czułość (nawet do jednej dziesięciotysięcznej stopnia na godzinę) orazbardzo niski dryft

23 Działanie magistrali Ethercat

EtherCAT jest standardem stworzonym przez firmę Beckhoff Automation Standardten jest jednak otwarty i powstają otwarte implementacje jego elementoacutew Choćsam standard swoje niższe warstwy opiera na Ethernecie (warstwę fizyczną orazczęść warstwy łącza danych) to wyższe z nich roacuteżnią się między sobą ze względuna zastosowanie na przykład protokołem warstwy aplikacji jest protokoacuteł Can OverEtherCAT opisany w podsekcji 231 W podsekcji 232 znajduje się wprowadzeniedo działania rozproszonego zegara magistrali EtherCAT

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 11

(a) Struktura ramki ethercat źroacutedło [5] (b) Struktura ramki danych źroacutedło [5]

Rysunek 21 Schematy przedstawiające strukturę ramki w komunikacji ethercat

231 COE

CANopen Over EtherCAT [5] jest protokołem warstwy aplikacji działającym w sieciEthercat jest to protokoacuteł komunikacyjny bazujący na systemie CAN Wykorzystanieimplementacji protokołu CoE sprawia że EtherCAT jest kompatybilny z urządze-niami obsługującymi CANopen Aby zrozumieć w jaki sposoacuteb datagramy CANopenprzekazywane są w protokole ethercat musimy przyjrzeć się strukturze jego ramkiTa (pokazana na rysunku 21(a)) zawiera czternastobajtowy nagłoacutewek ramki ether-cat dwubajtowe pole długości wiadomości wiadomość oraz czterobajtową sumękontrolną W polu wiadomości może znajdować się wiele datagramoacutew każdy z nichmoże być adresowany do innego urządzenia w magistrali ethercat Każdy datagramzawiera swoacutej nagłoacutewek ktoacutery adresuje go do danego urządzenia pole danych orazsumę kontrolną Skład pola danych pokazany na rysunku 21(b) a składa się onoz nagłoacutewka ktoacutery między innymi zawiera typ ktoacutery wskazuje na protokoacuteł warstwyaplikacji czyli na przykład CoE Następnie w ramce znajduje się komenda oraz od-powiadające jej dane

232 Synchronizacja zegara

Algorytm synchronizacji czasu w standardzie ethercat oparty jest o standard Pre-cision Time Protocol Jest to standard zaakceptowany przez IEEE o sygnaturzeIEEE 1588-2008 służący do submikrosekundowej synchronizacji czasu między urzą-dzeniami połączonymi w jedną sieć Więcej o działaniu samego protokołu możemyprzeczytać w publikacjach [15] [8] W protokole EtherCAT do synchronizacji zegarawyznaczany jest pierwsze urządzenie podrzędne na magistrali Ponieważ urządzeniapodrzędne magistrali EtherCAT wspierają synchronizacje sprzętową za pomocą pro-tokołu PTP możliwe jest odciążenie węzła głoacutewnego

24 Jednostki inercyjne ze złączami I2C oraz SPI

Szerokie spektrum tych urządzeń korzysta z interfejsoacutew I2C oraz SPI Taki spo-soacuteb podłączenia daje możliwość szybkiego przesyłu danych jednak ograniczeniemw takim przypadku jest odległość na jaka można prowadzić komunikację I2C niema zdefiniowanego maksymalnego zasięgu a maksymalną pojemność linii komuni-kacyjnej na poziomie 400 pF [16] co ogranicza zasięg komunikacji do pojedynczychmetroacutew w warunkach idealnych Magistrala SPI roacutewnież nie podaje maksymalnegozasięgu jednak w optymalnych warunkach jest to ok 2m

12 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Jednostki inercyjne ze złączami I2C oraz SPI są zazwyczaj tańszymi modelamiW większości są oferowane jako układy elektroniczne do montażu powierzchniowegoOto kilka przykładoacutew tego typu jednostek

bull LSM9DS0 - Jednostka inercyjna firmy ST electronics z interfejsem komunika-cyjnym SPII2C wyposażona w akcelerometr żyroskop i magnetometr Mak-symalne proacutebkowanie magnetometru to 100Hz akcelerometru 1600Hz żyro-skop ma maksymalną częstotliwość dostarczania danych 760Hz Jednostka do-starczana jest w obudowie LGA-24 o rozmiarach 4x4x1mm

bull ADIS16362 - Jednostka inercyjna firmy Analog devices jest to jednostka za-wierająca akcelerometr oraz żyroskop (oba troacutejosiowe) Charakteryzuje się onawysoką maksymalną częstotliwością proacutebkowania ponieważ dla obu czujnikoacutewwynosi ona 1200Hz Interfejs komunikacyjny stanowi złącze SPI Urządzeniedostarczane jest w obudowie w kształcie sześcianu 2x2 cm z miejscami naprzykręcenie jej do urządzenia W przeciwieństwie do poprzedniego urządze-nia jednostka posiada program kalibracji

25 Jednostki inercyjne ze złączami przemysłowymi

Inne z tych urządzeń komunikują się za pomocą dedykowanych standardoacutew prze-mysłowych ktoacutere charakteryzują się znacznie większą odpornością na zakłoacuteceniaa także na ciężkie warunki pracy Warto w tym miejscu zaznaczyć że jak dotąd narynku nie ma jednostki inercyjnej ktoacutera komunikowała by się za pomocą magistraliEthercat

Jednostki inercyjne omawiane w tym podrozdziale są urządzeniami ze złączamiprzemysłowymi przez co rozumiemy że złącza te charakteryzują się dużą nieza-wodnością Niestety modele zapewniające taki komfort są zazwyczaj znacznie więk-sze oraz droższe

bull MTi 100-series - Jednostki inercyjne firmy Xsens jest jednostką zawierającąakcelerometr i żyroskop Maksymalna częstotliwość dostarczania danych to2000Hz Jednostka ta komunikuje się za pomocą interfejsoacutew RS232 RS485422 UART USB Urządzenie to jest dostarczone w wodoodpornej obudo-wie mocowanej do urządzenia na śruby z przemysłowym złączem służącym dokomunikacji oraz zasilania Producent deklaruje że urządzenie posiada żyro-skop filtrujący wpływ wibracji

bull KVH 1750 IMU - Jednostka inercyjna firmy KVH jest urządzeniem posiada-jącym żyroskop światłowodowy oraz 3 oddzielne akcelerometry MEMS Inter-fejs komunikacyjny tego modułu to RS-422 Obudowa tej jednostki to waleco średnicy 89 mm i wysokości 74 mm przytwierdzany za pomocą sześciu śrubUrządzenie może produkować dane z częstotliwością do 1000Hz Charaktery-zuje się ono bardzo niskim dryftem żyroskopu Dryft ten typowo musi byćkorygowany za pomocą magnetometru

bull 6DF-1N2-C2-HWL - Jednostka inercyjna firmy Honeywell wyposażona jestw akcelerometr oraz żyroskop (oba troacutejosiowe) Interfejs komunikacyjny tegourządzenia to CAN Maksymalna częstotliwość dostarczania danych to 100Hz

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 13

26 Możliwe zastosowania proponowanego układu

Możliwe zastosowania jednostek inercyjnych w robotyce pojawiają się tam gdziewystępuje ruch inny od jednostajnego prostoliniowego W tym rozdziale rozważymydwa zastosowania w ktoacuterych proponowany układ może pokazać swoje zalety wy-nikające ze sposobu komunikacji W sekcji 261 omoacutewimy przypadek zastosowaniajednostki inercyjnej do wspomagania procesu nawigacji robota mobilnego o napę-dzie roacuteżnicowym Sekcja 262 dotyczyć będzie przypadku użycia takiego urządzeniado korekty odczytu siły z czujnika umieszczonego w nadgarstku manipulatora

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym

W tej sekcji przedstawiony zostanie przykład nawigacji robota mobilnego o napę-dzie roacuteżnicowym Przykładem takiego robota może być robot o konstrukcji takiejjak baza mobilna elektron [10] czyli robot posiadający wiele koacuteł po jednej stro-nie co generuje problemy z odtworzeniem trajektorii ruchu robota przez odczytyz czujnikoacutew odometrycznych Dzieje się tak przez to że budowa robota wprowa-dza poślizgi podczas skrętoacutew Robot taki ze względu na roacuteżne czynniki zewnętrznenp typ podłoża nieroacutewności podłoża skręca w okoacuteł trudnej do ustalenia osi obrotuktoacutera w dodatku może być zmienna (np gdy jedno koło jest na innym typie podłożaniż wszystkie inne) Aby temu zapobiec można użyć do estymacji trajektorii ruchurobota pomiaroacutew pochodzących z czujnika inercyjnego razem z odczytami z senso-roacutew odometrycznych Aby tego dokonać potrzebna jest fuzja odczytoacutew z roacuteżnychczujnikoacutew Istnieje wiele metod podejścia do tego tematu jednak jednym z częściejspotykanych w literaturze jest użycie rozszerzonego filtru Kalmana [18] [17] [4]

262 Korekcja odczytu siły z czujnika nadgarstkowego

Drugim przykładem zastosowania jednostki inercyjnej w robotyce jest korekcja od-czytu z czujnika siły [20] Nie jest to zastosowanie tak szeroko omawiane w literaturzejak nawigacja wspomagana inercyjnie opisana w sekcji 261 Pomimo niewielkiegozainteresowania jest to bardzo ciekawy temat badawczy głoacutewnie ze względu na chęćwyeliminowania bdquopikoacutewrdquo odczytoacutew siły wynikających z nagłej zmiany kierunku ru-chu ramienia bądź też drgnięcia wynikającego z niedoskonałości regulatora Do tegozastosowania konstruuje się obserwator siły oparty o filtr Kalmana [6]

14 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Streszczenie

Układ elektroniczny do akwizycji danych z jednostkiinercyjnej za pośrednictwem EtherCAT

Przedmiotem niniejszej pracy jest stworzenie oraz oprogramowanie układu elek-tronicznego umożliwiającego podłączenie jednostki inercyjnej ADIS16362 do magi-strali EtherCAT W pracy tej opisana jest struktura zaprojektowanego układu odmodułu głoacutewnego poprzez urządzenie komunikujące się z istniejącą infrastrukturąkomunikacyjną EtherCAT po podłączenie jednostki inercyjnej Znajdziemy roacutew-nież opis wytworzonego oprogramowania znajdującego się zaroacutewno na wytworzonymurządzeniu jak i urządzeniu nadrzędnym w komunikacji (komputerze PC) W ramachpracy został roacutewnież przeprowadzony przegląd dostępnych na rynku rozwiązań tegotypu oraz algorytmoacutew służących do korekcji odczytu siły Zostały roacutewnież zaimple-mentowane programy testowe i przeprowadzone na ich podstawie badania

Podjęcie tego tematu było motywowane wynikami eksperymentoacutew z jednostkąinercyjną komunikującą się za pomocą interfejsu USB Ujawniły one zmienne poacuteź-nienie w komunikacji Opoacuteźnienie to wahało się nawet do kilkudziesięciu milisekundDodatkowym powodem wyboru sposobu komunikacji była chęć ujednolicenia sposo-boacutew połączenia urządzeń w ramach całego robota

Słowa kluczowe EtherCAT IMU Anybus B40 IRp-6

2

Abstract

The electronic system for data acquisition from inertial unitvia EtherCAT

The subject of this thesis is to create software and electronic system that allowsconnecting the ADIS16362 inertial unit to the EtherCAT bus In this work thestructure of the designed system is described from the main hardware unit by thedevice communicating with existing EtherCAT infrastructure to hardware interfaceconnecting to IMU In this paper we also find a description of the produced softwarelocated on both the manufactured device and the master communication device(PC) As a part of the work was also carried out a review of commercially availablesolutions of this type and correction algorithms for force measurement There havealso been implemented test programs and executed experiments with its use

The choice of this topic was motivated by the result of experiments with iner-tial unit communicating via the USB interface They revealed a variable delay incommunication This delay ranged up to tens of milliseconds Another reason forchoosing this way of communication was to standardize the ways to link deviceswithin the entire robot

Keywords EtherCAT IMU Anybus B40 IRp-6

3

4

Spis treści

1 Wstęp 711 Założenia i problematyka pracy 712 Układ Pracy 7

2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwezastosowania układu 921 Definicje 922 Działanie jednostki inercyjnej 10

221 Akcelerometry MEMS 10222 Żyroskopy MEMS 11223 Żyroskopy światłowodowe 11

23 Działanie magistrali Ethercat 11231 COE 12232 Synchronizacja zegara 12

24 Jednostki inercyjne ze złączami I2C oraz SPI 1225 Jednostki inercyjne ze złączami przemysłowymi 1326 Możliwe zastosowania proponowanego układu 14

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym 14262 Korekcja odczytu siły z czujnika nadgarstkowego 14

3 Opis budowanego układu 1531 Opis Układu 15

311 Moduł głoacutewny 16312 Moduł jednostki inercyjnej (IMU) 16313 Anybus M40 EtherCAT 17

32 Opis złącz 17321 Złącze międzypłytkowe 17322 Złącze zasilania 18323 Złącze interfejsu EtherCAT 19324 Złącze Jtag 20325 Złącze Anybus B40-CPU 20

4 Opis oprogramowania 2341 Protokoły komunikacyjne 23

411 Standard EtherCAT 23412 Can over EtherCAT 23

42 Oprogramowanie MCU 24421 Konfiguracja peryferioacutew 24422 Sterownik Anybus 25423 Przerwania 30

43 Sterownik w strukturze OROCOS 31

Spis treści 5

5 Badania weryfikacyjne 3551 Opis stanowiska eksperymentalnego 3552 Projekt eksperymentoacutew 3553 Wyniki eksperymentoacutew 3754 Podsumowanie 42

A Schematy modułoacutew urządzenia 51

B Wykresy pobranych trajektorii 57

6 Spis treści

Rozdział 1

Wstęp

11 Założenia i problematyka pracy

Robotyka jest bardzo szeroką i ciekawą dziedziną Z czasem systemy robotyczne wy-posażone są w coraz większą ilość czujnikoacutew ktoacutere mierzą najroacuteżniejsze parametrypracy robota jak i roacutewnież środowiska w ktoacuterym pracuje

Założeniem pracy było stworzenie układu interfejsującego jednostkę inercyjnąADIS16362 Rozwiązanie musiało zapewnić niezawodną komunikacje z kompute-rem sterującym robotem Ponieważ systemy robotyczne muszą działać w reżimieczasu rzeczywistego roacutewnież komunikacja z projektowanym układem musiała dzia-łać w ścisłym reżimie czasowym Dodatkowo ważnym czynnikiem decydującym o uży-teczności w niektoacuterych systemach robotycznych jest nie tylko okres pomiaru aleroacutewnież ustalenie momentu pomiaru z jak największą dokładnością Dlatego wymo-giem okazał się pomiar synchronizowany do czego potrzebny jest interfejs umożli-wiający synchronizację zegara we wszystkich urządzeniach magistrali Dodatkowymograniczeniem był rozmiar ponieważ układ ma nadać się do montażu w ograni-czonej przestrzeni nadgarstka robota IRP6 oraz w potencjalnych innych robotachgdzie występują ograniczenia przestrzenne Układ został więc zbudowany z mięk-kim (z możliwością niewielkiego przekroczenia) ograniczeniem rozmiaru ustalonymna 50x50x50mm

12 Układ Pracy

Niniejsza praca składa się z pięciu rozdziałoacutew Rozdział 2 zawiera podstawy teore-tyczne działania jednostki inercyjnej oraz jej elementoacutew magistrali EtherCAT orazprzegląd dostępnych na rynku jednostek inercyjnych i przykłady zastosowania jed-nostek inercyjnych w robotyce Poroacutewnywane są w nim ich parametry oraz sposobykomunikacji ich wady i zalety Opisane są tam roacutewnież przykładowe zastosowaniajednostki inercyjnej w robotyce usługowej Rozdział 3 zawiera opis mechaniczny orazelektroniczny budowanego układu opis złącz oraz wyprowadzonych na ich styki sy-gnałoacutew W rozdziale 4 znajduje się opis oprogramowania wytworzonego w ramachpracy znajdującego się zaroacutewno w wytworzonym urządzeniu jak i tego pracującegona komputerze sterującym robota Wreszcie rozdział 5 w ktoacuterym znajdziemy pro-jekt oraz wyniki eksperymentoacutew przeprowadzonych z wykorzystaniem manipulatoraIrp6 oraz wytworzonego układu

Rozdział 1 Wstęp 7

8 Rozdział 1 Wstęp

Rozdział 2

Wstęp teoretyczny przegląddostępnych rozwiązań oraz możliwezastosowania układu

Jednostka inercyjna jest urządzeniem służącym do pomiaru przyspieszeń liniowychprędkości obrotowych oraz w niektoacuterych przypadkach orientacji w polu magnetycz-nym Zawiera ono w sobie troacutejosiowe akcelerometr oraz żyroskop (dodatkowo nie-ktoacutere modele zawierają troacutejosiowy magnetometr pozwalający na wyznaczenie orien-tacji względem pola magnetycznego) Urządzenia takie roacuteżnią się pomiędzy sobąparametrami oraz sposobem połączenia z urządzeniem nadrzędnym

W tym rozdziale czytelnik zostanie wprowadzony w podstawowe zagadnienia natemat działania jednostki inercyjnej Najpierw w sekcji 21 przedstawiony zosta-nie spis definicji oraz skroacutetoacutew Zasady działania jednostki inercyjnej zostały opi-sane w sekcji 22 W sekcji 23 przedstawione zostały podstawy działania magistraliEtherCAT oraz jej mechanizmoacutew

Jednostki inercyjne dzielą się przede wszystkim ze względu na obecność magne-tometru częstotliwość pomiaru (oraz dostarczania danych wyjściowych) oraz nasposoacuteb komunikacji Przykłady urządzeń komunikujących się za pomocą interfejsoacutewI2C oraz SPI opisane są w sekcji 24 a te komunikujące się za pomocą standardoacutewprzemysłowych opisane są w sekcji 25 Na końcu wzbogacony tą wiedzą w sekcji 26czytelnik pozna przykładowe zastosowania takiego urządzenia w zadaniach robotyki

21 Definicje

IMU - z Angielskiego Inertial Measurement Unit - zespoacuteł czujnikoacutew pozwalającyna pomiar Przyspieszeń ciała oraz prędkości kontowych z jakimi się porusza Cza-sami roacutewnież zawiera on Magnetometr pozwalający na pomiar orientacji urządzeniawzględem pola magnetycznego (zazwyczaj ziemi)

ETHERCAT - Ethernet for Control Automation Technology - jest standardemkomunikacji w urządzeniach automatyki Bazuje on na architekturze Ethernet Stan-dard ten został stworzony do zastosowań czasu rzeczywistego i pozwala na ustalenieczasu pomiaru z dokładnością do kilkudziesięciu mikrosekund

PDO - Process Data Object - Paczka danych procesowych służąca do przekazy-wania stanu urządzenia Ten typ obiektoacutew pozwala na ciągły odczyt danych

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 9

SDO - Service Data Object - Są to paczki danych pozwalające na konfiguracjeurządzenia podrzędnego

FMMU - jest urządzeniem odpowiedzialnym za mapowanie obiektoacutew PDO dopamięci urządzenia podrzędnego Pośredniczy ono w każdej komunikacji za pomocąmagistrali ethercat [7]

ROS - Robot Operating System - Jest to zbioacuter bibliotek oraz oprogramowaniezapewniające komunikacje międzyprocesowa spoacutejne środowisko uruchomienioweoraz środowisko do budowy oprogramowania

Węzeł - Jest to pojedyncza instancja programu uruchomionego w ramach opro-gramowania ROS

Temat - Jest to kanał komunikacji międzyprocesowej udostępniany przez opro-gramowanie ROS Topik posiada dwa typy Nodoacutew podłączonych do niego Publisherjet to node ktoacutery wysyła dane w danym topicu Subscriber jest nodem odbierającymdane z topicu Każdy kanał ma określony typ danych ktoacutere można na nim nadawaćoraz może mieć wielu nadawcoacutew (Publisher) i odbiorcoacutew (Subscriber)

OROCOS - Open RObot Control Software - jest otwartym oprogramowaniemsłużącym do sterowania systemami robotycznymi w czasie rzeczywistym Wszystkieakcje ktoacutere muszą być wykonywane w reżimie czasu rzeczywistego są uruchamianew ramach struktury OROCOSMCU - W skroacutecie mikrokontroler układ scalony zawierający w sobie rdzeń proce-sora oraz peryferia zewnętrzne W tym przypadku MCU to mikrokontrolerTM4C129ENCPDT wyposażony w rdzeń Cortex-M4F

MEMS - Od angielskiego microelectromechanical system Jest to zintegrowanyukład elektromechaniczny ktoacuterego co najmniej jeden wymiar znajduje się w skalimikro

22 Działanie jednostki inercyjnej

Jednostka inercyjna jest urządzeniem złożonym z kilku czujnikoacutew Najczęściej wszyst-kie zawarte w niej urządzenia wykonane sa w technologi MEMS Często spotykanąw ostatnich latach sytuacją jest że wszystkie te urządzenia znajdują się w jed-nym układzie scalonym Akcelerometry zawierające się w jednostkach inercyjnychwystępują praktycznie tylko w wersji MEMS a podstawy ich działania opisane sąw podsekcji 221 Żyroskopy roacutewnież w zdecydowanej większości występują jakowykonane w tej technologii elementy krzemowe ktoacuterych zasadę działania opisanow podsekcji 222 Jednak okazało się że można roacutewnież spotkać jednostki inercyjnezawierające żyroskopy światłowodowe przedstawione w podsekcji 223 Urządzeniate jednak najczęściej dedykowane są do zastosowań na dużych maszynach takich jakstatki morskie czy samoloty

221 Akcelerometry MEMS

Akcelerometry MEMSmożna dzielić na dwa typy Pierwszym z nich są akcelerometrypiezorezystancyjne [3] Akcelerometry te opierają się o dosyć prostą zasadę działania

10 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Masa odchylająca się pod wpływem siły bezwładności proacutebuje odkształcić piezorezy-story wmontowane w belkę na ktoacuterej jest zaczepiona Mierząc rezystancję wspomnia-nych piezorezystoroacutew można ustalić siłę działającą na masę oraz dzięki znajomościmasy wartość przyspieszenia ktoacuteremu poddawany jest układ Oczywiście wspoacutełcze-sne układy stosują zaawansowane techniki w celu zmniejszenia wpływu przyspieszeńw innych osiach niż badana oraz zabezpieczeniu przed uszkodzeniem wywołanymnadmiernymi przeciążeniami Jednak zasada działania pozostaje ta sama

Drugim rodzajem akcelerometroacutew MEMS są akcelerometry pojemnościowe Ak-celerometr pojemnościowy do swojego działania wymaga dwoacutech elementoacutew elek-trody stałej oraz elektrody połączonej z masą ktoacutera będzie poruszana siłą bezwład-ności Gdy masa zbliża się do elektrody stałej pojemność się zwiększa gdy natomiastsiła bezwładności działa w drugą stronę pojemność się zmniejsza Takie założeniapozwalają na pomiar przyspieszenia w akcelerometrze pojemnościowym

222 Żyroskopy MEMS

Obecnie produkowane żyroskopy MEMS to żyroskopy wibracyjne [3] Ich zasadadziałania jest nieco bardziej skomplikowana niż zasada działania akcelerometroacutewMEMS W układzie takim masę należy wprawić w wibracje wzdłuż jednej z osi (naprzykład x) leżącej na płaszczyźnie zawierającej dwie z nich (na przykład płaszczy-zna xy) W tej sytuacji pomiar możliwy będzie przy obrocie wokoacuteł trzeciej osi (z)Przy obrocie wokoacuteł osi Z czujnika siła Coriolisa przeniesie drgania z osi X na oś Ymierząc amplitudę tych drgań (na przykład za pomocą metody pojemnościowej jakw akcelerometrze pojemnościowym) można ustalić prędkość obrotu wokoacuteł danej osi

223 Żyroskopy światłowodowe

Działanie żyroskopoacutew światłowodowych oparte jest o efekt Sagnaca [12] jest toefekt polegający na przesunięciu w fazie dwuch rozszczepionych wiązek światła bie-gnących po pętli w przeciwnych kierunkach jeżeli układ się obraca Efekt ten jestspowodowany tym że w obracającym się układzie światło lecące w jedną stronę musipokonać dłuższą drogę niż światło lecące w stronę przeciwną Efekt ten stosuje sięw żyroskopach światłowodowych wielokrotnie zwiekszając drogę jaką musi pokonaćświatło poprzez zwiększenie ilości zwojoacutew światłowodu Żyroskopy takie mają bar-dzo wysoką czułość (nawet do jednej dziesięciotysięcznej stopnia na godzinę) orazbardzo niski dryft

23 Działanie magistrali Ethercat

EtherCAT jest standardem stworzonym przez firmę Beckhoff Automation Standardten jest jednak otwarty i powstają otwarte implementacje jego elementoacutew Choćsam standard swoje niższe warstwy opiera na Ethernecie (warstwę fizyczną orazczęść warstwy łącza danych) to wyższe z nich roacuteżnią się między sobą ze względuna zastosowanie na przykład protokołem warstwy aplikacji jest protokoacuteł Can OverEtherCAT opisany w podsekcji 231 W podsekcji 232 znajduje się wprowadzeniedo działania rozproszonego zegara magistrali EtherCAT

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 11

(a) Struktura ramki ethercat źroacutedło [5] (b) Struktura ramki danych źroacutedło [5]

Rysunek 21 Schematy przedstawiające strukturę ramki w komunikacji ethercat

231 COE

CANopen Over EtherCAT [5] jest protokołem warstwy aplikacji działającym w sieciEthercat jest to protokoacuteł komunikacyjny bazujący na systemie CAN Wykorzystanieimplementacji protokołu CoE sprawia że EtherCAT jest kompatybilny z urządze-niami obsługującymi CANopen Aby zrozumieć w jaki sposoacuteb datagramy CANopenprzekazywane są w protokole ethercat musimy przyjrzeć się strukturze jego ramkiTa (pokazana na rysunku 21(a)) zawiera czternastobajtowy nagłoacutewek ramki ether-cat dwubajtowe pole długości wiadomości wiadomość oraz czterobajtową sumękontrolną W polu wiadomości może znajdować się wiele datagramoacutew każdy z nichmoże być adresowany do innego urządzenia w magistrali ethercat Każdy datagramzawiera swoacutej nagłoacutewek ktoacutery adresuje go do danego urządzenia pole danych orazsumę kontrolną Skład pola danych pokazany na rysunku 21(b) a składa się onoz nagłoacutewka ktoacutery między innymi zawiera typ ktoacutery wskazuje na protokoacuteł warstwyaplikacji czyli na przykład CoE Następnie w ramce znajduje się komenda oraz od-powiadające jej dane

232 Synchronizacja zegara

Algorytm synchronizacji czasu w standardzie ethercat oparty jest o standard Pre-cision Time Protocol Jest to standard zaakceptowany przez IEEE o sygnaturzeIEEE 1588-2008 służący do submikrosekundowej synchronizacji czasu między urzą-dzeniami połączonymi w jedną sieć Więcej o działaniu samego protokołu możemyprzeczytać w publikacjach [15] [8] W protokole EtherCAT do synchronizacji zegarawyznaczany jest pierwsze urządzenie podrzędne na magistrali Ponieważ urządzeniapodrzędne magistrali EtherCAT wspierają synchronizacje sprzętową za pomocą pro-tokołu PTP możliwe jest odciążenie węzła głoacutewnego

24 Jednostki inercyjne ze złączami I2C oraz SPI

Szerokie spektrum tych urządzeń korzysta z interfejsoacutew I2C oraz SPI Taki spo-soacuteb podłączenia daje możliwość szybkiego przesyłu danych jednak ograniczeniemw takim przypadku jest odległość na jaka można prowadzić komunikację I2C niema zdefiniowanego maksymalnego zasięgu a maksymalną pojemność linii komuni-kacyjnej na poziomie 400 pF [16] co ogranicza zasięg komunikacji do pojedynczychmetroacutew w warunkach idealnych Magistrala SPI roacutewnież nie podaje maksymalnegozasięgu jednak w optymalnych warunkach jest to ok 2m

12 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Jednostki inercyjne ze złączami I2C oraz SPI są zazwyczaj tańszymi modelamiW większości są oferowane jako układy elektroniczne do montażu powierzchniowegoOto kilka przykładoacutew tego typu jednostek

bull LSM9DS0 - Jednostka inercyjna firmy ST electronics z interfejsem komunika-cyjnym SPII2C wyposażona w akcelerometr żyroskop i magnetometr Mak-symalne proacutebkowanie magnetometru to 100Hz akcelerometru 1600Hz żyro-skop ma maksymalną częstotliwość dostarczania danych 760Hz Jednostka do-starczana jest w obudowie LGA-24 o rozmiarach 4x4x1mm

bull ADIS16362 - Jednostka inercyjna firmy Analog devices jest to jednostka za-wierająca akcelerometr oraz żyroskop (oba troacutejosiowe) Charakteryzuje się onawysoką maksymalną częstotliwością proacutebkowania ponieważ dla obu czujnikoacutewwynosi ona 1200Hz Interfejs komunikacyjny stanowi złącze SPI Urządzeniedostarczane jest w obudowie w kształcie sześcianu 2x2 cm z miejscami naprzykręcenie jej do urządzenia W przeciwieństwie do poprzedniego urządze-nia jednostka posiada program kalibracji

25 Jednostki inercyjne ze złączami przemysłowymi

Inne z tych urządzeń komunikują się za pomocą dedykowanych standardoacutew prze-mysłowych ktoacutere charakteryzują się znacznie większą odpornością na zakłoacuteceniaa także na ciężkie warunki pracy Warto w tym miejscu zaznaczyć że jak dotąd narynku nie ma jednostki inercyjnej ktoacutera komunikowała by się za pomocą magistraliEthercat

Jednostki inercyjne omawiane w tym podrozdziale są urządzeniami ze złączamiprzemysłowymi przez co rozumiemy że złącza te charakteryzują się dużą nieza-wodnością Niestety modele zapewniające taki komfort są zazwyczaj znacznie więk-sze oraz droższe

bull MTi 100-series - Jednostki inercyjne firmy Xsens jest jednostką zawierającąakcelerometr i żyroskop Maksymalna częstotliwość dostarczania danych to2000Hz Jednostka ta komunikuje się za pomocą interfejsoacutew RS232 RS485422 UART USB Urządzenie to jest dostarczone w wodoodpornej obudo-wie mocowanej do urządzenia na śruby z przemysłowym złączem służącym dokomunikacji oraz zasilania Producent deklaruje że urządzenie posiada żyro-skop filtrujący wpływ wibracji

bull KVH 1750 IMU - Jednostka inercyjna firmy KVH jest urządzeniem posiada-jącym żyroskop światłowodowy oraz 3 oddzielne akcelerometry MEMS Inter-fejs komunikacyjny tego modułu to RS-422 Obudowa tej jednostki to waleco średnicy 89 mm i wysokości 74 mm przytwierdzany za pomocą sześciu śrubUrządzenie może produkować dane z częstotliwością do 1000Hz Charaktery-zuje się ono bardzo niskim dryftem żyroskopu Dryft ten typowo musi byćkorygowany za pomocą magnetometru

bull 6DF-1N2-C2-HWL - Jednostka inercyjna firmy Honeywell wyposażona jestw akcelerometr oraz żyroskop (oba troacutejosiowe) Interfejs komunikacyjny tegourządzenia to CAN Maksymalna częstotliwość dostarczania danych to 100Hz

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 13

26 Możliwe zastosowania proponowanego układu

Możliwe zastosowania jednostek inercyjnych w robotyce pojawiają się tam gdziewystępuje ruch inny od jednostajnego prostoliniowego W tym rozdziale rozważymydwa zastosowania w ktoacuterych proponowany układ może pokazać swoje zalety wy-nikające ze sposobu komunikacji W sekcji 261 omoacutewimy przypadek zastosowaniajednostki inercyjnej do wspomagania procesu nawigacji robota mobilnego o napę-dzie roacuteżnicowym Sekcja 262 dotyczyć będzie przypadku użycia takiego urządzeniado korekty odczytu siły z czujnika umieszczonego w nadgarstku manipulatora

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym

W tej sekcji przedstawiony zostanie przykład nawigacji robota mobilnego o napę-dzie roacuteżnicowym Przykładem takiego robota może być robot o konstrukcji takiejjak baza mobilna elektron [10] czyli robot posiadający wiele koacuteł po jednej stro-nie co generuje problemy z odtworzeniem trajektorii ruchu robota przez odczytyz czujnikoacutew odometrycznych Dzieje się tak przez to że budowa robota wprowa-dza poślizgi podczas skrętoacutew Robot taki ze względu na roacuteżne czynniki zewnętrznenp typ podłoża nieroacutewności podłoża skręca w okoacuteł trudnej do ustalenia osi obrotuktoacutera w dodatku może być zmienna (np gdy jedno koło jest na innym typie podłożaniż wszystkie inne) Aby temu zapobiec można użyć do estymacji trajektorii ruchurobota pomiaroacutew pochodzących z czujnika inercyjnego razem z odczytami z senso-roacutew odometrycznych Aby tego dokonać potrzebna jest fuzja odczytoacutew z roacuteżnychczujnikoacutew Istnieje wiele metod podejścia do tego tematu jednak jednym z częściejspotykanych w literaturze jest użycie rozszerzonego filtru Kalmana [18] [17] [4]

262 Korekcja odczytu siły z czujnika nadgarstkowego

Drugim przykładem zastosowania jednostki inercyjnej w robotyce jest korekcja od-czytu z czujnika siły [20] Nie jest to zastosowanie tak szeroko omawiane w literaturzejak nawigacja wspomagana inercyjnie opisana w sekcji 261 Pomimo niewielkiegozainteresowania jest to bardzo ciekawy temat badawczy głoacutewnie ze względu na chęćwyeliminowania bdquopikoacutewrdquo odczytoacutew siły wynikających z nagłej zmiany kierunku ru-chu ramienia bądź też drgnięcia wynikającego z niedoskonałości regulatora Do tegozastosowania konstruuje się obserwator siły oparty o filtr Kalmana [6]

14 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Abstract

The electronic system for data acquisition from inertial unitvia EtherCAT

The subject of this thesis is to create software and electronic system that allowsconnecting the ADIS16362 inertial unit to the EtherCAT bus In this work thestructure of the designed system is described from the main hardware unit by thedevice communicating with existing EtherCAT infrastructure to hardware interfaceconnecting to IMU In this paper we also find a description of the produced softwarelocated on both the manufactured device and the master communication device(PC) As a part of the work was also carried out a review of commercially availablesolutions of this type and correction algorithms for force measurement There havealso been implemented test programs and executed experiments with its use

The choice of this topic was motivated by the result of experiments with iner-tial unit communicating via the USB interface They revealed a variable delay incommunication This delay ranged up to tens of milliseconds Another reason forchoosing this way of communication was to standardize the ways to link deviceswithin the entire robot

Keywords EtherCAT IMU Anybus B40 IRp-6

3

4

Spis treści

1 Wstęp 711 Założenia i problematyka pracy 712 Układ Pracy 7

2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwezastosowania układu 921 Definicje 922 Działanie jednostki inercyjnej 10

221 Akcelerometry MEMS 10222 Żyroskopy MEMS 11223 Żyroskopy światłowodowe 11

23 Działanie magistrali Ethercat 11231 COE 12232 Synchronizacja zegara 12

24 Jednostki inercyjne ze złączami I2C oraz SPI 1225 Jednostki inercyjne ze złączami przemysłowymi 1326 Możliwe zastosowania proponowanego układu 14

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym 14262 Korekcja odczytu siły z czujnika nadgarstkowego 14

3 Opis budowanego układu 1531 Opis Układu 15

311 Moduł głoacutewny 16312 Moduł jednostki inercyjnej (IMU) 16313 Anybus M40 EtherCAT 17

32 Opis złącz 17321 Złącze międzypłytkowe 17322 Złącze zasilania 18323 Złącze interfejsu EtherCAT 19324 Złącze Jtag 20325 Złącze Anybus B40-CPU 20

4 Opis oprogramowania 2341 Protokoły komunikacyjne 23

411 Standard EtherCAT 23412 Can over EtherCAT 23

42 Oprogramowanie MCU 24421 Konfiguracja peryferioacutew 24422 Sterownik Anybus 25423 Przerwania 30

43 Sterownik w strukturze OROCOS 31

Spis treści 5

5 Badania weryfikacyjne 3551 Opis stanowiska eksperymentalnego 3552 Projekt eksperymentoacutew 3553 Wyniki eksperymentoacutew 3754 Podsumowanie 42

A Schematy modułoacutew urządzenia 51

B Wykresy pobranych trajektorii 57

6 Spis treści

Rozdział 1

Wstęp

11 Założenia i problematyka pracy

Robotyka jest bardzo szeroką i ciekawą dziedziną Z czasem systemy robotyczne wy-posażone są w coraz większą ilość czujnikoacutew ktoacutere mierzą najroacuteżniejsze parametrypracy robota jak i roacutewnież środowiska w ktoacuterym pracuje

Założeniem pracy było stworzenie układu interfejsującego jednostkę inercyjnąADIS16362 Rozwiązanie musiało zapewnić niezawodną komunikacje z kompute-rem sterującym robotem Ponieważ systemy robotyczne muszą działać w reżimieczasu rzeczywistego roacutewnież komunikacja z projektowanym układem musiała dzia-łać w ścisłym reżimie czasowym Dodatkowo ważnym czynnikiem decydującym o uży-teczności w niektoacuterych systemach robotycznych jest nie tylko okres pomiaru aleroacutewnież ustalenie momentu pomiaru z jak największą dokładnością Dlatego wymo-giem okazał się pomiar synchronizowany do czego potrzebny jest interfejs umożli-wiający synchronizację zegara we wszystkich urządzeniach magistrali Dodatkowymograniczeniem był rozmiar ponieważ układ ma nadać się do montażu w ograni-czonej przestrzeni nadgarstka robota IRP6 oraz w potencjalnych innych robotachgdzie występują ograniczenia przestrzenne Układ został więc zbudowany z mięk-kim (z możliwością niewielkiego przekroczenia) ograniczeniem rozmiaru ustalonymna 50x50x50mm

12 Układ Pracy

Niniejsza praca składa się z pięciu rozdziałoacutew Rozdział 2 zawiera podstawy teore-tyczne działania jednostki inercyjnej oraz jej elementoacutew magistrali EtherCAT orazprzegląd dostępnych na rynku jednostek inercyjnych i przykłady zastosowania jed-nostek inercyjnych w robotyce Poroacutewnywane są w nim ich parametry oraz sposobykomunikacji ich wady i zalety Opisane są tam roacutewnież przykładowe zastosowaniajednostki inercyjnej w robotyce usługowej Rozdział 3 zawiera opis mechaniczny orazelektroniczny budowanego układu opis złącz oraz wyprowadzonych na ich styki sy-gnałoacutew W rozdziale 4 znajduje się opis oprogramowania wytworzonego w ramachpracy znajdującego się zaroacutewno w wytworzonym urządzeniu jak i tego pracującegona komputerze sterującym robota Wreszcie rozdział 5 w ktoacuterym znajdziemy pro-jekt oraz wyniki eksperymentoacutew przeprowadzonych z wykorzystaniem manipulatoraIrp6 oraz wytworzonego układu

Rozdział 1 Wstęp 7

8 Rozdział 1 Wstęp

Rozdział 2

Wstęp teoretyczny przegląddostępnych rozwiązań oraz możliwezastosowania układu

Jednostka inercyjna jest urządzeniem służącym do pomiaru przyspieszeń liniowychprędkości obrotowych oraz w niektoacuterych przypadkach orientacji w polu magnetycz-nym Zawiera ono w sobie troacutejosiowe akcelerometr oraz żyroskop (dodatkowo nie-ktoacutere modele zawierają troacutejosiowy magnetometr pozwalający na wyznaczenie orien-tacji względem pola magnetycznego) Urządzenia takie roacuteżnią się pomiędzy sobąparametrami oraz sposobem połączenia z urządzeniem nadrzędnym

W tym rozdziale czytelnik zostanie wprowadzony w podstawowe zagadnienia natemat działania jednostki inercyjnej Najpierw w sekcji 21 przedstawiony zosta-nie spis definicji oraz skroacutetoacutew Zasady działania jednostki inercyjnej zostały opi-sane w sekcji 22 W sekcji 23 przedstawione zostały podstawy działania magistraliEtherCAT oraz jej mechanizmoacutew

Jednostki inercyjne dzielą się przede wszystkim ze względu na obecność magne-tometru częstotliwość pomiaru (oraz dostarczania danych wyjściowych) oraz nasposoacuteb komunikacji Przykłady urządzeń komunikujących się za pomocą interfejsoacutewI2C oraz SPI opisane są w sekcji 24 a te komunikujące się za pomocą standardoacutewprzemysłowych opisane są w sekcji 25 Na końcu wzbogacony tą wiedzą w sekcji 26czytelnik pozna przykładowe zastosowania takiego urządzenia w zadaniach robotyki

21 Definicje

IMU - z Angielskiego Inertial Measurement Unit - zespoacuteł czujnikoacutew pozwalającyna pomiar Przyspieszeń ciała oraz prędkości kontowych z jakimi się porusza Cza-sami roacutewnież zawiera on Magnetometr pozwalający na pomiar orientacji urządzeniawzględem pola magnetycznego (zazwyczaj ziemi)

ETHERCAT - Ethernet for Control Automation Technology - jest standardemkomunikacji w urządzeniach automatyki Bazuje on na architekturze Ethernet Stan-dard ten został stworzony do zastosowań czasu rzeczywistego i pozwala na ustalenieczasu pomiaru z dokładnością do kilkudziesięciu mikrosekund

PDO - Process Data Object - Paczka danych procesowych służąca do przekazy-wania stanu urządzenia Ten typ obiektoacutew pozwala na ciągły odczyt danych

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 9

SDO - Service Data Object - Są to paczki danych pozwalające na konfiguracjeurządzenia podrzędnego

FMMU - jest urządzeniem odpowiedzialnym za mapowanie obiektoacutew PDO dopamięci urządzenia podrzędnego Pośredniczy ono w każdej komunikacji za pomocąmagistrali ethercat [7]

ROS - Robot Operating System - Jest to zbioacuter bibliotek oraz oprogramowaniezapewniające komunikacje międzyprocesowa spoacutejne środowisko uruchomienioweoraz środowisko do budowy oprogramowania

Węzeł - Jest to pojedyncza instancja programu uruchomionego w ramach opro-gramowania ROS

Temat - Jest to kanał komunikacji międzyprocesowej udostępniany przez opro-gramowanie ROS Topik posiada dwa typy Nodoacutew podłączonych do niego Publisherjet to node ktoacutery wysyła dane w danym topicu Subscriber jest nodem odbierającymdane z topicu Każdy kanał ma określony typ danych ktoacutere można na nim nadawaćoraz może mieć wielu nadawcoacutew (Publisher) i odbiorcoacutew (Subscriber)

OROCOS - Open RObot Control Software - jest otwartym oprogramowaniemsłużącym do sterowania systemami robotycznymi w czasie rzeczywistym Wszystkieakcje ktoacutere muszą być wykonywane w reżimie czasu rzeczywistego są uruchamianew ramach struktury OROCOSMCU - W skroacutecie mikrokontroler układ scalony zawierający w sobie rdzeń proce-sora oraz peryferia zewnętrzne W tym przypadku MCU to mikrokontrolerTM4C129ENCPDT wyposażony w rdzeń Cortex-M4F

MEMS - Od angielskiego microelectromechanical system Jest to zintegrowanyukład elektromechaniczny ktoacuterego co najmniej jeden wymiar znajduje się w skalimikro

22 Działanie jednostki inercyjnej

Jednostka inercyjna jest urządzeniem złożonym z kilku czujnikoacutew Najczęściej wszyst-kie zawarte w niej urządzenia wykonane sa w technologi MEMS Często spotykanąw ostatnich latach sytuacją jest że wszystkie te urządzenia znajdują się w jed-nym układzie scalonym Akcelerometry zawierające się w jednostkach inercyjnychwystępują praktycznie tylko w wersji MEMS a podstawy ich działania opisane sąw podsekcji 221 Żyroskopy roacutewnież w zdecydowanej większości występują jakowykonane w tej technologii elementy krzemowe ktoacuterych zasadę działania opisanow podsekcji 222 Jednak okazało się że można roacutewnież spotkać jednostki inercyjnezawierające żyroskopy światłowodowe przedstawione w podsekcji 223 Urządzeniate jednak najczęściej dedykowane są do zastosowań na dużych maszynach takich jakstatki morskie czy samoloty

221 Akcelerometry MEMS

Akcelerometry MEMSmożna dzielić na dwa typy Pierwszym z nich są akcelerometrypiezorezystancyjne [3] Akcelerometry te opierają się o dosyć prostą zasadę działania

10 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Masa odchylająca się pod wpływem siły bezwładności proacutebuje odkształcić piezorezy-story wmontowane w belkę na ktoacuterej jest zaczepiona Mierząc rezystancję wspomnia-nych piezorezystoroacutew można ustalić siłę działającą na masę oraz dzięki znajomościmasy wartość przyspieszenia ktoacuteremu poddawany jest układ Oczywiście wspoacutełcze-sne układy stosują zaawansowane techniki w celu zmniejszenia wpływu przyspieszeńw innych osiach niż badana oraz zabezpieczeniu przed uszkodzeniem wywołanymnadmiernymi przeciążeniami Jednak zasada działania pozostaje ta sama

Drugim rodzajem akcelerometroacutew MEMS są akcelerometry pojemnościowe Ak-celerometr pojemnościowy do swojego działania wymaga dwoacutech elementoacutew elek-trody stałej oraz elektrody połączonej z masą ktoacutera będzie poruszana siłą bezwład-ności Gdy masa zbliża się do elektrody stałej pojemność się zwiększa gdy natomiastsiła bezwładności działa w drugą stronę pojemność się zmniejsza Takie założeniapozwalają na pomiar przyspieszenia w akcelerometrze pojemnościowym

222 Żyroskopy MEMS

Obecnie produkowane żyroskopy MEMS to żyroskopy wibracyjne [3] Ich zasadadziałania jest nieco bardziej skomplikowana niż zasada działania akcelerometroacutewMEMS W układzie takim masę należy wprawić w wibracje wzdłuż jednej z osi (naprzykład x) leżącej na płaszczyźnie zawierającej dwie z nich (na przykład płaszczy-zna xy) W tej sytuacji pomiar możliwy będzie przy obrocie wokoacuteł trzeciej osi (z)Przy obrocie wokoacuteł osi Z czujnika siła Coriolisa przeniesie drgania z osi X na oś Ymierząc amplitudę tych drgań (na przykład za pomocą metody pojemnościowej jakw akcelerometrze pojemnościowym) można ustalić prędkość obrotu wokoacuteł danej osi

223 Żyroskopy światłowodowe

Działanie żyroskopoacutew światłowodowych oparte jest o efekt Sagnaca [12] jest toefekt polegający na przesunięciu w fazie dwuch rozszczepionych wiązek światła bie-gnących po pętli w przeciwnych kierunkach jeżeli układ się obraca Efekt ten jestspowodowany tym że w obracającym się układzie światło lecące w jedną stronę musipokonać dłuższą drogę niż światło lecące w stronę przeciwną Efekt ten stosuje sięw żyroskopach światłowodowych wielokrotnie zwiekszając drogę jaką musi pokonaćświatło poprzez zwiększenie ilości zwojoacutew światłowodu Żyroskopy takie mają bar-dzo wysoką czułość (nawet do jednej dziesięciotysięcznej stopnia na godzinę) orazbardzo niski dryft

23 Działanie magistrali Ethercat

EtherCAT jest standardem stworzonym przez firmę Beckhoff Automation Standardten jest jednak otwarty i powstają otwarte implementacje jego elementoacutew Choćsam standard swoje niższe warstwy opiera na Ethernecie (warstwę fizyczną orazczęść warstwy łącza danych) to wyższe z nich roacuteżnią się między sobą ze względuna zastosowanie na przykład protokołem warstwy aplikacji jest protokoacuteł Can OverEtherCAT opisany w podsekcji 231 W podsekcji 232 znajduje się wprowadzeniedo działania rozproszonego zegara magistrali EtherCAT

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 11

(a) Struktura ramki ethercat źroacutedło [5] (b) Struktura ramki danych źroacutedło [5]

Rysunek 21 Schematy przedstawiające strukturę ramki w komunikacji ethercat

231 COE

CANopen Over EtherCAT [5] jest protokołem warstwy aplikacji działającym w sieciEthercat jest to protokoacuteł komunikacyjny bazujący na systemie CAN Wykorzystanieimplementacji protokołu CoE sprawia że EtherCAT jest kompatybilny z urządze-niami obsługującymi CANopen Aby zrozumieć w jaki sposoacuteb datagramy CANopenprzekazywane są w protokole ethercat musimy przyjrzeć się strukturze jego ramkiTa (pokazana na rysunku 21(a)) zawiera czternastobajtowy nagłoacutewek ramki ether-cat dwubajtowe pole długości wiadomości wiadomość oraz czterobajtową sumękontrolną W polu wiadomości może znajdować się wiele datagramoacutew każdy z nichmoże być adresowany do innego urządzenia w magistrali ethercat Każdy datagramzawiera swoacutej nagłoacutewek ktoacutery adresuje go do danego urządzenia pole danych orazsumę kontrolną Skład pola danych pokazany na rysunku 21(b) a składa się onoz nagłoacutewka ktoacutery między innymi zawiera typ ktoacutery wskazuje na protokoacuteł warstwyaplikacji czyli na przykład CoE Następnie w ramce znajduje się komenda oraz od-powiadające jej dane

232 Synchronizacja zegara

Algorytm synchronizacji czasu w standardzie ethercat oparty jest o standard Pre-cision Time Protocol Jest to standard zaakceptowany przez IEEE o sygnaturzeIEEE 1588-2008 służący do submikrosekundowej synchronizacji czasu między urzą-dzeniami połączonymi w jedną sieć Więcej o działaniu samego protokołu możemyprzeczytać w publikacjach [15] [8] W protokole EtherCAT do synchronizacji zegarawyznaczany jest pierwsze urządzenie podrzędne na magistrali Ponieważ urządzeniapodrzędne magistrali EtherCAT wspierają synchronizacje sprzętową za pomocą pro-tokołu PTP możliwe jest odciążenie węzła głoacutewnego

24 Jednostki inercyjne ze złączami I2C oraz SPI

Szerokie spektrum tych urządzeń korzysta z interfejsoacutew I2C oraz SPI Taki spo-soacuteb podłączenia daje możliwość szybkiego przesyłu danych jednak ograniczeniemw takim przypadku jest odległość na jaka można prowadzić komunikację I2C niema zdefiniowanego maksymalnego zasięgu a maksymalną pojemność linii komuni-kacyjnej na poziomie 400 pF [16] co ogranicza zasięg komunikacji do pojedynczychmetroacutew w warunkach idealnych Magistrala SPI roacutewnież nie podaje maksymalnegozasięgu jednak w optymalnych warunkach jest to ok 2m

12 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Jednostki inercyjne ze złączami I2C oraz SPI są zazwyczaj tańszymi modelamiW większości są oferowane jako układy elektroniczne do montażu powierzchniowegoOto kilka przykładoacutew tego typu jednostek

bull LSM9DS0 - Jednostka inercyjna firmy ST electronics z interfejsem komunika-cyjnym SPII2C wyposażona w akcelerometr żyroskop i magnetometr Mak-symalne proacutebkowanie magnetometru to 100Hz akcelerometru 1600Hz żyro-skop ma maksymalną częstotliwość dostarczania danych 760Hz Jednostka do-starczana jest w obudowie LGA-24 o rozmiarach 4x4x1mm

bull ADIS16362 - Jednostka inercyjna firmy Analog devices jest to jednostka za-wierająca akcelerometr oraz żyroskop (oba troacutejosiowe) Charakteryzuje się onawysoką maksymalną częstotliwością proacutebkowania ponieważ dla obu czujnikoacutewwynosi ona 1200Hz Interfejs komunikacyjny stanowi złącze SPI Urządzeniedostarczane jest w obudowie w kształcie sześcianu 2x2 cm z miejscami naprzykręcenie jej do urządzenia W przeciwieństwie do poprzedniego urządze-nia jednostka posiada program kalibracji

25 Jednostki inercyjne ze złączami przemysłowymi

Inne z tych urządzeń komunikują się za pomocą dedykowanych standardoacutew prze-mysłowych ktoacutere charakteryzują się znacznie większą odpornością na zakłoacuteceniaa także na ciężkie warunki pracy Warto w tym miejscu zaznaczyć że jak dotąd narynku nie ma jednostki inercyjnej ktoacutera komunikowała by się za pomocą magistraliEthercat

Jednostki inercyjne omawiane w tym podrozdziale są urządzeniami ze złączamiprzemysłowymi przez co rozumiemy że złącza te charakteryzują się dużą nieza-wodnością Niestety modele zapewniające taki komfort są zazwyczaj znacznie więk-sze oraz droższe

bull MTi 100-series - Jednostki inercyjne firmy Xsens jest jednostką zawierającąakcelerometr i żyroskop Maksymalna częstotliwość dostarczania danych to2000Hz Jednostka ta komunikuje się za pomocą interfejsoacutew RS232 RS485422 UART USB Urządzenie to jest dostarczone w wodoodpornej obudo-wie mocowanej do urządzenia na śruby z przemysłowym złączem służącym dokomunikacji oraz zasilania Producent deklaruje że urządzenie posiada żyro-skop filtrujący wpływ wibracji

bull KVH 1750 IMU - Jednostka inercyjna firmy KVH jest urządzeniem posiada-jącym żyroskop światłowodowy oraz 3 oddzielne akcelerometry MEMS Inter-fejs komunikacyjny tego modułu to RS-422 Obudowa tej jednostki to waleco średnicy 89 mm i wysokości 74 mm przytwierdzany za pomocą sześciu śrubUrządzenie może produkować dane z częstotliwością do 1000Hz Charaktery-zuje się ono bardzo niskim dryftem żyroskopu Dryft ten typowo musi byćkorygowany za pomocą magnetometru

bull 6DF-1N2-C2-HWL - Jednostka inercyjna firmy Honeywell wyposażona jestw akcelerometr oraz żyroskop (oba troacutejosiowe) Interfejs komunikacyjny tegourządzenia to CAN Maksymalna częstotliwość dostarczania danych to 100Hz

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 13

26 Możliwe zastosowania proponowanego układu

Możliwe zastosowania jednostek inercyjnych w robotyce pojawiają się tam gdziewystępuje ruch inny od jednostajnego prostoliniowego W tym rozdziale rozważymydwa zastosowania w ktoacuterych proponowany układ może pokazać swoje zalety wy-nikające ze sposobu komunikacji W sekcji 261 omoacutewimy przypadek zastosowaniajednostki inercyjnej do wspomagania procesu nawigacji robota mobilnego o napę-dzie roacuteżnicowym Sekcja 262 dotyczyć będzie przypadku użycia takiego urządzeniado korekty odczytu siły z czujnika umieszczonego w nadgarstku manipulatora

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym

W tej sekcji przedstawiony zostanie przykład nawigacji robota mobilnego o napę-dzie roacuteżnicowym Przykładem takiego robota może być robot o konstrukcji takiejjak baza mobilna elektron [10] czyli robot posiadający wiele koacuteł po jednej stro-nie co generuje problemy z odtworzeniem trajektorii ruchu robota przez odczytyz czujnikoacutew odometrycznych Dzieje się tak przez to że budowa robota wprowa-dza poślizgi podczas skrętoacutew Robot taki ze względu na roacuteżne czynniki zewnętrznenp typ podłoża nieroacutewności podłoża skręca w okoacuteł trudnej do ustalenia osi obrotuktoacutera w dodatku może być zmienna (np gdy jedno koło jest na innym typie podłożaniż wszystkie inne) Aby temu zapobiec można użyć do estymacji trajektorii ruchurobota pomiaroacutew pochodzących z czujnika inercyjnego razem z odczytami z senso-roacutew odometrycznych Aby tego dokonać potrzebna jest fuzja odczytoacutew z roacuteżnychczujnikoacutew Istnieje wiele metod podejścia do tego tematu jednak jednym z częściejspotykanych w literaturze jest użycie rozszerzonego filtru Kalmana [18] [17] [4]

262 Korekcja odczytu siły z czujnika nadgarstkowego

Drugim przykładem zastosowania jednostki inercyjnej w robotyce jest korekcja od-czytu z czujnika siły [20] Nie jest to zastosowanie tak szeroko omawiane w literaturzejak nawigacja wspomagana inercyjnie opisana w sekcji 261 Pomimo niewielkiegozainteresowania jest to bardzo ciekawy temat badawczy głoacutewnie ze względu na chęćwyeliminowania bdquopikoacutewrdquo odczytoacutew siły wynikających z nagłej zmiany kierunku ru-chu ramienia bądź też drgnięcia wynikającego z niedoskonałości regulatora Do tegozastosowania konstruuje się obserwator siły oparty o filtr Kalmana [6]

14 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

4

Spis treści

1 Wstęp 711 Założenia i problematyka pracy 712 Układ Pracy 7

2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwezastosowania układu 921 Definicje 922 Działanie jednostki inercyjnej 10

221 Akcelerometry MEMS 10222 Żyroskopy MEMS 11223 Żyroskopy światłowodowe 11

23 Działanie magistrali Ethercat 11231 COE 12232 Synchronizacja zegara 12

24 Jednostki inercyjne ze złączami I2C oraz SPI 1225 Jednostki inercyjne ze złączami przemysłowymi 1326 Możliwe zastosowania proponowanego układu 14

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym 14262 Korekcja odczytu siły z czujnika nadgarstkowego 14

3 Opis budowanego układu 1531 Opis Układu 15

311 Moduł głoacutewny 16312 Moduł jednostki inercyjnej (IMU) 16313 Anybus M40 EtherCAT 17

32 Opis złącz 17321 Złącze międzypłytkowe 17322 Złącze zasilania 18323 Złącze interfejsu EtherCAT 19324 Złącze Jtag 20325 Złącze Anybus B40-CPU 20

4 Opis oprogramowania 2341 Protokoły komunikacyjne 23

411 Standard EtherCAT 23412 Can over EtherCAT 23

42 Oprogramowanie MCU 24421 Konfiguracja peryferioacutew 24422 Sterownik Anybus 25423 Przerwania 30

43 Sterownik w strukturze OROCOS 31

Spis treści 5

5 Badania weryfikacyjne 3551 Opis stanowiska eksperymentalnego 3552 Projekt eksperymentoacutew 3553 Wyniki eksperymentoacutew 3754 Podsumowanie 42

A Schematy modułoacutew urządzenia 51

B Wykresy pobranych trajektorii 57

6 Spis treści

Rozdział 1

Wstęp

11 Założenia i problematyka pracy

Robotyka jest bardzo szeroką i ciekawą dziedziną Z czasem systemy robotyczne wy-posażone są w coraz większą ilość czujnikoacutew ktoacutere mierzą najroacuteżniejsze parametrypracy robota jak i roacutewnież środowiska w ktoacuterym pracuje

Założeniem pracy było stworzenie układu interfejsującego jednostkę inercyjnąADIS16362 Rozwiązanie musiało zapewnić niezawodną komunikacje z kompute-rem sterującym robotem Ponieważ systemy robotyczne muszą działać w reżimieczasu rzeczywistego roacutewnież komunikacja z projektowanym układem musiała dzia-łać w ścisłym reżimie czasowym Dodatkowo ważnym czynnikiem decydującym o uży-teczności w niektoacuterych systemach robotycznych jest nie tylko okres pomiaru aleroacutewnież ustalenie momentu pomiaru z jak największą dokładnością Dlatego wymo-giem okazał się pomiar synchronizowany do czego potrzebny jest interfejs umożli-wiający synchronizację zegara we wszystkich urządzeniach magistrali Dodatkowymograniczeniem był rozmiar ponieważ układ ma nadać się do montażu w ograni-czonej przestrzeni nadgarstka robota IRP6 oraz w potencjalnych innych robotachgdzie występują ograniczenia przestrzenne Układ został więc zbudowany z mięk-kim (z możliwością niewielkiego przekroczenia) ograniczeniem rozmiaru ustalonymna 50x50x50mm

12 Układ Pracy

Niniejsza praca składa się z pięciu rozdziałoacutew Rozdział 2 zawiera podstawy teore-tyczne działania jednostki inercyjnej oraz jej elementoacutew magistrali EtherCAT orazprzegląd dostępnych na rynku jednostek inercyjnych i przykłady zastosowania jed-nostek inercyjnych w robotyce Poroacutewnywane są w nim ich parametry oraz sposobykomunikacji ich wady i zalety Opisane są tam roacutewnież przykładowe zastosowaniajednostki inercyjnej w robotyce usługowej Rozdział 3 zawiera opis mechaniczny orazelektroniczny budowanego układu opis złącz oraz wyprowadzonych na ich styki sy-gnałoacutew W rozdziale 4 znajduje się opis oprogramowania wytworzonego w ramachpracy znajdującego się zaroacutewno w wytworzonym urządzeniu jak i tego pracującegona komputerze sterującym robota Wreszcie rozdział 5 w ktoacuterym znajdziemy pro-jekt oraz wyniki eksperymentoacutew przeprowadzonych z wykorzystaniem manipulatoraIrp6 oraz wytworzonego układu

Rozdział 1 Wstęp 7

8 Rozdział 1 Wstęp

Rozdział 2

Wstęp teoretyczny przegląddostępnych rozwiązań oraz możliwezastosowania układu

Jednostka inercyjna jest urządzeniem służącym do pomiaru przyspieszeń liniowychprędkości obrotowych oraz w niektoacuterych przypadkach orientacji w polu magnetycz-nym Zawiera ono w sobie troacutejosiowe akcelerometr oraz żyroskop (dodatkowo nie-ktoacutere modele zawierają troacutejosiowy magnetometr pozwalający na wyznaczenie orien-tacji względem pola magnetycznego) Urządzenia takie roacuteżnią się pomiędzy sobąparametrami oraz sposobem połączenia z urządzeniem nadrzędnym

W tym rozdziale czytelnik zostanie wprowadzony w podstawowe zagadnienia natemat działania jednostki inercyjnej Najpierw w sekcji 21 przedstawiony zosta-nie spis definicji oraz skroacutetoacutew Zasady działania jednostki inercyjnej zostały opi-sane w sekcji 22 W sekcji 23 przedstawione zostały podstawy działania magistraliEtherCAT oraz jej mechanizmoacutew

Jednostki inercyjne dzielą się przede wszystkim ze względu na obecność magne-tometru częstotliwość pomiaru (oraz dostarczania danych wyjściowych) oraz nasposoacuteb komunikacji Przykłady urządzeń komunikujących się za pomocą interfejsoacutewI2C oraz SPI opisane są w sekcji 24 a te komunikujące się za pomocą standardoacutewprzemysłowych opisane są w sekcji 25 Na końcu wzbogacony tą wiedzą w sekcji 26czytelnik pozna przykładowe zastosowania takiego urządzenia w zadaniach robotyki

21 Definicje

IMU - z Angielskiego Inertial Measurement Unit - zespoacuteł czujnikoacutew pozwalającyna pomiar Przyspieszeń ciała oraz prędkości kontowych z jakimi się porusza Cza-sami roacutewnież zawiera on Magnetometr pozwalający na pomiar orientacji urządzeniawzględem pola magnetycznego (zazwyczaj ziemi)

ETHERCAT - Ethernet for Control Automation Technology - jest standardemkomunikacji w urządzeniach automatyki Bazuje on na architekturze Ethernet Stan-dard ten został stworzony do zastosowań czasu rzeczywistego i pozwala na ustalenieczasu pomiaru z dokładnością do kilkudziesięciu mikrosekund

PDO - Process Data Object - Paczka danych procesowych służąca do przekazy-wania stanu urządzenia Ten typ obiektoacutew pozwala na ciągły odczyt danych

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 9

SDO - Service Data Object - Są to paczki danych pozwalające na konfiguracjeurządzenia podrzędnego

FMMU - jest urządzeniem odpowiedzialnym za mapowanie obiektoacutew PDO dopamięci urządzenia podrzędnego Pośredniczy ono w każdej komunikacji za pomocąmagistrali ethercat [7]

ROS - Robot Operating System - Jest to zbioacuter bibliotek oraz oprogramowaniezapewniające komunikacje międzyprocesowa spoacutejne środowisko uruchomienioweoraz środowisko do budowy oprogramowania

Węzeł - Jest to pojedyncza instancja programu uruchomionego w ramach opro-gramowania ROS

Temat - Jest to kanał komunikacji międzyprocesowej udostępniany przez opro-gramowanie ROS Topik posiada dwa typy Nodoacutew podłączonych do niego Publisherjet to node ktoacutery wysyła dane w danym topicu Subscriber jest nodem odbierającymdane z topicu Każdy kanał ma określony typ danych ktoacutere można na nim nadawaćoraz może mieć wielu nadawcoacutew (Publisher) i odbiorcoacutew (Subscriber)

OROCOS - Open RObot Control Software - jest otwartym oprogramowaniemsłużącym do sterowania systemami robotycznymi w czasie rzeczywistym Wszystkieakcje ktoacutere muszą być wykonywane w reżimie czasu rzeczywistego są uruchamianew ramach struktury OROCOSMCU - W skroacutecie mikrokontroler układ scalony zawierający w sobie rdzeń proce-sora oraz peryferia zewnętrzne W tym przypadku MCU to mikrokontrolerTM4C129ENCPDT wyposażony w rdzeń Cortex-M4F

MEMS - Od angielskiego microelectromechanical system Jest to zintegrowanyukład elektromechaniczny ktoacuterego co najmniej jeden wymiar znajduje się w skalimikro

22 Działanie jednostki inercyjnej

Jednostka inercyjna jest urządzeniem złożonym z kilku czujnikoacutew Najczęściej wszyst-kie zawarte w niej urządzenia wykonane sa w technologi MEMS Często spotykanąw ostatnich latach sytuacją jest że wszystkie te urządzenia znajdują się w jed-nym układzie scalonym Akcelerometry zawierające się w jednostkach inercyjnychwystępują praktycznie tylko w wersji MEMS a podstawy ich działania opisane sąw podsekcji 221 Żyroskopy roacutewnież w zdecydowanej większości występują jakowykonane w tej technologii elementy krzemowe ktoacuterych zasadę działania opisanow podsekcji 222 Jednak okazało się że można roacutewnież spotkać jednostki inercyjnezawierające żyroskopy światłowodowe przedstawione w podsekcji 223 Urządzeniate jednak najczęściej dedykowane są do zastosowań na dużych maszynach takich jakstatki morskie czy samoloty

221 Akcelerometry MEMS

Akcelerometry MEMSmożna dzielić na dwa typy Pierwszym z nich są akcelerometrypiezorezystancyjne [3] Akcelerometry te opierają się o dosyć prostą zasadę działania

10 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Masa odchylająca się pod wpływem siły bezwładności proacutebuje odkształcić piezorezy-story wmontowane w belkę na ktoacuterej jest zaczepiona Mierząc rezystancję wspomnia-nych piezorezystoroacutew można ustalić siłę działającą na masę oraz dzięki znajomościmasy wartość przyspieszenia ktoacuteremu poddawany jest układ Oczywiście wspoacutełcze-sne układy stosują zaawansowane techniki w celu zmniejszenia wpływu przyspieszeńw innych osiach niż badana oraz zabezpieczeniu przed uszkodzeniem wywołanymnadmiernymi przeciążeniami Jednak zasada działania pozostaje ta sama

Drugim rodzajem akcelerometroacutew MEMS są akcelerometry pojemnościowe Ak-celerometr pojemnościowy do swojego działania wymaga dwoacutech elementoacutew elek-trody stałej oraz elektrody połączonej z masą ktoacutera będzie poruszana siłą bezwład-ności Gdy masa zbliża się do elektrody stałej pojemność się zwiększa gdy natomiastsiła bezwładności działa w drugą stronę pojemność się zmniejsza Takie założeniapozwalają na pomiar przyspieszenia w akcelerometrze pojemnościowym

222 Żyroskopy MEMS

Obecnie produkowane żyroskopy MEMS to żyroskopy wibracyjne [3] Ich zasadadziałania jest nieco bardziej skomplikowana niż zasada działania akcelerometroacutewMEMS W układzie takim masę należy wprawić w wibracje wzdłuż jednej z osi (naprzykład x) leżącej na płaszczyźnie zawierającej dwie z nich (na przykład płaszczy-zna xy) W tej sytuacji pomiar możliwy będzie przy obrocie wokoacuteł trzeciej osi (z)Przy obrocie wokoacuteł osi Z czujnika siła Coriolisa przeniesie drgania z osi X na oś Ymierząc amplitudę tych drgań (na przykład za pomocą metody pojemnościowej jakw akcelerometrze pojemnościowym) można ustalić prędkość obrotu wokoacuteł danej osi

223 Żyroskopy światłowodowe

Działanie żyroskopoacutew światłowodowych oparte jest o efekt Sagnaca [12] jest toefekt polegający na przesunięciu w fazie dwuch rozszczepionych wiązek światła bie-gnących po pętli w przeciwnych kierunkach jeżeli układ się obraca Efekt ten jestspowodowany tym że w obracającym się układzie światło lecące w jedną stronę musipokonać dłuższą drogę niż światło lecące w stronę przeciwną Efekt ten stosuje sięw żyroskopach światłowodowych wielokrotnie zwiekszając drogę jaką musi pokonaćświatło poprzez zwiększenie ilości zwojoacutew światłowodu Żyroskopy takie mają bar-dzo wysoką czułość (nawet do jednej dziesięciotysięcznej stopnia na godzinę) orazbardzo niski dryft

23 Działanie magistrali Ethercat

EtherCAT jest standardem stworzonym przez firmę Beckhoff Automation Standardten jest jednak otwarty i powstają otwarte implementacje jego elementoacutew Choćsam standard swoje niższe warstwy opiera na Ethernecie (warstwę fizyczną orazczęść warstwy łącza danych) to wyższe z nich roacuteżnią się między sobą ze względuna zastosowanie na przykład protokołem warstwy aplikacji jest protokoacuteł Can OverEtherCAT opisany w podsekcji 231 W podsekcji 232 znajduje się wprowadzeniedo działania rozproszonego zegara magistrali EtherCAT

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 11

(a) Struktura ramki ethercat źroacutedło [5] (b) Struktura ramki danych źroacutedło [5]

Rysunek 21 Schematy przedstawiające strukturę ramki w komunikacji ethercat

231 COE

CANopen Over EtherCAT [5] jest protokołem warstwy aplikacji działającym w sieciEthercat jest to protokoacuteł komunikacyjny bazujący na systemie CAN Wykorzystanieimplementacji protokołu CoE sprawia że EtherCAT jest kompatybilny z urządze-niami obsługującymi CANopen Aby zrozumieć w jaki sposoacuteb datagramy CANopenprzekazywane są w protokole ethercat musimy przyjrzeć się strukturze jego ramkiTa (pokazana na rysunku 21(a)) zawiera czternastobajtowy nagłoacutewek ramki ether-cat dwubajtowe pole długości wiadomości wiadomość oraz czterobajtową sumękontrolną W polu wiadomości może znajdować się wiele datagramoacutew każdy z nichmoże być adresowany do innego urządzenia w magistrali ethercat Każdy datagramzawiera swoacutej nagłoacutewek ktoacutery adresuje go do danego urządzenia pole danych orazsumę kontrolną Skład pola danych pokazany na rysunku 21(b) a składa się onoz nagłoacutewka ktoacutery między innymi zawiera typ ktoacutery wskazuje na protokoacuteł warstwyaplikacji czyli na przykład CoE Następnie w ramce znajduje się komenda oraz od-powiadające jej dane

232 Synchronizacja zegara

Algorytm synchronizacji czasu w standardzie ethercat oparty jest o standard Pre-cision Time Protocol Jest to standard zaakceptowany przez IEEE o sygnaturzeIEEE 1588-2008 służący do submikrosekundowej synchronizacji czasu między urzą-dzeniami połączonymi w jedną sieć Więcej o działaniu samego protokołu możemyprzeczytać w publikacjach [15] [8] W protokole EtherCAT do synchronizacji zegarawyznaczany jest pierwsze urządzenie podrzędne na magistrali Ponieważ urządzeniapodrzędne magistrali EtherCAT wspierają synchronizacje sprzętową za pomocą pro-tokołu PTP możliwe jest odciążenie węzła głoacutewnego

24 Jednostki inercyjne ze złączami I2C oraz SPI

Szerokie spektrum tych urządzeń korzysta z interfejsoacutew I2C oraz SPI Taki spo-soacuteb podłączenia daje możliwość szybkiego przesyłu danych jednak ograniczeniemw takim przypadku jest odległość na jaka można prowadzić komunikację I2C niema zdefiniowanego maksymalnego zasięgu a maksymalną pojemność linii komuni-kacyjnej na poziomie 400 pF [16] co ogranicza zasięg komunikacji do pojedynczychmetroacutew w warunkach idealnych Magistrala SPI roacutewnież nie podaje maksymalnegozasięgu jednak w optymalnych warunkach jest to ok 2m

12 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Jednostki inercyjne ze złączami I2C oraz SPI są zazwyczaj tańszymi modelamiW większości są oferowane jako układy elektroniczne do montażu powierzchniowegoOto kilka przykładoacutew tego typu jednostek

bull LSM9DS0 - Jednostka inercyjna firmy ST electronics z interfejsem komunika-cyjnym SPII2C wyposażona w akcelerometr żyroskop i magnetometr Mak-symalne proacutebkowanie magnetometru to 100Hz akcelerometru 1600Hz żyro-skop ma maksymalną częstotliwość dostarczania danych 760Hz Jednostka do-starczana jest w obudowie LGA-24 o rozmiarach 4x4x1mm

bull ADIS16362 - Jednostka inercyjna firmy Analog devices jest to jednostka za-wierająca akcelerometr oraz żyroskop (oba troacutejosiowe) Charakteryzuje się onawysoką maksymalną częstotliwością proacutebkowania ponieważ dla obu czujnikoacutewwynosi ona 1200Hz Interfejs komunikacyjny stanowi złącze SPI Urządzeniedostarczane jest w obudowie w kształcie sześcianu 2x2 cm z miejscami naprzykręcenie jej do urządzenia W przeciwieństwie do poprzedniego urządze-nia jednostka posiada program kalibracji

25 Jednostki inercyjne ze złączami przemysłowymi

Inne z tych urządzeń komunikują się za pomocą dedykowanych standardoacutew prze-mysłowych ktoacutere charakteryzują się znacznie większą odpornością na zakłoacuteceniaa także na ciężkie warunki pracy Warto w tym miejscu zaznaczyć że jak dotąd narynku nie ma jednostki inercyjnej ktoacutera komunikowała by się za pomocą magistraliEthercat

Jednostki inercyjne omawiane w tym podrozdziale są urządzeniami ze złączamiprzemysłowymi przez co rozumiemy że złącza te charakteryzują się dużą nieza-wodnością Niestety modele zapewniające taki komfort są zazwyczaj znacznie więk-sze oraz droższe

bull MTi 100-series - Jednostki inercyjne firmy Xsens jest jednostką zawierającąakcelerometr i żyroskop Maksymalna częstotliwość dostarczania danych to2000Hz Jednostka ta komunikuje się za pomocą interfejsoacutew RS232 RS485422 UART USB Urządzenie to jest dostarczone w wodoodpornej obudo-wie mocowanej do urządzenia na śruby z przemysłowym złączem służącym dokomunikacji oraz zasilania Producent deklaruje że urządzenie posiada żyro-skop filtrujący wpływ wibracji

bull KVH 1750 IMU - Jednostka inercyjna firmy KVH jest urządzeniem posiada-jącym żyroskop światłowodowy oraz 3 oddzielne akcelerometry MEMS Inter-fejs komunikacyjny tego modułu to RS-422 Obudowa tej jednostki to waleco średnicy 89 mm i wysokości 74 mm przytwierdzany za pomocą sześciu śrubUrządzenie może produkować dane z częstotliwością do 1000Hz Charaktery-zuje się ono bardzo niskim dryftem żyroskopu Dryft ten typowo musi byćkorygowany za pomocą magnetometru

bull 6DF-1N2-C2-HWL - Jednostka inercyjna firmy Honeywell wyposażona jestw akcelerometr oraz żyroskop (oba troacutejosiowe) Interfejs komunikacyjny tegourządzenia to CAN Maksymalna częstotliwość dostarczania danych to 100Hz

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 13

26 Możliwe zastosowania proponowanego układu

Możliwe zastosowania jednostek inercyjnych w robotyce pojawiają się tam gdziewystępuje ruch inny od jednostajnego prostoliniowego W tym rozdziale rozważymydwa zastosowania w ktoacuterych proponowany układ może pokazać swoje zalety wy-nikające ze sposobu komunikacji W sekcji 261 omoacutewimy przypadek zastosowaniajednostki inercyjnej do wspomagania procesu nawigacji robota mobilnego o napę-dzie roacuteżnicowym Sekcja 262 dotyczyć będzie przypadku użycia takiego urządzeniado korekty odczytu siły z czujnika umieszczonego w nadgarstku manipulatora

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym

W tej sekcji przedstawiony zostanie przykład nawigacji robota mobilnego o napę-dzie roacuteżnicowym Przykładem takiego robota może być robot o konstrukcji takiejjak baza mobilna elektron [10] czyli robot posiadający wiele koacuteł po jednej stro-nie co generuje problemy z odtworzeniem trajektorii ruchu robota przez odczytyz czujnikoacutew odometrycznych Dzieje się tak przez to że budowa robota wprowa-dza poślizgi podczas skrętoacutew Robot taki ze względu na roacuteżne czynniki zewnętrznenp typ podłoża nieroacutewności podłoża skręca w okoacuteł trudnej do ustalenia osi obrotuktoacutera w dodatku może być zmienna (np gdy jedno koło jest na innym typie podłożaniż wszystkie inne) Aby temu zapobiec można użyć do estymacji trajektorii ruchurobota pomiaroacutew pochodzących z czujnika inercyjnego razem z odczytami z senso-roacutew odometrycznych Aby tego dokonać potrzebna jest fuzja odczytoacutew z roacuteżnychczujnikoacutew Istnieje wiele metod podejścia do tego tematu jednak jednym z częściejspotykanych w literaturze jest użycie rozszerzonego filtru Kalmana [18] [17] [4]

262 Korekcja odczytu siły z czujnika nadgarstkowego

Drugim przykładem zastosowania jednostki inercyjnej w robotyce jest korekcja od-czytu z czujnika siły [20] Nie jest to zastosowanie tak szeroko omawiane w literaturzejak nawigacja wspomagana inercyjnie opisana w sekcji 261 Pomimo niewielkiegozainteresowania jest to bardzo ciekawy temat badawczy głoacutewnie ze względu na chęćwyeliminowania bdquopikoacutewrdquo odczytoacutew siły wynikających z nagłej zmiany kierunku ru-chu ramienia bądź też drgnięcia wynikającego z niedoskonałości regulatora Do tegozastosowania konstruuje się obserwator siły oparty o filtr Kalmana [6]

14 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Spis treści

1 Wstęp 711 Założenia i problematyka pracy 712 Układ Pracy 7

2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwezastosowania układu 921 Definicje 922 Działanie jednostki inercyjnej 10

221 Akcelerometry MEMS 10222 Żyroskopy MEMS 11223 Żyroskopy światłowodowe 11

23 Działanie magistrali Ethercat 11231 COE 12232 Synchronizacja zegara 12

24 Jednostki inercyjne ze złączami I2C oraz SPI 1225 Jednostki inercyjne ze złączami przemysłowymi 1326 Możliwe zastosowania proponowanego układu 14

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym 14262 Korekcja odczytu siły z czujnika nadgarstkowego 14

3 Opis budowanego układu 1531 Opis Układu 15

311 Moduł głoacutewny 16312 Moduł jednostki inercyjnej (IMU) 16313 Anybus M40 EtherCAT 17

32 Opis złącz 17321 Złącze międzypłytkowe 17322 Złącze zasilania 18323 Złącze interfejsu EtherCAT 19324 Złącze Jtag 20325 Złącze Anybus B40-CPU 20

4 Opis oprogramowania 2341 Protokoły komunikacyjne 23

411 Standard EtherCAT 23412 Can over EtherCAT 23

42 Oprogramowanie MCU 24421 Konfiguracja peryferioacutew 24422 Sterownik Anybus 25423 Przerwania 30

43 Sterownik w strukturze OROCOS 31

Spis treści 5

5 Badania weryfikacyjne 3551 Opis stanowiska eksperymentalnego 3552 Projekt eksperymentoacutew 3553 Wyniki eksperymentoacutew 3754 Podsumowanie 42

A Schematy modułoacutew urządzenia 51

B Wykresy pobranych trajektorii 57

6 Spis treści

Rozdział 1

Wstęp

11 Założenia i problematyka pracy

Robotyka jest bardzo szeroką i ciekawą dziedziną Z czasem systemy robotyczne wy-posażone są w coraz większą ilość czujnikoacutew ktoacutere mierzą najroacuteżniejsze parametrypracy robota jak i roacutewnież środowiska w ktoacuterym pracuje

Założeniem pracy było stworzenie układu interfejsującego jednostkę inercyjnąADIS16362 Rozwiązanie musiało zapewnić niezawodną komunikacje z kompute-rem sterującym robotem Ponieważ systemy robotyczne muszą działać w reżimieczasu rzeczywistego roacutewnież komunikacja z projektowanym układem musiała dzia-łać w ścisłym reżimie czasowym Dodatkowo ważnym czynnikiem decydującym o uży-teczności w niektoacuterych systemach robotycznych jest nie tylko okres pomiaru aleroacutewnież ustalenie momentu pomiaru z jak największą dokładnością Dlatego wymo-giem okazał się pomiar synchronizowany do czego potrzebny jest interfejs umożli-wiający synchronizację zegara we wszystkich urządzeniach magistrali Dodatkowymograniczeniem był rozmiar ponieważ układ ma nadać się do montażu w ograni-czonej przestrzeni nadgarstka robota IRP6 oraz w potencjalnych innych robotachgdzie występują ograniczenia przestrzenne Układ został więc zbudowany z mięk-kim (z możliwością niewielkiego przekroczenia) ograniczeniem rozmiaru ustalonymna 50x50x50mm

12 Układ Pracy

Niniejsza praca składa się z pięciu rozdziałoacutew Rozdział 2 zawiera podstawy teore-tyczne działania jednostki inercyjnej oraz jej elementoacutew magistrali EtherCAT orazprzegląd dostępnych na rynku jednostek inercyjnych i przykłady zastosowania jed-nostek inercyjnych w robotyce Poroacutewnywane są w nim ich parametry oraz sposobykomunikacji ich wady i zalety Opisane są tam roacutewnież przykładowe zastosowaniajednostki inercyjnej w robotyce usługowej Rozdział 3 zawiera opis mechaniczny orazelektroniczny budowanego układu opis złącz oraz wyprowadzonych na ich styki sy-gnałoacutew W rozdziale 4 znajduje się opis oprogramowania wytworzonego w ramachpracy znajdującego się zaroacutewno w wytworzonym urządzeniu jak i tego pracującegona komputerze sterującym robota Wreszcie rozdział 5 w ktoacuterym znajdziemy pro-jekt oraz wyniki eksperymentoacutew przeprowadzonych z wykorzystaniem manipulatoraIrp6 oraz wytworzonego układu

Rozdział 1 Wstęp 7

8 Rozdział 1 Wstęp

Rozdział 2

Wstęp teoretyczny przegląddostępnych rozwiązań oraz możliwezastosowania układu

Jednostka inercyjna jest urządzeniem służącym do pomiaru przyspieszeń liniowychprędkości obrotowych oraz w niektoacuterych przypadkach orientacji w polu magnetycz-nym Zawiera ono w sobie troacutejosiowe akcelerometr oraz żyroskop (dodatkowo nie-ktoacutere modele zawierają troacutejosiowy magnetometr pozwalający na wyznaczenie orien-tacji względem pola magnetycznego) Urządzenia takie roacuteżnią się pomiędzy sobąparametrami oraz sposobem połączenia z urządzeniem nadrzędnym

W tym rozdziale czytelnik zostanie wprowadzony w podstawowe zagadnienia natemat działania jednostki inercyjnej Najpierw w sekcji 21 przedstawiony zosta-nie spis definicji oraz skroacutetoacutew Zasady działania jednostki inercyjnej zostały opi-sane w sekcji 22 W sekcji 23 przedstawione zostały podstawy działania magistraliEtherCAT oraz jej mechanizmoacutew

Jednostki inercyjne dzielą się przede wszystkim ze względu na obecność magne-tometru częstotliwość pomiaru (oraz dostarczania danych wyjściowych) oraz nasposoacuteb komunikacji Przykłady urządzeń komunikujących się za pomocą interfejsoacutewI2C oraz SPI opisane są w sekcji 24 a te komunikujące się za pomocą standardoacutewprzemysłowych opisane są w sekcji 25 Na końcu wzbogacony tą wiedzą w sekcji 26czytelnik pozna przykładowe zastosowania takiego urządzenia w zadaniach robotyki

21 Definicje

IMU - z Angielskiego Inertial Measurement Unit - zespoacuteł czujnikoacutew pozwalającyna pomiar Przyspieszeń ciała oraz prędkości kontowych z jakimi się porusza Cza-sami roacutewnież zawiera on Magnetometr pozwalający na pomiar orientacji urządzeniawzględem pola magnetycznego (zazwyczaj ziemi)

ETHERCAT - Ethernet for Control Automation Technology - jest standardemkomunikacji w urządzeniach automatyki Bazuje on na architekturze Ethernet Stan-dard ten został stworzony do zastosowań czasu rzeczywistego i pozwala na ustalenieczasu pomiaru z dokładnością do kilkudziesięciu mikrosekund

PDO - Process Data Object - Paczka danych procesowych służąca do przekazy-wania stanu urządzenia Ten typ obiektoacutew pozwala na ciągły odczyt danych

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 9

SDO - Service Data Object - Są to paczki danych pozwalające na konfiguracjeurządzenia podrzędnego

FMMU - jest urządzeniem odpowiedzialnym za mapowanie obiektoacutew PDO dopamięci urządzenia podrzędnego Pośredniczy ono w każdej komunikacji za pomocąmagistrali ethercat [7]

ROS - Robot Operating System - Jest to zbioacuter bibliotek oraz oprogramowaniezapewniające komunikacje międzyprocesowa spoacutejne środowisko uruchomienioweoraz środowisko do budowy oprogramowania

Węzeł - Jest to pojedyncza instancja programu uruchomionego w ramach opro-gramowania ROS

Temat - Jest to kanał komunikacji międzyprocesowej udostępniany przez opro-gramowanie ROS Topik posiada dwa typy Nodoacutew podłączonych do niego Publisherjet to node ktoacutery wysyła dane w danym topicu Subscriber jest nodem odbierającymdane z topicu Każdy kanał ma określony typ danych ktoacutere można na nim nadawaćoraz może mieć wielu nadawcoacutew (Publisher) i odbiorcoacutew (Subscriber)

OROCOS - Open RObot Control Software - jest otwartym oprogramowaniemsłużącym do sterowania systemami robotycznymi w czasie rzeczywistym Wszystkieakcje ktoacutere muszą być wykonywane w reżimie czasu rzeczywistego są uruchamianew ramach struktury OROCOSMCU - W skroacutecie mikrokontroler układ scalony zawierający w sobie rdzeń proce-sora oraz peryferia zewnętrzne W tym przypadku MCU to mikrokontrolerTM4C129ENCPDT wyposażony w rdzeń Cortex-M4F

MEMS - Od angielskiego microelectromechanical system Jest to zintegrowanyukład elektromechaniczny ktoacuterego co najmniej jeden wymiar znajduje się w skalimikro

22 Działanie jednostki inercyjnej

Jednostka inercyjna jest urządzeniem złożonym z kilku czujnikoacutew Najczęściej wszyst-kie zawarte w niej urządzenia wykonane sa w technologi MEMS Często spotykanąw ostatnich latach sytuacją jest że wszystkie te urządzenia znajdują się w jed-nym układzie scalonym Akcelerometry zawierające się w jednostkach inercyjnychwystępują praktycznie tylko w wersji MEMS a podstawy ich działania opisane sąw podsekcji 221 Żyroskopy roacutewnież w zdecydowanej większości występują jakowykonane w tej technologii elementy krzemowe ktoacuterych zasadę działania opisanow podsekcji 222 Jednak okazało się że można roacutewnież spotkać jednostki inercyjnezawierające żyroskopy światłowodowe przedstawione w podsekcji 223 Urządzeniate jednak najczęściej dedykowane są do zastosowań na dużych maszynach takich jakstatki morskie czy samoloty

221 Akcelerometry MEMS

Akcelerometry MEMSmożna dzielić na dwa typy Pierwszym z nich są akcelerometrypiezorezystancyjne [3] Akcelerometry te opierają się o dosyć prostą zasadę działania

10 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Masa odchylająca się pod wpływem siły bezwładności proacutebuje odkształcić piezorezy-story wmontowane w belkę na ktoacuterej jest zaczepiona Mierząc rezystancję wspomnia-nych piezorezystoroacutew można ustalić siłę działającą na masę oraz dzięki znajomościmasy wartość przyspieszenia ktoacuteremu poddawany jest układ Oczywiście wspoacutełcze-sne układy stosują zaawansowane techniki w celu zmniejszenia wpływu przyspieszeńw innych osiach niż badana oraz zabezpieczeniu przed uszkodzeniem wywołanymnadmiernymi przeciążeniami Jednak zasada działania pozostaje ta sama

Drugim rodzajem akcelerometroacutew MEMS są akcelerometry pojemnościowe Ak-celerometr pojemnościowy do swojego działania wymaga dwoacutech elementoacutew elek-trody stałej oraz elektrody połączonej z masą ktoacutera będzie poruszana siłą bezwład-ności Gdy masa zbliża się do elektrody stałej pojemność się zwiększa gdy natomiastsiła bezwładności działa w drugą stronę pojemność się zmniejsza Takie założeniapozwalają na pomiar przyspieszenia w akcelerometrze pojemnościowym

222 Żyroskopy MEMS

Obecnie produkowane żyroskopy MEMS to żyroskopy wibracyjne [3] Ich zasadadziałania jest nieco bardziej skomplikowana niż zasada działania akcelerometroacutewMEMS W układzie takim masę należy wprawić w wibracje wzdłuż jednej z osi (naprzykład x) leżącej na płaszczyźnie zawierającej dwie z nich (na przykład płaszczy-zna xy) W tej sytuacji pomiar możliwy będzie przy obrocie wokoacuteł trzeciej osi (z)Przy obrocie wokoacuteł osi Z czujnika siła Coriolisa przeniesie drgania z osi X na oś Ymierząc amplitudę tych drgań (na przykład za pomocą metody pojemnościowej jakw akcelerometrze pojemnościowym) można ustalić prędkość obrotu wokoacuteł danej osi

223 Żyroskopy światłowodowe

Działanie żyroskopoacutew światłowodowych oparte jest o efekt Sagnaca [12] jest toefekt polegający na przesunięciu w fazie dwuch rozszczepionych wiązek światła bie-gnących po pętli w przeciwnych kierunkach jeżeli układ się obraca Efekt ten jestspowodowany tym że w obracającym się układzie światło lecące w jedną stronę musipokonać dłuższą drogę niż światło lecące w stronę przeciwną Efekt ten stosuje sięw żyroskopach światłowodowych wielokrotnie zwiekszając drogę jaką musi pokonaćświatło poprzez zwiększenie ilości zwojoacutew światłowodu Żyroskopy takie mają bar-dzo wysoką czułość (nawet do jednej dziesięciotysięcznej stopnia na godzinę) orazbardzo niski dryft

23 Działanie magistrali Ethercat

EtherCAT jest standardem stworzonym przez firmę Beckhoff Automation Standardten jest jednak otwarty i powstają otwarte implementacje jego elementoacutew Choćsam standard swoje niższe warstwy opiera na Ethernecie (warstwę fizyczną orazczęść warstwy łącza danych) to wyższe z nich roacuteżnią się między sobą ze względuna zastosowanie na przykład protokołem warstwy aplikacji jest protokoacuteł Can OverEtherCAT opisany w podsekcji 231 W podsekcji 232 znajduje się wprowadzeniedo działania rozproszonego zegara magistrali EtherCAT

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 11

(a) Struktura ramki ethercat źroacutedło [5] (b) Struktura ramki danych źroacutedło [5]

Rysunek 21 Schematy przedstawiające strukturę ramki w komunikacji ethercat

231 COE

CANopen Over EtherCAT [5] jest protokołem warstwy aplikacji działającym w sieciEthercat jest to protokoacuteł komunikacyjny bazujący na systemie CAN Wykorzystanieimplementacji protokołu CoE sprawia że EtherCAT jest kompatybilny z urządze-niami obsługującymi CANopen Aby zrozumieć w jaki sposoacuteb datagramy CANopenprzekazywane są w protokole ethercat musimy przyjrzeć się strukturze jego ramkiTa (pokazana na rysunku 21(a)) zawiera czternastobajtowy nagłoacutewek ramki ether-cat dwubajtowe pole długości wiadomości wiadomość oraz czterobajtową sumękontrolną W polu wiadomości może znajdować się wiele datagramoacutew każdy z nichmoże być adresowany do innego urządzenia w magistrali ethercat Każdy datagramzawiera swoacutej nagłoacutewek ktoacutery adresuje go do danego urządzenia pole danych orazsumę kontrolną Skład pola danych pokazany na rysunku 21(b) a składa się onoz nagłoacutewka ktoacutery między innymi zawiera typ ktoacutery wskazuje na protokoacuteł warstwyaplikacji czyli na przykład CoE Następnie w ramce znajduje się komenda oraz od-powiadające jej dane

232 Synchronizacja zegara

Algorytm synchronizacji czasu w standardzie ethercat oparty jest o standard Pre-cision Time Protocol Jest to standard zaakceptowany przez IEEE o sygnaturzeIEEE 1588-2008 służący do submikrosekundowej synchronizacji czasu między urzą-dzeniami połączonymi w jedną sieć Więcej o działaniu samego protokołu możemyprzeczytać w publikacjach [15] [8] W protokole EtherCAT do synchronizacji zegarawyznaczany jest pierwsze urządzenie podrzędne na magistrali Ponieważ urządzeniapodrzędne magistrali EtherCAT wspierają synchronizacje sprzętową za pomocą pro-tokołu PTP możliwe jest odciążenie węzła głoacutewnego

24 Jednostki inercyjne ze złączami I2C oraz SPI

Szerokie spektrum tych urządzeń korzysta z interfejsoacutew I2C oraz SPI Taki spo-soacuteb podłączenia daje możliwość szybkiego przesyłu danych jednak ograniczeniemw takim przypadku jest odległość na jaka można prowadzić komunikację I2C niema zdefiniowanego maksymalnego zasięgu a maksymalną pojemność linii komuni-kacyjnej na poziomie 400 pF [16] co ogranicza zasięg komunikacji do pojedynczychmetroacutew w warunkach idealnych Magistrala SPI roacutewnież nie podaje maksymalnegozasięgu jednak w optymalnych warunkach jest to ok 2m

12 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Jednostki inercyjne ze złączami I2C oraz SPI są zazwyczaj tańszymi modelamiW większości są oferowane jako układy elektroniczne do montażu powierzchniowegoOto kilka przykładoacutew tego typu jednostek

bull LSM9DS0 - Jednostka inercyjna firmy ST electronics z interfejsem komunika-cyjnym SPII2C wyposażona w akcelerometr żyroskop i magnetometr Mak-symalne proacutebkowanie magnetometru to 100Hz akcelerometru 1600Hz żyro-skop ma maksymalną częstotliwość dostarczania danych 760Hz Jednostka do-starczana jest w obudowie LGA-24 o rozmiarach 4x4x1mm

bull ADIS16362 - Jednostka inercyjna firmy Analog devices jest to jednostka za-wierająca akcelerometr oraz żyroskop (oba troacutejosiowe) Charakteryzuje się onawysoką maksymalną częstotliwością proacutebkowania ponieważ dla obu czujnikoacutewwynosi ona 1200Hz Interfejs komunikacyjny stanowi złącze SPI Urządzeniedostarczane jest w obudowie w kształcie sześcianu 2x2 cm z miejscami naprzykręcenie jej do urządzenia W przeciwieństwie do poprzedniego urządze-nia jednostka posiada program kalibracji

25 Jednostki inercyjne ze złączami przemysłowymi

Inne z tych urządzeń komunikują się za pomocą dedykowanych standardoacutew prze-mysłowych ktoacutere charakteryzują się znacznie większą odpornością na zakłoacuteceniaa także na ciężkie warunki pracy Warto w tym miejscu zaznaczyć że jak dotąd narynku nie ma jednostki inercyjnej ktoacutera komunikowała by się za pomocą magistraliEthercat

Jednostki inercyjne omawiane w tym podrozdziale są urządzeniami ze złączamiprzemysłowymi przez co rozumiemy że złącza te charakteryzują się dużą nieza-wodnością Niestety modele zapewniające taki komfort są zazwyczaj znacznie więk-sze oraz droższe

bull MTi 100-series - Jednostki inercyjne firmy Xsens jest jednostką zawierającąakcelerometr i żyroskop Maksymalna częstotliwość dostarczania danych to2000Hz Jednostka ta komunikuje się za pomocą interfejsoacutew RS232 RS485422 UART USB Urządzenie to jest dostarczone w wodoodpornej obudo-wie mocowanej do urządzenia na śruby z przemysłowym złączem służącym dokomunikacji oraz zasilania Producent deklaruje że urządzenie posiada żyro-skop filtrujący wpływ wibracji

bull KVH 1750 IMU - Jednostka inercyjna firmy KVH jest urządzeniem posiada-jącym żyroskop światłowodowy oraz 3 oddzielne akcelerometry MEMS Inter-fejs komunikacyjny tego modułu to RS-422 Obudowa tej jednostki to waleco średnicy 89 mm i wysokości 74 mm przytwierdzany za pomocą sześciu śrubUrządzenie może produkować dane z częstotliwością do 1000Hz Charaktery-zuje się ono bardzo niskim dryftem żyroskopu Dryft ten typowo musi byćkorygowany za pomocą magnetometru

bull 6DF-1N2-C2-HWL - Jednostka inercyjna firmy Honeywell wyposażona jestw akcelerometr oraz żyroskop (oba troacutejosiowe) Interfejs komunikacyjny tegourządzenia to CAN Maksymalna częstotliwość dostarczania danych to 100Hz

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 13

26 Możliwe zastosowania proponowanego układu

Możliwe zastosowania jednostek inercyjnych w robotyce pojawiają się tam gdziewystępuje ruch inny od jednostajnego prostoliniowego W tym rozdziale rozważymydwa zastosowania w ktoacuterych proponowany układ może pokazać swoje zalety wy-nikające ze sposobu komunikacji W sekcji 261 omoacutewimy przypadek zastosowaniajednostki inercyjnej do wspomagania procesu nawigacji robota mobilnego o napę-dzie roacuteżnicowym Sekcja 262 dotyczyć będzie przypadku użycia takiego urządzeniado korekty odczytu siły z czujnika umieszczonego w nadgarstku manipulatora

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym

W tej sekcji przedstawiony zostanie przykład nawigacji robota mobilnego o napę-dzie roacuteżnicowym Przykładem takiego robota może być robot o konstrukcji takiejjak baza mobilna elektron [10] czyli robot posiadający wiele koacuteł po jednej stro-nie co generuje problemy z odtworzeniem trajektorii ruchu robota przez odczytyz czujnikoacutew odometrycznych Dzieje się tak przez to że budowa robota wprowa-dza poślizgi podczas skrętoacutew Robot taki ze względu na roacuteżne czynniki zewnętrznenp typ podłoża nieroacutewności podłoża skręca w okoacuteł trudnej do ustalenia osi obrotuktoacutera w dodatku może być zmienna (np gdy jedno koło jest na innym typie podłożaniż wszystkie inne) Aby temu zapobiec można użyć do estymacji trajektorii ruchurobota pomiaroacutew pochodzących z czujnika inercyjnego razem z odczytami z senso-roacutew odometrycznych Aby tego dokonać potrzebna jest fuzja odczytoacutew z roacuteżnychczujnikoacutew Istnieje wiele metod podejścia do tego tematu jednak jednym z częściejspotykanych w literaturze jest użycie rozszerzonego filtru Kalmana [18] [17] [4]

262 Korekcja odczytu siły z czujnika nadgarstkowego

Drugim przykładem zastosowania jednostki inercyjnej w robotyce jest korekcja od-czytu z czujnika siły [20] Nie jest to zastosowanie tak szeroko omawiane w literaturzejak nawigacja wspomagana inercyjnie opisana w sekcji 261 Pomimo niewielkiegozainteresowania jest to bardzo ciekawy temat badawczy głoacutewnie ze względu na chęćwyeliminowania bdquopikoacutewrdquo odczytoacutew siły wynikających z nagłej zmiany kierunku ru-chu ramienia bądź też drgnięcia wynikającego z niedoskonałości regulatora Do tegozastosowania konstruuje się obserwator siły oparty o filtr Kalmana [6]

14 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

5 Badania weryfikacyjne 3551 Opis stanowiska eksperymentalnego 3552 Projekt eksperymentoacutew 3553 Wyniki eksperymentoacutew 3754 Podsumowanie 42

A Schematy modułoacutew urządzenia 51

B Wykresy pobranych trajektorii 57

6 Spis treści

Rozdział 1

Wstęp

11 Założenia i problematyka pracy

Robotyka jest bardzo szeroką i ciekawą dziedziną Z czasem systemy robotyczne wy-posażone są w coraz większą ilość czujnikoacutew ktoacutere mierzą najroacuteżniejsze parametrypracy robota jak i roacutewnież środowiska w ktoacuterym pracuje

Założeniem pracy było stworzenie układu interfejsującego jednostkę inercyjnąADIS16362 Rozwiązanie musiało zapewnić niezawodną komunikacje z kompute-rem sterującym robotem Ponieważ systemy robotyczne muszą działać w reżimieczasu rzeczywistego roacutewnież komunikacja z projektowanym układem musiała dzia-łać w ścisłym reżimie czasowym Dodatkowo ważnym czynnikiem decydującym o uży-teczności w niektoacuterych systemach robotycznych jest nie tylko okres pomiaru aleroacutewnież ustalenie momentu pomiaru z jak największą dokładnością Dlatego wymo-giem okazał się pomiar synchronizowany do czego potrzebny jest interfejs umożli-wiający synchronizację zegara we wszystkich urządzeniach magistrali Dodatkowymograniczeniem był rozmiar ponieważ układ ma nadać się do montażu w ograni-czonej przestrzeni nadgarstka robota IRP6 oraz w potencjalnych innych robotachgdzie występują ograniczenia przestrzenne Układ został więc zbudowany z mięk-kim (z możliwością niewielkiego przekroczenia) ograniczeniem rozmiaru ustalonymna 50x50x50mm

12 Układ Pracy

Niniejsza praca składa się z pięciu rozdziałoacutew Rozdział 2 zawiera podstawy teore-tyczne działania jednostki inercyjnej oraz jej elementoacutew magistrali EtherCAT orazprzegląd dostępnych na rynku jednostek inercyjnych i przykłady zastosowania jed-nostek inercyjnych w robotyce Poroacutewnywane są w nim ich parametry oraz sposobykomunikacji ich wady i zalety Opisane są tam roacutewnież przykładowe zastosowaniajednostki inercyjnej w robotyce usługowej Rozdział 3 zawiera opis mechaniczny orazelektroniczny budowanego układu opis złącz oraz wyprowadzonych na ich styki sy-gnałoacutew W rozdziale 4 znajduje się opis oprogramowania wytworzonego w ramachpracy znajdującego się zaroacutewno w wytworzonym urządzeniu jak i tego pracującegona komputerze sterującym robota Wreszcie rozdział 5 w ktoacuterym znajdziemy pro-jekt oraz wyniki eksperymentoacutew przeprowadzonych z wykorzystaniem manipulatoraIrp6 oraz wytworzonego układu

Rozdział 1 Wstęp 7

8 Rozdział 1 Wstęp

Rozdział 2

Wstęp teoretyczny przegląddostępnych rozwiązań oraz możliwezastosowania układu

Jednostka inercyjna jest urządzeniem służącym do pomiaru przyspieszeń liniowychprędkości obrotowych oraz w niektoacuterych przypadkach orientacji w polu magnetycz-nym Zawiera ono w sobie troacutejosiowe akcelerometr oraz żyroskop (dodatkowo nie-ktoacutere modele zawierają troacutejosiowy magnetometr pozwalający na wyznaczenie orien-tacji względem pola magnetycznego) Urządzenia takie roacuteżnią się pomiędzy sobąparametrami oraz sposobem połączenia z urządzeniem nadrzędnym

W tym rozdziale czytelnik zostanie wprowadzony w podstawowe zagadnienia natemat działania jednostki inercyjnej Najpierw w sekcji 21 przedstawiony zosta-nie spis definicji oraz skroacutetoacutew Zasady działania jednostki inercyjnej zostały opi-sane w sekcji 22 W sekcji 23 przedstawione zostały podstawy działania magistraliEtherCAT oraz jej mechanizmoacutew

Jednostki inercyjne dzielą się przede wszystkim ze względu na obecność magne-tometru częstotliwość pomiaru (oraz dostarczania danych wyjściowych) oraz nasposoacuteb komunikacji Przykłady urządzeń komunikujących się za pomocą interfejsoacutewI2C oraz SPI opisane są w sekcji 24 a te komunikujące się za pomocą standardoacutewprzemysłowych opisane są w sekcji 25 Na końcu wzbogacony tą wiedzą w sekcji 26czytelnik pozna przykładowe zastosowania takiego urządzenia w zadaniach robotyki

21 Definicje

IMU - z Angielskiego Inertial Measurement Unit - zespoacuteł czujnikoacutew pozwalającyna pomiar Przyspieszeń ciała oraz prędkości kontowych z jakimi się porusza Cza-sami roacutewnież zawiera on Magnetometr pozwalający na pomiar orientacji urządzeniawzględem pola magnetycznego (zazwyczaj ziemi)

ETHERCAT - Ethernet for Control Automation Technology - jest standardemkomunikacji w urządzeniach automatyki Bazuje on na architekturze Ethernet Stan-dard ten został stworzony do zastosowań czasu rzeczywistego i pozwala na ustalenieczasu pomiaru z dokładnością do kilkudziesięciu mikrosekund

PDO - Process Data Object - Paczka danych procesowych służąca do przekazy-wania stanu urządzenia Ten typ obiektoacutew pozwala na ciągły odczyt danych

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 9

SDO - Service Data Object - Są to paczki danych pozwalające na konfiguracjeurządzenia podrzędnego

FMMU - jest urządzeniem odpowiedzialnym za mapowanie obiektoacutew PDO dopamięci urządzenia podrzędnego Pośredniczy ono w każdej komunikacji za pomocąmagistrali ethercat [7]

ROS - Robot Operating System - Jest to zbioacuter bibliotek oraz oprogramowaniezapewniające komunikacje międzyprocesowa spoacutejne środowisko uruchomienioweoraz środowisko do budowy oprogramowania

Węzeł - Jest to pojedyncza instancja programu uruchomionego w ramach opro-gramowania ROS

Temat - Jest to kanał komunikacji międzyprocesowej udostępniany przez opro-gramowanie ROS Topik posiada dwa typy Nodoacutew podłączonych do niego Publisherjet to node ktoacutery wysyła dane w danym topicu Subscriber jest nodem odbierającymdane z topicu Każdy kanał ma określony typ danych ktoacutere można na nim nadawaćoraz może mieć wielu nadawcoacutew (Publisher) i odbiorcoacutew (Subscriber)

OROCOS - Open RObot Control Software - jest otwartym oprogramowaniemsłużącym do sterowania systemami robotycznymi w czasie rzeczywistym Wszystkieakcje ktoacutere muszą być wykonywane w reżimie czasu rzeczywistego są uruchamianew ramach struktury OROCOSMCU - W skroacutecie mikrokontroler układ scalony zawierający w sobie rdzeń proce-sora oraz peryferia zewnętrzne W tym przypadku MCU to mikrokontrolerTM4C129ENCPDT wyposażony w rdzeń Cortex-M4F

MEMS - Od angielskiego microelectromechanical system Jest to zintegrowanyukład elektromechaniczny ktoacuterego co najmniej jeden wymiar znajduje się w skalimikro

22 Działanie jednostki inercyjnej

Jednostka inercyjna jest urządzeniem złożonym z kilku czujnikoacutew Najczęściej wszyst-kie zawarte w niej urządzenia wykonane sa w technologi MEMS Często spotykanąw ostatnich latach sytuacją jest że wszystkie te urządzenia znajdują się w jed-nym układzie scalonym Akcelerometry zawierające się w jednostkach inercyjnychwystępują praktycznie tylko w wersji MEMS a podstawy ich działania opisane sąw podsekcji 221 Żyroskopy roacutewnież w zdecydowanej większości występują jakowykonane w tej technologii elementy krzemowe ktoacuterych zasadę działania opisanow podsekcji 222 Jednak okazało się że można roacutewnież spotkać jednostki inercyjnezawierające żyroskopy światłowodowe przedstawione w podsekcji 223 Urządzeniate jednak najczęściej dedykowane są do zastosowań na dużych maszynach takich jakstatki morskie czy samoloty

221 Akcelerometry MEMS

Akcelerometry MEMSmożna dzielić na dwa typy Pierwszym z nich są akcelerometrypiezorezystancyjne [3] Akcelerometry te opierają się o dosyć prostą zasadę działania

10 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Masa odchylająca się pod wpływem siły bezwładności proacutebuje odkształcić piezorezy-story wmontowane w belkę na ktoacuterej jest zaczepiona Mierząc rezystancję wspomnia-nych piezorezystoroacutew można ustalić siłę działającą na masę oraz dzięki znajomościmasy wartość przyspieszenia ktoacuteremu poddawany jest układ Oczywiście wspoacutełcze-sne układy stosują zaawansowane techniki w celu zmniejszenia wpływu przyspieszeńw innych osiach niż badana oraz zabezpieczeniu przed uszkodzeniem wywołanymnadmiernymi przeciążeniami Jednak zasada działania pozostaje ta sama

Drugim rodzajem akcelerometroacutew MEMS są akcelerometry pojemnościowe Ak-celerometr pojemnościowy do swojego działania wymaga dwoacutech elementoacutew elek-trody stałej oraz elektrody połączonej z masą ktoacutera będzie poruszana siłą bezwład-ności Gdy masa zbliża się do elektrody stałej pojemność się zwiększa gdy natomiastsiła bezwładności działa w drugą stronę pojemność się zmniejsza Takie założeniapozwalają na pomiar przyspieszenia w akcelerometrze pojemnościowym

222 Żyroskopy MEMS

Obecnie produkowane żyroskopy MEMS to żyroskopy wibracyjne [3] Ich zasadadziałania jest nieco bardziej skomplikowana niż zasada działania akcelerometroacutewMEMS W układzie takim masę należy wprawić w wibracje wzdłuż jednej z osi (naprzykład x) leżącej na płaszczyźnie zawierającej dwie z nich (na przykład płaszczy-zna xy) W tej sytuacji pomiar możliwy będzie przy obrocie wokoacuteł trzeciej osi (z)Przy obrocie wokoacuteł osi Z czujnika siła Coriolisa przeniesie drgania z osi X na oś Ymierząc amplitudę tych drgań (na przykład za pomocą metody pojemnościowej jakw akcelerometrze pojemnościowym) można ustalić prędkość obrotu wokoacuteł danej osi

223 Żyroskopy światłowodowe

Działanie żyroskopoacutew światłowodowych oparte jest o efekt Sagnaca [12] jest toefekt polegający na przesunięciu w fazie dwuch rozszczepionych wiązek światła bie-gnących po pętli w przeciwnych kierunkach jeżeli układ się obraca Efekt ten jestspowodowany tym że w obracającym się układzie światło lecące w jedną stronę musipokonać dłuższą drogę niż światło lecące w stronę przeciwną Efekt ten stosuje sięw żyroskopach światłowodowych wielokrotnie zwiekszając drogę jaką musi pokonaćświatło poprzez zwiększenie ilości zwojoacutew światłowodu Żyroskopy takie mają bar-dzo wysoką czułość (nawet do jednej dziesięciotysięcznej stopnia na godzinę) orazbardzo niski dryft

23 Działanie magistrali Ethercat

EtherCAT jest standardem stworzonym przez firmę Beckhoff Automation Standardten jest jednak otwarty i powstają otwarte implementacje jego elementoacutew Choćsam standard swoje niższe warstwy opiera na Ethernecie (warstwę fizyczną orazczęść warstwy łącza danych) to wyższe z nich roacuteżnią się między sobą ze względuna zastosowanie na przykład protokołem warstwy aplikacji jest protokoacuteł Can OverEtherCAT opisany w podsekcji 231 W podsekcji 232 znajduje się wprowadzeniedo działania rozproszonego zegara magistrali EtherCAT

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 11

(a) Struktura ramki ethercat źroacutedło [5] (b) Struktura ramki danych źroacutedło [5]

Rysunek 21 Schematy przedstawiające strukturę ramki w komunikacji ethercat

231 COE

CANopen Over EtherCAT [5] jest protokołem warstwy aplikacji działającym w sieciEthercat jest to protokoacuteł komunikacyjny bazujący na systemie CAN Wykorzystanieimplementacji protokołu CoE sprawia że EtherCAT jest kompatybilny z urządze-niami obsługującymi CANopen Aby zrozumieć w jaki sposoacuteb datagramy CANopenprzekazywane są w protokole ethercat musimy przyjrzeć się strukturze jego ramkiTa (pokazana na rysunku 21(a)) zawiera czternastobajtowy nagłoacutewek ramki ether-cat dwubajtowe pole długości wiadomości wiadomość oraz czterobajtową sumękontrolną W polu wiadomości może znajdować się wiele datagramoacutew każdy z nichmoże być adresowany do innego urządzenia w magistrali ethercat Każdy datagramzawiera swoacutej nagłoacutewek ktoacutery adresuje go do danego urządzenia pole danych orazsumę kontrolną Skład pola danych pokazany na rysunku 21(b) a składa się onoz nagłoacutewka ktoacutery między innymi zawiera typ ktoacutery wskazuje na protokoacuteł warstwyaplikacji czyli na przykład CoE Następnie w ramce znajduje się komenda oraz od-powiadające jej dane

232 Synchronizacja zegara

Algorytm synchronizacji czasu w standardzie ethercat oparty jest o standard Pre-cision Time Protocol Jest to standard zaakceptowany przez IEEE o sygnaturzeIEEE 1588-2008 służący do submikrosekundowej synchronizacji czasu między urzą-dzeniami połączonymi w jedną sieć Więcej o działaniu samego protokołu możemyprzeczytać w publikacjach [15] [8] W protokole EtherCAT do synchronizacji zegarawyznaczany jest pierwsze urządzenie podrzędne na magistrali Ponieważ urządzeniapodrzędne magistrali EtherCAT wspierają synchronizacje sprzętową za pomocą pro-tokołu PTP możliwe jest odciążenie węzła głoacutewnego

24 Jednostki inercyjne ze złączami I2C oraz SPI

Szerokie spektrum tych urządzeń korzysta z interfejsoacutew I2C oraz SPI Taki spo-soacuteb podłączenia daje możliwość szybkiego przesyłu danych jednak ograniczeniemw takim przypadku jest odległość na jaka można prowadzić komunikację I2C niema zdefiniowanego maksymalnego zasięgu a maksymalną pojemność linii komuni-kacyjnej na poziomie 400 pF [16] co ogranicza zasięg komunikacji do pojedynczychmetroacutew w warunkach idealnych Magistrala SPI roacutewnież nie podaje maksymalnegozasięgu jednak w optymalnych warunkach jest to ok 2m

12 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Jednostki inercyjne ze złączami I2C oraz SPI są zazwyczaj tańszymi modelamiW większości są oferowane jako układy elektroniczne do montażu powierzchniowegoOto kilka przykładoacutew tego typu jednostek

bull LSM9DS0 - Jednostka inercyjna firmy ST electronics z interfejsem komunika-cyjnym SPII2C wyposażona w akcelerometr żyroskop i magnetometr Mak-symalne proacutebkowanie magnetometru to 100Hz akcelerometru 1600Hz żyro-skop ma maksymalną częstotliwość dostarczania danych 760Hz Jednostka do-starczana jest w obudowie LGA-24 o rozmiarach 4x4x1mm

bull ADIS16362 - Jednostka inercyjna firmy Analog devices jest to jednostka za-wierająca akcelerometr oraz żyroskop (oba troacutejosiowe) Charakteryzuje się onawysoką maksymalną częstotliwością proacutebkowania ponieważ dla obu czujnikoacutewwynosi ona 1200Hz Interfejs komunikacyjny stanowi złącze SPI Urządzeniedostarczane jest w obudowie w kształcie sześcianu 2x2 cm z miejscami naprzykręcenie jej do urządzenia W przeciwieństwie do poprzedniego urządze-nia jednostka posiada program kalibracji

25 Jednostki inercyjne ze złączami przemysłowymi

Inne z tych urządzeń komunikują się za pomocą dedykowanych standardoacutew prze-mysłowych ktoacutere charakteryzują się znacznie większą odpornością na zakłoacuteceniaa także na ciężkie warunki pracy Warto w tym miejscu zaznaczyć że jak dotąd narynku nie ma jednostki inercyjnej ktoacutera komunikowała by się za pomocą magistraliEthercat

Jednostki inercyjne omawiane w tym podrozdziale są urządzeniami ze złączamiprzemysłowymi przez co rozumiemy że złącza te charakteryzują się dużą nieza-wodnością Niestety modele zapewniające taki komfort są zazwyczaj znacznie więk-sze oraz droższe

bull MTi 100-series - Jednostki inercyjne firmy Xsens jest jednostką zawierającąakcelerometr i żyroskop Maksymalna częstotliwość dostarczania danych to2000Hz Jednostka ta komunikuje się za pomocą interfejsoacutew RS232 RS485422 UART USB Urządzenie to jest dostarczone w wodoodpornej obudo-wie mocowanej do urządzenia na śruby z przemysłowym złączem służącym dokomunikacji oraz zasilania Producent deklaruje że urządzenie posiada żyro-skop filtrujący wpływ wibracji

bull KVH 1750 IMU - Jednostka inercyjna firmy KVH jest urządzeniem posiada-jącym żyroskop światłowodowy oraz 3 oddzielne akcelerometry MEMS Inter-fejs komunikacyjny tego modułu to RS-422 Obudowa tej jednostki to waleco średnicy 89 mm i wysokości 74 mm przytwierdzany za pomocą sześciu śrubUrządzenie może produkować dane z częstotliwością do 1000Hz Charaktery-zuje się ono bardzo niskim dryftem żyroskopu Dryft ten typowo musi byćkorygowany za pomocą magnetometru

bull 6DF-1N2-C2-HWL - Jednostka inercyjna firmy Honeywell wyposażona jestw akcelerometr oraz żyroskop (oba troacutejosiowe) Interfejs komunikacyjny tegourządzenia to CAN Maksymalna częstotliwość dostarczania danych to 100Hz

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 13

26 Możliwe zastosowania proponowanego układu

Możliwe zastosowania jednostek inercyjnych w robotyce pojawiają się tam gdziewystępuje ruch inny od jednostajnego prostoliniowego W tym rozdziale rozważymydwa zastosowania w ktoacuterych proponowany układ może pokazać swoje zalety wy-nikające ze sposobu komunikacji W sekcji 261 omoacutewimy przypadek zastosowaniajednostki inercyjnej do wspomagania procesu nawigacji robota mobilnego o napę-dzie roacuteżnicowym Sekcja 262 dotyczyć będzie przypadku użycia takiego urządzeniado korekty odczytu siły z czujnika umieszczonego w nadgarstku manipulatora

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym

W tej sekcji przedstawiony zostanie przykład nawigacji robota mobilnego o napę-dzie roacuteżnicowym Przykładem takiego robota może być robot o konstrukcji takiejjak baza mobilna elektron [10] czyli robot posiadający wiele koacuteł po jednej stro-nie co generuje problemy z odtworzeniem trajektorii ruchu robota przez odczytyz czujnikoacutew odometrycznych Dzieje się tak przez to że budowa robota wprowa-dza poślizgi podczas skrętoacutew Robot taki ze względu na roacuteżne czynniki zewnętrznenp typ podłoża nieroacutewności podłoża skręca w okoacuteł trudnej do ustalenia osi obrotuktoacutera w dodatku może być zmienna (np gdy jedno koło jest na innym typie podłożaniż wszystkie inne) Aby temu zapobiec można użyć do estymacji trajektorii ruchurobota pomiaroacutew pochodzących z czujnika inercyjnego razem z odczytami z senso-roacutew odometrycznych Aby tego dokonać potrzebna jest fuzja odczytoacutew z roacuteżnychczujnikoacutew Istnieje wiele metod podejścia do tego tematu jednak jednym z częściejspotykanych w literaturze jest użycie rozszerzonego filtru Kalmana [18] [17] [4]

262 Korekcja odczytu siły z czujnika nadgarstkowego

Drugim przykładem zastosowania jednostki inercyjnej w robotyce jest korekcja od-czytu z czujnika siły [20] Nie jest to zastosowanie tak szeroko omawiane w literaturzejak nawigacja wspomagana inercyjnie opisana w sekcji 261 Pomimo niewielkiegozainteresowania jest to bardzo ciekawy temat badawczy głoacutewnie ze względu na chęćwyeliminowania bdquopikoacutewrdquo odczytoacutew siły wynikających z nagłej zmiany kierunku ru-chu ramienia bądź też drgnięcia wynikającego z niedoskonałości regulatora Do tegozastosowania konstruuje się obserwator siły oparty o filtr Kalmana [6]

14 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Rozdział 1

Wstęp

11 Założenia i problematyka pracy

Robotyka jest bardzo szeroką i ciekawą dziedziną Z czasem systemy robotyczne wy-posażone są w coraz większą ilość czujnikoacutew ktoacutere mierzą najroacuteżniejsze parametrypracy robota jak i roacutewnież środowiska w ktoacuterym pracuje

Założeniem pracy było stworzenie układu interfejsującego jednostkę inercyjnąADIS16362 Rozwiązanie musiało zapewnić niezawodną komunikacje z kompute-rem sterującym robotem Ponieważ systemy robotyczne muszą działać w reżimieczasu rzeczywistego roacutewnież komunikacja z projektowanym układem musiała dzia-łać w ścisłym reżimie czasowym Dodatkowo ważnym czynnikiem decydującym o uży-teczności w niektoacuterych systemach robotycznych jest nie tylko okres pomiaru aleroacutewnież ustalenie momentu pomiaru z jak największą dokładnością Dlatego wymo-giem okazał się pomiar synchronizowany do czego potrzebny jest interfejs umożli-wiający synchronizację zegara we wszystkich urządzeniach magistrali Dodatkowymograniczeniem był rozmiar ponieważ układ ma nadać się do montażu w ograni-czonej przestrzeni nadgarstka robota IRP6 oraz w potencjalnych innych robotachgdzie występują ograniczenia przestrzenne Układ został więc zbudowany z mięk-kim (z możliwością niewielkiego przekroczenia) ograniczeniem rozmiaru ustalonymna 50x50x50mm

12 Układ Pracy

Niniejsza praca składa się z pięciu rozdziałoacutew Rozdział 2 zawiera podstawy teore-tyczne działania jednostki inercyjnej oraz jej elementoacutew magistrali EtherCAT orazprzegląd dostępnych na rynku jednostek inercyjnych i przykłady zastosowania jed-nostek inercyjnych w robotyce Poroacutewnywane są w nim ich parametry oraz sposobykomunikacji ich wady i zalety Opisane są tam roacutewnież przykładowe zastosowaniajednostki inercyjnej w robotyce usługowej Rozdział 3 zawiera opis mechaniczny orazelektroniczny budowanego układu opis złącz oraz wyprowadzonych na ich styki sy-gnałoacutew W rozdziale 4 znajduje się opis oprogramowania wytworzonego w ramachpracy znajdującego się zaroacutewno w wytworzonym urządzeniu jak i tego pracującegona komputerze sterującym robota Wreszcie rozdział 5 w ktoacuterym znajdziemy pro-jekt oraz wyniki eksperymentoacutew przeprowadzonych z wykorzystaniem manipulatoraIrp6 oraz wytworzonego układu

Rozdział 1 Wstęp 7

8 Rozdział 1 Wstęp

Rozdział 2

Wstęp teoretyczny przegląddostępnych rozwiązań oraz możliwezastosowania układu

Jednostka inercyjna jest urządzeniem służącym do pomiaru przyspieszeń liniowychprędkości obrotowych oraz w niektoacuterych przypadkach orientacji w polu magnetycz-nym Zawiera ono w sobie troacutejosiowe akcelerometr oraz żyroskop (dodatkowo nie-ktoacutere modele zawierają troacutejosiowy magnetometr pozwalający na wyznaczenie orien-tacji względem pola magnetycznego) Urządzenia takie roacuteżnią się pomiędzy sobąparametrami oraz sposobem połączenia z urządzeniem nadrzędnym

W tym rozdziale czytelnik zostanie wprowadzony w podstawowe zagadnienia natemat działania jednostki inercyjnej Najpierw w sekcji 21 przedstawiony zosta-nie spis definicji oraz skroacutetoacutew Zasady działania jednostki inercyjnej zostały opi-sane w sekcji 22 W sekcji 23 przedstawione zostały podstawy działania magistraliEtherCAT oraz jej mechanizmoacutew

Jednostki inercyjne dzielą się przede wszystkim ze względu na obecność magne-tometru częstotliwość pomiaru (oraz dostarczania danych wyjściowych) oraz nasposoacuteb komunikacji Przykłady urządzeń komunikujących się za pomocą interfejsoacutewI2C oraz SPI opisane są w sekcji 24 a te komunikujące się za pomocą standardoacutewprzemysłowych opisane są w sekcji 25 Na końcu wzbogacony tą wiedzą w sekcji 26czytelnik pozna przykładowe zastosowania takiego urządzenia w zadaniach robotyki

21 Definicje

IMU - z Angielskiego Inertial Measurement Unit - zespoacuteł czujnikoacutew pozwalającyna pomiar Przyspieszeń ciała oraz prędkości kontowych z jakimi się porusza Cza-sami roacutewnież zawiera on Magnetometr pozwalający na pomiar orientacji urządzeniawzględem pola magnetycznego (zazwyczaj ziemi)

ETHERCAT - Ethernet for Control Automation Technology - jest standardemkomunikacji w urządzeniach automatyki Bazuje on na architekturze Ethernet Stan-dard ten został stworzony do zastosowań czasu rzeczywistego i pozwala na ustalenieczasu pomiaru z dokładnością do kilkudziesięciu mikrosekund

PDO - Process Data Object - Paczka danych procesowych służąca do przekazy-wania stanu urządzenia Ten typ obiektoacutew pozwala na ciągły odczyt danych

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 9

SDO - Service Data Object - Są to paczki danych pozwalające na konfiguracjeurządzenia podrzędnego

FMMU - jest urządzeniem odpowiedzialnym za mapowanie obiektoacutew PDO dopamięci urządzenia podrzędnego Pośredniczy ono w każdej komunikacji za pomocąmagistrali ethercat [7]

ROS - Robot Operating System - Jest to zbioacuter bibliotek oraz oprogramowaniezapewniające komunikacje międzyprocesowa spoacutejne środowisko uruchomienioweoraz środowisko do budowy oprogramowania

Węzeł - Jest to pojedyncza instancja programu uruchomionego w ramach opro-gramowania ROS

Temat - Jest to kanał komunikacji międzyprocesowej udostępniany przez opro-gramowanie ROS Topik posiada dwa typy Nodoacutew podłączonych do niego Publisherjet to node ktoacutery wysyła dane w danym topicu Subscriber jest nodem odbierającymdane z topicu Każdy kanał ma określony typ danych ktoacutere można na nim nadawaćoraz może mieć wielu nadawcoacutew (Publisher) i odbiorcoacutew (Subscriber)

OROCOS - Open RObot Control Software - jest otwartym oprogramowaniemsłużącym do sterowania systemami robotycznymi w czasie rzeczywistym Wszystkieakcje ktoacutere muszą być wykonywane w reżimie czasu rzeczywistego są uruchamianew ramach struktury OROCOSMCU - W skroacutecie mikrokontroler układ scalony zawierający w sobie rdzeń proce-sora oraz peryferia zewnętrzne W tym przypadku MCU to mikrokontrolerTM4C129ENCPDT wyposażony w rdzeń Cortex-M4F

MEMS - Od angielskiego microelectromechanical system Jest to zintegrowanyukład elektromechaniczny ktoacuterego co najmniej jeden wymiar znajduje się w skalimikro

22 Działanie jednostki inercyjnej

Jednostka inercyjna jest urządzeniem złożonym z kilku czujnikoacutew Najczęściej wszyst-kie zawarte w niej urządzenia wykonane sa w technologi MEMS Często spotykanąw ostatnich latach sytuacją jest że wszystkie te urządzenia znajdują się w jed-nym układzie scalonym Akcelerometry zawierające się w jednostkach inercyjnychwystępują praktycznie tylko w wersji MEMS a podstawy ich działania opisane sąw podsekcji 221 Żyroskopy roacutewnież w zdecydowanej większości występują jakowykonane w tej technologii elementy krzemowe ktoacuterych zasadę działania opisanow podsekcji 222 Jednak okazało się że można roacutewnież spotkać jednostki inercyjnezawierające żyroskopy światłowodowe przedstawione w podsekcji 223 Urządzeniate jednak najczęściej dedykowane są do zastosowań na dużych maszynach takich jakstatki morskie czy samoloty

221 Akcelerometry MEMS

Akcelerometry MEMSmożna dzielić na dwa typy Pierwszym z nich są akcelerometrypiezorezystancyjne [3] Akcelerometry te opierają się o dosyć prostą zasadę działania

10 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Masa odchylająca się pod wpływem siły bezwładności proacutebuje odkształcić piezorezy-story wmontowane w belkę na ktoacuterej jest zaczepiona Mierząc rezystancję wspomnia-nych piezorezystoroacutew można ustalić siłę działającą na masę oraz dzięki znajomościmasy wartość przyspieszenia ktoacuteremu poddawany jest układ Oczywiście wspoacutełcze-sne układy stosują zaawansowane techniki w celu zmniejszenia wpływu przyspieszeńw innych osiach niż badana oraz zabezpieczeniu przed uszkodzeniem wywołanymnadmiernymi przeciążeniami Jednak zasada działania pozostaje ta sama

Drugim rodzajem akcelerometroacutew MEMS są akcelerometry pojemnościowe Ak-celerometr pojemnościowy do swojego działania wymaga dwoacutech elementoacutew elek-trody stałej oraz elektrody połączonej z masą ktoacutera będzie poruszana siłą bezwład-ności Gdy masa zbliża się do elektrody stałej pojemność się zwiększa gdy natomiastsiła bezwładności działa w drugą stronę pojemność się zmniejsza Takie założeniapozwalają na pomiar przyspieszenia w akcelerometrze pojemnościowym

222 Żyroskopy MEMS

Obecnie produkowane żyroskopy MEMS to żyroskopy wibracyjne [3] Ich zasadadziałania jest nieco bardziej skomplikowana niż zasada działania akcelerometroacutewMEMS W układzie takim masę należy wprawić w wibracje wzdłuż jednej z osi (naprzykład x) leżącej na płaszczyźnie zawierającej dwie z nich (na przykład płaszczy-zna xy) W tej sytuacji pomiar możliwy będzie przy obrocie wokoacuteł trzeciej osi (z)Przy obrocie wokoacuteł osi Z czujnika siła Coriolisa przeniesie drgania z osi X na oś Ymierząc amplitudę tych drgań (na przykład za pomocą metody pojemnościowej jakw akcelerometrze pojemnościowym) można ustalić prędkość obrotu wokoacuteł danej osi

223 Żyroskopy światłowodowe

Działanie żyroskopoacutew światłowodowych oparte jest o efekt Sagnaca [12] jest toefekt polegający na przesunięciu w fazie dwuch rozszczepionych wiązek światła bie-gnących po pętli w przeciwnych kierunkach jeżeli układ się obraca Efekt ten jestspowodowany tym że w obracającym się układzie światło lecące w jedną stronę musipokonać dłuższą drogę niż światło lecące w stronę przeciwną Efekt ten stosuje sięw żyroskopach światłowodowych wielokrotnie zwiekszając drogę jaką musi pokonaćświatło poprzez zwiększenie ilości zwojoacutew światłowodu Żyroskopy takie mają bar-dzo wysoką czułość (nawet do jednej dziesięciotysięcznej stopnia na godzinę) orazbardzo niski dryft

23 Działanie magistrali Ethercat

EtherCAT jest standardem stworzonym przez firmę Beckhoff Automation Standardten jest jednak otwarty i powstają otwarte implementacje jego elementoacutew Choćsam standard swoje niższe warstwy opiera na Ethernecie (warstwę fizyczną orazczęść warstwy łącza danych) to wyższe z nich roacuteżnią się między sobą ze względuna zastosowanie na przykład protokołem warstwy aplikacji jest protokoacuteł Can OverEtherCAT opisany w podsekcji 231 W podsekcji 232 znajduje się wprowadzeniedo działania rozproszonego zegara magistrali EtherCAT

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 11

(a) Struktura ramki ethercat źroacutedło [5] (b) Struktura ramki danych źroacutedło [5]

Rysunek 21 Schematy przedstawiające strukturę ramki w komunikacji ethercat

231 COE

CANopen Over EtherCAT [5] jest protokołem warstwy aplikacji działającym w sieciEthercat jest to protokoacuteł komunikacyjny bazujący na systemie CAN Wykorzystanieimplementacji protokołu CoE sprawia że EtherCAT jest kompatybilny z urządze-niami obsługującymi CANopen Aby zrozumieć w jaki sposoacuteb datagramy CANopenprzekazywane są w protokole ethercat musimy przyjrzeć się strukturze jego ramkiTa (pokazana na rysunku 21(a)) zawiera czternastobajtowy nagłoacutewek ramki ether-cat dwubajtowe pole długości wiadomości wiadomość oraz czterobajtową sumękontrolną W polu wiadomości może znajdować się wiele datagramoacutew każdy z nichmoże być adresowany do innego urządzenia w magistrali ethercat Każdy datagramzawiera swoacutej nagłoacutewek ktoacutery adresuje go do danego urządzenia pole danych orazsumę kontrolną Skład pola danych pokazany na rysunku 21(b) a składa się onoz nagłoacutewka ktoacutery między innymi zawiera typ ktoacutery wskazuje na protokoacuteł warstwyaplikacji czyli na przykład CoE Następnie w ramce znajduje się komenda oraz od-powiadające jej dane

232 Synchronizacja zegara

Algorytm synchronizacji czasu w standardzie ethercat oparty jest o standard Pre-cision Time Protocol Jest to standard zaakceptowany przez IEEE o sygnaturzeIEEE 1588-2008 służący do submikrosekundowej synchronizacji czasu między urzą-dzeniami połączonymi w jedną sieć Więcej o działaniu samego protokołu możemyprzeczytać w publikacjach [15] [8] W protokole EtherCAT do synchronizacji zegarawyznaczany jest pierwsze urządzenie podrzędne na magistrali Ponieważ urządzeniapodrzędne magistrali EtherCAT wspierają synchronizacje sprzętową za pomocą pro-tokołu PTP możliwe jest odciążenie węzła głoacutewnego

24 Jednostki inercyjne ze złączami I2C oraz SPI

Szerokie spektrum tych urządzeń korzysta z interfejsoacutew I2C oraz SPI Taki spo-soacuteb podłączenia daje możliwość szybkiego przesyłu danych jednak ograniczeniemw takim przypadku jest odległość na jaka można prowadzić komunikację I2C niema zdefiniowanego maksymalnego zasięgu a maksymalną pojemność linii komuni-kacyjnej na poziomie 400 pF [16] co ogranicza zasięg komunikacji do pojedynczychmetroacutew w warunkach idealnych Magistrala SPI roacutewnież nie podaje maksymalnegozasięgu jednak w optymalnych warunkach jest to ok 2m

12 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Jednostki inercyjne ze złączami I2C oraz SPI są zazwyczaj tańszymi modelamiW większości są oferowane jako układy elektroniczne do montażu powierzchniowegoOto kilka przykładoacutew tego typu jednostek

bull LSM9DS0 - Jednostka inercyjna firmy ST electronics z interfejsem komunika-cyjnym SPII2C wyposażona w akcelerometr żyroskop i magnetometr Mak-symalne proacutebkowanie magnetometru to 100Hz akcelerometru 1600Hz żyro-skop ma maksymalną częstotliwość dostarczania danych 760Hz Jednostka do-starczana jest w obudowie LGA-24 o rozmiarach 4x4x1mm

bull ADIS16362 - Jednostka inercyjna firmy Analog devices jest to jednostka za-wierająca akcelerometr oraz żyroskop (oba troacutejosiowe) Charakteryzuje się onawysoką maksymalną częstotliwością proacutebkowania ponieważ dla obu czujnikoacutewwynosi ona 1200Hz Interfejs komunikacyjny stanowi złącze SPI Urządzeniedostarczane jest w obudowie w kształcie sześcianu 2x2 cm z miejscami naprzykręcenie jej do urządzenia W przeciwieństwie do poprzedniego urządze-nia jednostka posiada program kalibracji

25 Jednostki inercyjne ze złączami przemysłowymi

Inne z tych urządzeń komunikują się za pomocą dedykowanych standardoacutew prze-mysłowych ktoacutere charakteryzują się znacznie większą odpornością na zakłoacuteceniaa także na ciężkie warunki pracy Warto w tym miejscu zaznaczyć że jak dotąd narynku nie ma jednostki inercyjnej ktoacutera komunikowała by się za pomocą magistraliEthercat

Jednostki inercyjne omawiane w tym podrozdziale są urządzeniami ze złączamiprzemysłowymi przez co rozumiemy że złącza te charakteryzują się dużą nieza-wodnością Niestety modele zapewniające taki komfort są zazwyczaj znacznie więk-sze oraz droższe

bull MTi 100-series - Jednostki inercyjne firmy Xsens jest jednostką zawierającąakcelerometr i żyroskop Maksymalna częstotliwość dostarczania danych to2000Hz Jednostka ta komunikuje się za pomocą interfejsoacutew RS232 RS485422 UART USB Urządzenie to jest dostarczone w wodoodpornej obudo-wie mocowanej do urządzenia na śruby z przemysłowym złączem służącym dokomunikacji oraz zasilania Producent deklaruje że urządzenie posiada żyro-skop filtrujący wpływ wibracji

bull KVH 1750 IMU - Jednostka inercyjna firmy KVH jest urządzeniem posiada-jącym żyroskop światłowodowy oraz 3 oddzielne akcelerometry MEMS Inter-fejs komunikacyjny tego modułu to RS-422 Obudowa tej jednostki to waleco średnicy 89 mm i wysokości 74 mm przytwierdzany za pomocą sześciu śrubUrządzenie może produkować dane z częstotliwością do 1000Hz Charaktery-zuje się ono bardzo niskim dryftem żyroskopu Dryft ten typowo musi byćkorygowany za pomocą magnetometru

bull 6DF-1N2-C2-HWL - Jednostka inercyjna firmy Honeywell wyposażona jestw akcelerometr oraz żyroskop (oba troacutejosiowe) Interfejs komunikacyjny tegourządzenia to CAN Maksymalna częstotliwość dostarczania danych to 100Hz

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 13

26 Możliwe zastosowania proponowanego układu

Możliwe zastosowania jednostek inercyjnych w robotyce pojawiają się tam gdziewystępuje ruch inny od jednostajnego prostoliniowego W tym rozdziale rozważymydwa zastosowania w ktoacuterych proponowany układ może pokazać swoje zalety wy-nikające ze sposobu komunikacji W sekcji 261 omoacutewimy przypadek zastosowaniajednostki inercyjnej do wspomagania procesu nawigacji robota mobilnego o napę-dzie roacuteżnicowym Sekcja 262 dotyczyć będzie przypadku użycia takiego urządzeniado korekty odczytu siły z czujnika umieszczonego w nadgarstku manipulatora

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym

W tej sekcji przedstawiony zostanie przykład nawigacji robota mobilnego o napę-dzie roacuteżnicowym Przykładem takiego robota może być robot o konstrukcji takiejjak baza mobilna elektron [10] czyli robot posiadający wiele koacuteł po jednej stro-nie co generuje problemy z odtworzeniem trajektorii ruchu robota przez odczytyz czujnikoacutew odometrycznych Dzieje się tak przez to że budowa robota wprowa-dza poślizgi podczas skrętoacutew Robot taki ze względu na roacuteżne czynniki zewnętrznenp typ podłoża nieroacutewności podłoża skręca w okoacuteł trudnej do ustalenia osi obrotuktoacutera w dodatku może być zmienna (np gdy jedno koło jest na innym typie podłożaniż wszystkie inne) Aby temu zapobiec można użyć do estymacji trajektorii ruchurobota pomiaroacutew pochodzących z czujnika inercyjnego razem z odczytami z senso-roacutew odometrycznych Aby tego dokonać potrzebna jest fuzja odczytoacutew z roacuteżnychczujnikoacutew Istnieje wiele metod podejścia do tego tematu jednak jednym z częściejspotykanych w literaturze jest użycie rozszerzonego filtru Kalmana [18] [17] [4]

262 Korekcja odczytu siły z czujnika nadgarstkowego

Drugim przykładem zastosowania jednostki inercyjnej w robotyce jest korekcja od-czytu z czujnika siły [20] Nie jest to zastosowanie tak szeroko omawiane w literaturzejak nawigacja wspomagana inercyjnie opisana w sekcji 261 Pomimo niewielkiegozainteresowania jest to bardzo ciekawy temat badawczy głoacutewnie ze względu na chęćwyeliminowania bdquopikoacutewrdquo odczytoacutew siły wynikających z nagłej zmiany kierunku ru-chu ramienia bądź też drgnięcia wynikającego z niedoskonałości regulatora Do tegozastosowania konstruuje się obserwator siły oparty o filtr Kalmana [6]

14 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

8 Rozdział 1 Wstęp

Rozdział 2

Wstęp teoretyczny przegląddostępnych rozwiązań oraz możliwezastosowania układu

Jednostka inercyjna jest urządzeniem służącym do pomiaru przyspieszeń liniowychprędkości obrotowych oraz w niektoacuterych przypadkach orientacji w polu magnetycz-nym Zawiera ono w sobie troacutejosiowe akcelerometr oraz żyroskop (dodatkowo nie-ktoacutere modele zawierają troacutejosiowy magnetometr pozwalający na wyznaczenie orien-tacji względem pola magnetycznego) Urządzenia takie roacuteżnią się pomiędzy sobąparametrami oraz sposobem połączenia z urządzeniem nadrzędnym

W tym rozdziale czytelnik zostanie wprowadzony w podstawowe zagadnienia natemat działania jednostki inercyjnej Najpierw w sekcji 21 przedstawiony zosta-nie spis definicji oraz skroacutetoacutew Zasady działania jednostki inercyjnej zostały opi-sane w sekcji 22 W sekcji 23 przedstawione zostały podstawy działania magistraliEtherCAT oraz jej mechanizmoacutew

Jednostki inercyjne dzielą się przede wszystkim ze względu na obecność magne-tometru częstotliwość pomiaru (oraz dostarczania danych wyjściowych) oraz nasposoacuteb komunikacji Przykłady urządzeń komunikujących się za pomocą interfejsoacutewI2C oraz SPI opisane są w sekcji 24 a te komunikujące się za pomocą standardoacutewprzemysłowych opisane są w sekcji 25 Na końcu wzbogacony tą wiedzą w sekcji 26czytelnik pozna przykładowe zastosowania takiego urządzenia w zadaniach robotyki

21 Definicje

IMU - z Angielskiego Inertial Measurement Unit - zespoacuteł czujnikoacutew pozwalającyna pomiar Przyspieszeń ciała oraz prędkości kontowych z jakimi się porusza Cza-sami roacutewnież zawiera on Magnetometr pozwalający na pomiar orientacji urządzeniawzględem pola magnetycznego (zazwyczaj ziemi)

ETHERCAT - Ethernet for Control Automation Technology - jest standardemkomunikacji w urządzeniach automatyki Bazuje on na architekturze Ethernet Stan-dard ten został stworzony do zastosowań czasu rzeczywistego i pozwala na ustalenieczasu pomiaru z dokładnością do kilkudziesięciu mikrosekund

PDO - Process Data Object - Paczka danych procesowych służąca do przekazy-wania stanu urządzenia Ten typ obiektoacutew pozwala na ciągły odczyt danych

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 9

SDO - Service Data Object - Są to paczki danych pozwalające na konfiguracjeurządzenia podrzędnego

FMMU - jest urządzeniem odpowiedzialnym za mapowanie obiektoacutew PDO dopamięci urządzenia podrzędnego Pośredniczy ono w każdej komunikacji za pomocąmagistrali ethercat [7]

ROS - Robot Operating System - Jest to zbioacuter bibliotek oraz oprogramowaniezapewniające komunikacje międzyprocesowa spoacutejne środowisko uruchomienioweoraz środowisko do budowy oprogramowania

Węzeł - Jest to pojedyncza instancja programu uruchomionego w ramach opro-gramowania ROS

Temat - Jest to kanał komunikacji międzyprocesowej udostępniany przez opro-gramowanie ROS Topik posiada dwa typy Nodoacutew podłączonych do niego Publisherjet to node ktoacutery wysyła dane w danym topicu Subscriber jest nodem odbierającymdane z topicu Każdy kanał ma określony typ danych ktoacutere można na nim nadawaćoraz może mieć wielu nadawcoacutew (Publisher) i odbiorcoacutew (Subscriber)

OROCOS - Open RObot Control Software - jest otwartym oprogramowaniemsłużącym do sterowania systemami robotycznymi w czasie rzeczywistym Wszystkieakcje ktoacutere muszą być wykonywane w reżimie czasu rzeczywistego są uruchamianew ramach struktury OROCOSMCU - W skroacutecie mikrokontroler układ scalony zawierający w sobie rdzeń proce-sora oraz peryferia zewnętrzne W tym przypadku MCU to mikrokontrolerTM4C129ENCPDT wyposażony w rdzeń Cortex-M4F

MEMS - Od angielskiego microelectromechanical system Jest to zintegrowanyukład elektromechaniczny ktoacuterego co najmniej jeden wymiar znajduje się w skalimikro

22 Działanie jednostki inercyjnej

Jednostka inercyjna jest urządzeniem złożonym z kilku czujnikoacutew Najczęściej wszyst-kie zawarte w niej urządzenia wykonane sa w technologi MEMS Często spotykanąw ostatnich latach sytuacją jest że wszystkie te urządzenia znajdują się w jed-nym układzie scalonym Akcelerometry zawierające się w jednostkach inercyjnychwystępują praktycznie tylko w wersji MEMS a podstawy ich działania opisane sąw podsekcji 221 Żyroskopy roacutewnież w zdecydowanej większości występują jakowykonane w tej technologii elementy krzemowe ktoacuterych zasadę działania opisanow podsekcji 222 Jednak okazało się że można roacutewnież spotkać jednostki inercyjnezawierające żyroskopy światłowodowe przedstawione w podsekcji 223 Urządzeniate jednak najczęściej dedykowane są do zastosowań na dużych maszynach takich jakstatki morskie czy samoloty

221 Akcelerometry MEMS

Akcelerometry MEMSmożna dzielić na dwa typy Pierwszym z nich są akcelerometrypiezorezystancyjne [3] Akcelerometry te opierają się o dosyć prostą zasadę działania

10 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Masa odchylająca się pod wpływem siły bezwładności proacutebuje odkształcić piezorezy-story wmontowane w belkę na ktoacuterej jest zaczepiona Mierząc rezystancję wspomnia-nych piezorezystoroacutew można ustalić siłę działającą na masę oraz dzięki znajomościmasy wartość przyspieszenia ktoacuteremu poddawany jest układ Oczywiście wspoacutełcze-sne układy stosują zaawansowane techniki w celu zmniejszenia wpływu przyspieszeńw innych osiach niż badana oraz zabezpieczeniu przed uszkodzeniem wywołanymnadmiernymi przeciążeniami Jednak zasada działania pozostaje ta sama

Drugim rodzajem akcelerometroacutew MEMS są akcelerometry pojemnościowe Ak-celerometr pojemnościowy do swojego działania wymaga dwoacutech elementoacutew elek-trody stałej oraz elektrody połączonej z masą ktoacutera będzie poruszana siłą bezwład-ności Gdy masa zbliża się do elektrody stałej pojemność się zwiększa gdy natomiastsiła bezwładności działa w drugą stronę pojemność się zmniejsza Takie założeniapozwalają na pomiar przyspieszenia w akcelerometrze pojemnościowym

222 Żyroskopy MEMS

Obecnie produkowane żyroskopy MEMS to żyroskopy wibracyjne [3] Ich zasadadziałania jest nieco bardziej skomplikowana niż zasada działania akcelerometroacutewMEMS W układzie takim masę należy wprawić w wibracje wzdłuż jednej z osi (naprzykład x) leżącej na płaszczyźnie zawierającej dwie z nich (na przykład płaszczy-zna xy) W tej sytuacji pomiar możliwy będzie przy obrocie wokoacuteł trzeciej osi (z)Przy obrocie wokoacuteł osi Z czujnika siła Coriolisa przeniesie drgania z osi X na oś Ymierząc amplitudę tych drgań (na przykład za pomocą metody pojemnościowej jakw akcelerometrze pojemnościowym) można ustalić prędkość obrotu wokoacuteł danej osi

223 Żyroskopy światłowodowe

Działanie żyroskopoacutew światłowodowych oparte jest o efekt Sagnaca [12] jest toefekt polegający na przesunięciu w fazie dwuch rozszczepionych wiązek światła bie-gnących po pętli w przeciwnych kierunkach jeżeli układ się obraca Efekt ten jestspowodowany tym że w obracającym się układzie światło lecące w jedną stronę musipokonać dłuższą drogę niż światło lecące w stronę przeciwną Efekt ten stosuje sięw żyroskopach światłowodowych wielokrotnie zwiekszając drogę jaką musi pokonaćświatło poprzez zwiększenie ilości zwojoacutew światłowodu Żyroskopy takie mają bar-dzo wysoką czułość (nawet do jednej dziesięciotysięcznej stopnia na godzinę) orazbardzo niski dryft

23 Działanie magistrali Ethercat

EtherCAT jest standardem stworzonym przez firmę Beckhoff Automation Standardten jest jednak otwarty i powstają otwarte implementacje jego elementoacutew Choćsam standard swoje niższe warstwy opiera na Ethernecie (warstwę fizyczną orazczęść warstwy łącza danych) to wyższe z nich roacuteżnią się między sobą ze względuna zastosowanie na przykład protokołem warstwy aplikacji jest protokoacuteł Can OverEtherCAT opisany w podsekcji 231 W podsekcji 232 znajduje się wprowadzeniedo działania rozproszonego zegara magistrali EtherCAT

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 11

(a) Struktura ramki ethercat źroacutedło [5] (b) Struktura ramki danych źroacutedło [5]

Rysunek 21 Schematy przedstawiające strukturę ramki w komunikacji ethercat

231 COE

CANopen Over EtherCAT [5] jest protokołem warstwy aplikacji działającym w sieciEthercat jest to protokoacuteł komunikacyjny bazujący na systemie CAN Wykorzystanieimplementacji protokołu CoE sprawia że EtherCAT jest kompatybilny z urządze-niami obsługującymi CANopen Aby zrozumieć w jaki sposoacuteb datagramy CANopenprzekazywane są w protokole ethercat musimy przyjrzeć się strukturze jego ramkiTa (pokazana na rysunku 21(a)) zawiera czternastobajtowy nagłoacutewek ramki ether-cat dwubajtowe pole długości wiadomości wiadomość oraz czterobajtową sumękontrolną W polu wiadomości może znajdować się wiele datagramoacutew każdy z nichmoże być adresowany do innego urządzenia w magistrali ethercat Każdy datagramzawiera swoacutej nagłoacutewek ktoacutery adresuje go do danego urządzenia pole danych orazsumę kontrolną Skład pola danych pokazany na rysunku 21(b) a składa się onoz nagłoacutewka ktoacutery między innymi zawiera typ ktoacutery wskazuje na protokoacuteł warstwyaplikacji czyli na przykład CoE Następnie w ramce znajduje się komenda oraz od-powiadające jej dane

232 Synchronizacja zegara

Algorytm synchronizacji czasu w standardzie ethercat oparty jest o standard Pre-cision Time Protocol Jest to standard zaakceptowany przez IEEE o sygnaturzeIEEE 1588-2008 służący do submikrosekundowej synchronizacji czasu między urzą-dzeniami połączonymi w jedną sieć Więcej o działaniu samego protokołu możemyprzeczytać w publikacjach [15] [8] W protokole EtherCAT do synchronizacji zegarawyznaczany jest pierwsze urządzenie podrzędne na magistrali Ponieważ urządzeniapodrzędne magistrali EtherCAT wspierają synchronizacje sprzętową za pomocą pro-tokołu PTP możliwe jest odciążenie węzła głoacutewnego

24 Jednostki inercyjne ze złączami I2C oraz SPI

Szerokie spektrum tych urządzeń korzysta z interfejsoacutew I2C oraz SPI Taki spo-soacuteb podłączenia daje możliwość szybkiego przesyłu danych jednak ograniczeniemw takim przypadku jest odległość na jaka można prowadzić komunikację I2C niema zdefiniowanego maksymalnego zasięgu a maksymalną pojemność linii komuni-kacyjnej na poziomie 400 pF [16] co ogranicza zasięg komunikacji do pojedynczychmetroacutew w warunkach idealnych Magistrala SPI roacutewnież nie podaje maksymalnegozasięgu jednak w optymalnych warunkach jest to ok 2m

12 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Jednostki inercyjne ze złączami I2C oraz SPI są zazwyczaj tańszymi modelamiW większości są oferowane jako układy elektroniczne do montażu powierzchniowegoOto kilka przykładoacutew tego typu jednostek

bull LSM9DS0 - Jednostka inercyjna firmy ST electronics z interfejsem komunika-cyjnym SPII2C wyposażona w akcelerometr żyroskop i magnetometr Mak-symalne proacutebkowanie magnetometru to 100Hz akcelerometru 1600Hz żyro-skop ma maksymalną częstotliwość dostarczania danych 760Hz Jednostka do-starczana jest w obudowie LGA-24 o rozmiarach 4x4x1mm

bull ADIS16362 - Jednostka inercyjna firmy Analog devices jest to jednostka za-wierająca akcelerometr oraz żyroskop (oba troacutejosiowe) Charakteryzuje się onawysoką maksymalną częstotliwością proacutebkowania ponieważ dla obu czujnikoacutewwynosi ona 1200Hz Interfejs komunikacyjny stanowi złącze SPI Urządzeniedostarczane jest w obudowie w kształcie sześcianu 2x2 cm z miejscami naprzykręcenie jej do urządzenia W przeciwieństwie do poprzedniego urządze-nia jednostka posiada program kalibracji

25 Jednostki inercyjne ze złączami przemysłowymi

Inne z tych urządzeń komunikują się za pomocą dedykowanych standardoacutew prze-mysłowych ktoacutere charakteryzują się znacznie większą odpornością na zakłoacuteceniaa także na ciężkie warunki pracy Warto w tym miejscu zaznaczyć że jak dotąd narynku nie ma jednostki inercyjnej ktoacutera komunikowała by się za pomocą magistraliEthercat

Jednostki inercyjne omawiane w tym podrozdziale są urządzeniami ze złączamiprzemysłowymi przez co rozumiemy że złącza te charakteryzują się dużą nieza-wodnością Niestety modele zapewniające taki komfort są zazwyczaj znacznie więk-sze oraz droższe

bull MTi 100-series - Jednostki inercyjne firmy Xsens jest jednostką zawierającąakcelerometr i żyroskop Maksymalna częstotliwość dostarczania danych to2000Hz Jednostka ta komunikuje się za pomocą interfejsoacutew RS232 RS485422 UART USB Urządzenie to jest dostarczone w wodoodpornej obudo-wie mocowanej do urządzenia na śruby z przemysłowym złączem służącym dokomunikacji oraz zasilania Producent deklaruje że urządzenie posiada żyro-skop filtrujący wpływ wibracji

bull KVH 1750 IMU - Jednostka inercyjna firmy KVH jest urządzeniem posiada-jącym żyroskop światłowodowy oraz 3 oddzielne akcelerometry MEMS Inter-fejs komunikacyjny tego modułu to RS-422 Obudowa tej jednostki to waleco średnicy 89 mm i wysokości 74 mm przytwierdzany za pomocą sześciu śrubUrządzenie może produkować dane z częstotliwością do 1000Hz Charaktery-zuje się ono bardzo niskim dryftem żyroskopu Dryft ten typowo musi byćkorygowany za pomocą magnetometru

bull 6DF-1N2-C2-HWL - Jednostka inercyjna firmy Honeywell wyposażona jestw akcelerometr oraz żyroskop (oba troacutejosiowe) Interfejs komunikacyjny tegourządzenia to CAN Maksymalna częstotliwość dostarczania danych to 100Hz

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 13

26 Możliwe zastosowania proponowanego układu

Możliwe zastosowania jednostek inercyjnych w robotyce pojawiają się tam gdziewystępuje ruch inny od jednostajnego prostoliniowego W tym rozdziale rozważymydwa zastosowania w ktoacuterych proponowany układ może pokazać swoje zalety wy-nikające ze sposobu komunikacji W sekcji 261 omoacutewimy przypadek zastosowaniajednostki inercyjnej do wspomagania procesu nawigacji robota mobilnego o napę-dzie roacuteżnicowym Sekcja 262 dotyczyć będzie przypadku użycia takiego urządzeniado korekty odczytu siły z czujnika umieszczonego w nadgarstku manipulatora

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym

W tej sekcji przedstawiony zostanie przykład nawigacji robota mobilnego o napę-dzie roacuteżnicowym Przykładem takiego robota może być robot o konstrukcji takiejjak baza mobilna elektron [10] czyli robot posiadający wiele koacuteł po jednej stro-nie co generuje problemy z odtworzeniem trajektorii ruchu robota przez odczytyz czujnikoacutew odometrycznych Dzieje się tak przez to że budowa robota wprowa-dza poślizgi podczas skrętoacutew Robot taki ze względu na roacuteżne czynniki zewnętrznenp typ podłoża nieroacutewności podłoża skręca w okoacuteł trudnej do ustalenia osi obrotuktoacutera w dodatku może być zmienna (np gdy jedno koło jest na innym typie podłożaniż wszystkie inne) Aby temu zapobiec można użyć do estymacji trajektorii ruchurobota pomiaroacutew pochodzących z czujnika inercyjnego razem z odczytami z senso-roacutew odometrycznych Aby tego dokonać potrzebna jest fuzja odczytoacutew z roacuteżnychczujnikoacutew Istnieje wiele metod podejścia do tego tematu jednak jednym z częściejspotykanych w literaturze jest użycie rozszerzonego filtru Kalmana [18] [17] [4]

262 Korekcja odczytu siły z czujnika nadgarstkowego

Drugim przykładem zastosowania jednostki inercyjnej w robotyce jest korekcja od-czytu z czujnika siły [20] Nie jest to zastosowanie tak szeroko omawiane w literaturzejak nawigacja wspomagana inercyjnie opisana w sekcji 261 Pomimo niewielkiegozainteresowania jest to bardzo ciekawy temat badawczy głoacutewnie ze względu na chęćwyeliminowania bdquopikoacutewrdquo odczytoacutew siły wynikających z nagłej zmiany kierunku ru-chu ramienia bądź też drgnięcia wynikającego z niedoskonałości regulatora Do tegozastosowania konstruuje się obserwator siły oparty o filtr Kalmana [6]

14 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Rozdział 2

Wstęp teoretyczny przegląddostępnych rozwiązań oraz możliwezastosowania układu

Jednostka inercyjna jest urządzeniem służącym do pomiaru przyspieszeń liniowychprędkości obrotowych oraz w niektoacuterych przypadkach orientacji w polu magnetycz-nym Zawiera ono w sobie troacutejosiowe akcelerometr oraz żyroskop (dodatkowo nie-ktoacutere modele zawierają troacutejosiowy magnetometr pozwalający na wyznaczenie orien-tacji względem pola magnetycznego) Urządzenia takie roacuteżnią się pomiędzy sobąparametrami oraz sposobem połączenia z urządzeniem nadrzędnym

W tym rozdziale czytelnik zostanie wprowadzony w podstawowe zagadnienia natemat działania jednostki inercyjnej Najpierw w sekcji 21 przedstawiony zosta-nie spis definicji oraz skroacutetoacutew Zasady działania jednostki inercyjnej zostały opi-sane w sekcji 22 W sekcji 23 przedstawione zostały podstawy działania magistraliEtherCAT oraz jej mechanizmoacutew

Jednostki inercyjne dzielą się przede wszystkim ze względu na obecność magne-tometru częstotliwość pomiaru (oraz dostarczania danych wyjściowych) oraz nasposoacuteb komunikacji Przykłady urządzeń komunikujących się za pomocą interfejsoacutewI2C oraz SPI opisane są w sekcji 24 a te komunikujące się za pomocą standardoacutewprzemysłowych opisane są w sekcji 25 Na końcu wzbogacony tą wiedzą w sekcji 26czytelnik pozna przykładowe zastosowania takiego urządzenia w zadaniach robotyki

21 Definicje

IMU - z Angielskiego Inertial Measurement Unit - zespoacuteł czujnikoacutew pozwalającyna pomiar Przyspieszeń ciała oraz prędkości kontowych z jakimi się porusza Cza-sami roacutewnież zawiera on Magnetometr pozwalający na pomiar orientacji urządzeniawzględem pola magnetycznego (zazwyczaj ziemi)

ETHERCAT - Ethernet for Control Automation Technology - jest standardemkomunikacji w urządzeniach automatyki Bazuje on na architekturze Ethernet Stan-dard ten został stworzony do zastosowań czasu rzeczywistego i pozwala na ustalenieczasu pomiaru z dokładnością do kilkudziesięciu mikrosekund

PDO - Process Data Object - Paczka danych procesowych służąca do przekazy-wania stanu urządzenia Ten typ obiektoacutew pozwala na ciągły odczyt danych

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 9

SDO - Service Data Object - Są to paczki danych pozwalające na konfiguracjeurządzenia podrzędnego

FMMU - jest urządzeniem odpowiedzialnym za mapowanie obiektoacutew PDO dopamięci urządzenia podrzędnego Pośredniczy ono w każdej komunikacji za pomocąmagistrali ethercat [7]

ROS - Robot Operating System - Jest to zbioacuter bibliotek oraz oprogramowaniezapewniające komunikacje międzyprocesowa spoacutejne środowisko uruchomienioweoraz środowisko do budowy oprogramowania

Węzeł - Jest to pojedyncza instancja programu uruchomionego w ramach opro-gramowania ROS

Temat - Jest to kanał komunikacji międzyprocesowej udostępniany przez opro-gramowanie ROS Topik posiada dwa typy Nodoacutew podłączonych do niego Publisherjet to node ktoacutery wysyła dane w danym topicu Subscriber jest nodem odbierającymdane z topicu Każdy kanał ma określony typ danych ktoacutere można na nim nadawaćoraz może mieć wielu nadawcoacutew (Publisher) i odbiorcoacutew (Subscriber)

OROCOS - Open RObot Control Software - jest otwartym oprogramowaniemsłużącym do sterowania systemami robotycznymi w czasie rzeczywistym Wszystkieakcje ktoacutere muszą być wykonywane w reżimie czasu rzeczywistego są uruchamianew ramach struktury OROCOSMCU - W skroacutecie mikrokontroler układ scalony zawierający w sobie rdzeń proce-sora oraz peryferia zewnętrzne W tym przypadku MCU to mikrokontrolerTM4C129ENCPDT wyposażony w rdzeń Cortex-M4F

MEMS - Od angielskiego microelectromechanical system Jest to zintegrowanyukład elektromechaniczny ktoacuterego co najmniej jeden wymiar znajduje się w skalimikro

22 Działanie jednostki inercyjnej

Jednostka inercyjna jest urządzeniem złożonym z kilku czujnikoacutew Najczęściej wszyst-kie zawarte w niej urządzenia wykonane sa w technologi MEMS Często spotykanąw ostatnich latach sytuacją jest że wszystkie te urządzenia znajdują się w jed-nym układzie scalonym Akcelerometry zawierające się w jednostkach inercyjnychwystępują praktycznie tylko w wersji MEMS a podstawy ich działania opisane sąw podsekcji 221 Żyroskopy roacutewnież w zdecydowanej większości występują jakowykonane w tej technologii elementy krzemowe ktoacuterych zasadę działania opisanow podsekcji 222 Jednak okazało się że można roacutewnież spotkać jednostki inercyjnezawierające żyroskopy światłowodowe przedstawione w podsekcji 223 Urządzeniate jednak najczęściej dedykowane są do zastosowań na dużych maszynach takich jakstatki morskie czy samoloty

221 Akcelerometry MEMS

Akcelerometry MEMSmożna dzielić na dwa typy Pierwszym z nich są akcelerometrypiezorezystancyjne [3] Akcelerometry te opierają się o dosyć prostą zasadę działania

10 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Masa odchylająca się pod wpływem siły bezwładności proacutebuje odkształcić piezorezy-story wmontowane w belkę na ktoacuterej jest zaczepiona Mierząc rezystancję wspomnia-nych piezorezystoroacutew można ustalić siłę działającą na masę oraz dzięki znajomościmasy wartość przyspieszenia ktoacuteremu poddawany jest układ Oczywiście wspoacutełcze-sne układy stosują zaawansowane techniki w celu zmniejszenia wpływu przyspieszeńw innych osiach niż badana oraz zabezpieczeniu przed uszkodzeniem wywołanymnadmiernymi przeciążeniami Jednak zasada działania pozostaje ta sama

Drugim rodzajem akcelerometroacutew MEMS są akcelerometry pojemnościowe Ak-celerometr pojemnościowy do swojego działania wymaga dwoacutech elementoacutew elek-trody stałej oraz elektrody połączonej z masą ktoacutera będzie poruszana siłą bezwład-ności Gdy masa zbliża się do elektrody stałej pojemność się zwiększa gdy natomiastsiła bezwładności działa w drugą stronę pojemność się zmniejsza Takie założeniapozwalają na pomiar przyspieszenia w akcelerometrze pojemnościowym

222 Żyroskopy MEMS

Obecnie produkowane żyroskopy MEMS to żyroskopy wibracyjne [3] Ich zasadadziałania jest nieco bardziej skomplikowana niż zasada działania akcelerometroacutewMEMS W układzie takim masę należy wprawić w wibracje wzdłuż jednej z osi (naprzykład x) leżącej na płaszczyźnie zawierającej dwie z nich (na przykład płaszczy-zna xy) W tej sytuacji pomiar możliwy będzie przy obrocie wokoacuteł trzeciej osi (z)Przy obrocie wokoacuteł osi Z czujnika siła Coriolisa przeniesie drgania z osi X na oś Ymierząc amplitudę tych drgań (na przykład za pomocą metody pojemnościowej jakw akcelerometrze pojemnościowym) można ustalić prędkość obrotu wokoacuteł danej osi

223 Żyroskopy światłowodowe

Działanie żyroskopoacutew światłowodowych oparte jest o efekt Sagnaca [12] jest toefekt polegający na przesunięciu w fazie dwuch rozszczepionych wiązek światła bie-gnących po pętli w przeciwnych kierunkach jeżeli układ się obraca Efekt ten jestspowodowany tym że w obracającym się układzie światło lecące w jedną stronę musipokonać dłuższą drogę niż światło lecące w stronę przeciwną Efekt ten stosuje sięw żyroskopach światłowodowych wielokrotnie zwiekszając drogę jaką musi pokonaćświatło poprzez zwiększenie ilości zwojoacutew światłowodu Żyroskopy takie mają bar-dzo wysoką czułość (nawet do jednej dziesięciotysięcznej stopnia na godzinę) orazbardzo niski dryft

23 Działanie magistrali Ethercat

EtherCAT jest standardem stworzonym przez firmę Beckhoff Automation Standardten jest jednak otwarty i powstają otwarte implementacje jego elementoacutew Choćsam standard swoje niższe warstwy opiera na Ethernecie (warstwę fizyczną orazczęść warstwy łącza danych) to wyższe z nich roacuteżnią się między sobą ze względuna zastosowanie na przykład protokołem warstwy aplikacji jest protokoacuteł Can OverEtherCAT opisany w podsekcji 231 W podsekcji 232 znajduje się wprowadzeniedo działania rozproszonego zegara magistrali EtherCAT

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 11

(a) Struktura ramki ethercat źroacutedło [5] (b) Struktura ramki danych źroacutedło [5]

Rysunek 21 Schematy przedstawiające strukturę ramki w komunikacji ethercat

231 COE

CANopen Over EtherCAT [5] jest protokołem warstwy aplikacji działającym w sieciEthercat jest to protokoacuteł komunikacyjny bazujący na systemie CAN Wykorzystanieimplementacji protokołu CoE sprawia że EtherCAT jest kompatybilny z urządze-niami obsługującymi CANopen Aby zrozumieć w jaki sposoacuteb datagramy CANopenprzekazywane są w protokole ethercat musimy przyjrzeć się strukturze jego ramkiTa (pokazana na rysunku 21(a)) zawiera czternastobajtowy nagłoacutewek ramki ether-cat dwubajtowe pole długości wiadomości wiadomość oraz czterobajtową sumękontrolną W polu wiadomości może znajdować się wiele datagramoacutew każdy z nichmoże być adresowany do innego urządzenia w magistrali ethercat Każdy datagramzawiera swoacutej nagłoacutewek ktoacutery adresuje go do danego urządzenia pole danych orazsumę kontrolną Skład pola danych pokazany na rysunku 21(b) a składa się onoz nagłoacutewka ktoacutery między innymi zawiera typ ktoacutery wskazuje na protokoacuteł warstwyaplikacji czyli na przykład CoE Następnie w ramce znajduje się komenda oraz od-powiadające jej dane

232 Synchronizacja zegara

Algorytm synchronizacji czasu w standardzie ethercat oparty jest o standard Pre-cision Time Protocol Jest to standard zaakceptowany przez IEEE o sygnaturzeIEEE 1588-2008 służący do submikrosekundowej synchronizacji czasu między urzą-dzeniami połączonymi w jedną sieć Więcej o działaniu samego protokołu możemyprzeczytać w publikacjach [15] [8] W protokole EtherCAT do synchronizacji zegarawyznaczany jest pierwsze urządzenie podrzędne na magistrali Ponieważ urządzeniapodrzędne magistrali EtherCAT wspierają synchronizacje sprzętową za pomocą pro-tokołu PTP możliwe jest odciążenie węzła głoacutewnego

24 Jednostki inercyjne ze złączami I2C oraz SPI

Szerokie spektrum tych urządzeń korzysta z interfejsoacutew I2C oraz SPI Taki spo-soacuteb podłączenia daje możliwość szybkiego przesyłu danych jednak ograniczeniemw takim przypadku jest odległość na jaka można prowadzić komunikację I2C niema zdefiniowanego maksymalnego zasięgu a maksymalną pojemność linii komuni-kacyjnej na poziomie 400 pF [16] co ogranicza zasięg komunikacji do pojedynczychmetroacutew w warunkach idealnych Magistrala SPI roacutewnież nie podaje maksymalnegozasięgu jednak w optymalnych warunkach jest to ok 2m

12 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Jednostki inercyjne ze złączami I2C oraz SPI są zazwyczaj tańszymi modelamiW większości są oferowane jako układy elektroniczne do montażu powierzchniowegoOto kilka przykładoacutew tego typu jednostek

bull LSM9DS0 - Jednostka inercyjna firmy ST electronics z interfejsem komunika-cyjnym SPII2C wyposażona w akcelerometr żyroskop i magnetometr Mak-symalne proacutebkowanie magnetometru to 100Hz akcelerometru 1600Hz żyro-skop ma maksymalną częstotliwość dostarczania danych 760Hz Jednostka do-starczana jest w obudowie LGA-24 o rozmiarach 4x4x1mm

bull ADIS16362 - Jednostka inercyjna firmy Analog devices jest to jednostka za-wierająca akcelerometr oraz żyroskop (oba troacutejosiowe) Charakteryzuje się onawysoką maksymalną częstotliwością proacutebkowania ponieważ dla obu czujnikoacutewwynosi ona 1200Hz Interfejs komunikacyjny stanowi złącze SPI Urządzeniedostarczane jest w obudowie w kształcie sześcianu 2x2 cm z miejscami naprzykręcenie jej do urządzenia W przeciwieństwie do poprzedniego urządze-nia jednostka posiada program kalibracji

25 Jednostki inercyjne ze złączami przemysłowymi

Inne z tych urządzeń komunikują się za pomocą dedykowanych standardoacutew prze-mysłowych ktoacutere charakteryzują się znacznie większą odpornością na zakłoacuteceniaa także na ciężkie warunki pracy Warto w tym miejscu zaznaczyć że jak dotąd narynku nie ma jednostki inercyjnej ktoacutera komunikowała by się za pomocą magistraliEthercat

Jednostki inercyjne omawiane w tym podrozdziale są urządzeniami ze złączamiprzemysłowymi przez co rozumiemy że złącza te charakteryzują się dużą nieza-wodnością Niestety modele zapewniające taki komfort są zazwyczaj znacznie więk-sze oraz droższe

bull MTi 100-series - Jednostki inercyjne firmy Xsens jest jednostką zawierającąakcelerometr i żyroskop Maksymalna częstotliwość dostarczania danych to2000Hz Jednostka ta komunikuje się za pomocą interfejsoacutew RS232 RS485422 UART USB Urządzenie to jest dostarczone w wodoodpornej obudo-wie mocowanej do urządzenia na śruby z przemysłowym złączem służącym dokomunikacji oraz zasilania Producent deklaruje że urządzenie posiada żyro-skop filtrujący wpływ wibracji

bull KVH 1750 IMU - Jednostka inercyjna firmy KVH jest urządzeniem posiada-jącym żyroskop światłowodowy oraz 3 oddzielne akcelerometry MEMS Inter-fejs komunikacyjny tego modułu to RS-422 Obudowa tej jednostki to waleco średnicy 89 mm i wysokości 74 mm przytwierdzany za pomocą sześciu śrubUrządzenie może produkować dane z częstotliwością do 1000Hz Charaktery-zuje się ono bardzo niskim dryftem żyroskopu Dryft ten typowo musi byćkorygowany za pomocą magnetometru

bull 6DF-1N2-C2-HWL - Jednostka inercyjna firmy Honeywell wyposażona jestw akcelerometr oraz żyroskop (oba troacutejosiowe) Interfejs komunikacyjny tegourządzenia to CAN Maksymalna częstotliwość dostarczania danych to 100Hz

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 13

26 Możliwe zastosowania proponowanego układu

Możliwe zastosowania jednostek inercyjnych w robotyce pojawiają się tam gdziewystępuje ruch inny od jednostajnego prostoliniowego W tym rozdziale rozważymydwa zastosowania w ktoacuterych proponowany układ może pokazać swoje zalety wy-nikające ze sposobu komunikacji W sekcji 261 omoacutewimy przypadek zastosowaniajednostki inercyjnej do wspomagania procesu nawigacji robota mobilnego o napę-dzie roacuteżnicowym Sekcja 262 dotyczyć będzie przypadku użycia takiego urządzeniado korekty odczytu siły z czujnika umieszczonego w nadgarstku manipulatora

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym

W tej sekcji przedstawiony zostanie przykład nawigacji robota mobilnego o napę-dzie roacuteżnicowym Przykładem takiego robota może być robot o konstrukcji takiejjak baza mobilna elektron [10] czyli robot posiadający wiele koacuteł po jednej stro-nie co generuje problemy z odtworzeniem trajektorii ruchu robota przez odczytyz czujnikoacutew odometrycznych Dzieje się tak przez to że budowa robota wprowa-dza poślizgi podczas skrętoacutew Robot taki ze względu na roacuteżne czynniki zewnętrznenp typ podłoża nieroacutewności podłoża skręca w okoacuteł trudnej do ustalenia osi obrotuktoacutera w dodatku może być zmienna (np gdy jedno koło jest na innym typie podłożaniż wszystkie inne) Aby temu zapobiec można użyć do estymacji trajektorii ruchurobota pomiaroacutew pochodzących z czujnika inercyjnego razem z odczytami z senso-roacutew odometrycznych Aby tego dokonać potrzebna jest fuzja odczytoacutew z roacuteżnychczujnikoacutew Istnieje wiele metod podejścia do tego tematu jednak jednym z częściejspotykanych w literaturze jest użycie rozszerzonego filtru Kalmana [18] [17] [4]

262 Korekcja odczytu siły z czujnika nadgarstkowego

Drugim przykładem zastosowania jednostki inercyjnej w robotyce jest korekcja od-czytu z czujnika siły [20] Nie jest to zastosowanie tak szeroko omawiane w literaturzejak nawigacja wspomagana inercyjnie opisana w sekcji 261 Pomimo niewielkiegozainteresowania jest to bardzo ciekawy temat badawczy głoacutewnie ze względu na chęćwyeliminowania bdquopikoacutewrdquo odczytoacutew siły wynikających z nagłej zmiany kierunku ru-chu ramienia bądź też drgnięcia wynikającego z niedoskonałości regulatora Do tegozastosowania konstruuje się obserwator siły oparty o filtr Kalmana [6]

14 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

SDO - Service Data Object - Są to paczki danych pozwalające na konfiguracjeurządzenia podrzędnego

FMMU - jest urządzeniem odpowiedzialnym za mapowanie obiektoacutew PDO dopamięci urządzenia podrzędnego Pośredniczy ono w każdej komunikacji za pomocąmagistrali ethercat [7]

ROS - Robot Operating System - Jest to zbioacuter bibliotek oraz oprogramowaniezapewniające komunikacje międzyprocesowa spoacutejne środowisko uruchomienioweoraz środowisko do budowy oprogramowania

Węzeł - Jest to pojedyncza instancja programu uruchomionego w ramach opro-gramowania ROS

Temat - Jest to kanał komunikacji międzyprocesowej udostępniany przez opro-gramowanie ROS Topik posiada dwa typy Nodoacutew podłączonych do niego Publisherjet to node ktoacutery wysyła dane w danym topicu Subscriber jest nodem odbierającymdane z topicu Każdy kanał ma określony typ danych ktoacutere można na nim nadawaćoraz może mieć wielu nadawcoacutew (Publisher) i odbiorcoacutew (Subscriber)

OROCOS - Open RObot Control Software - jest otwartym oprogramowaniemsłużącym do sterowania systemami robotycznymi w czasie rzeczywistym Wszystkieakcje ktoacutere muszą być wykonywane w reżimie czasu rzeczywistego są uruchamianew ramach struktury OROCOSMCU - W skroacutecie mikrokontroler układ scalony zawierający w sobie rdzeń proce-sora oraz peryferia zewnętrzne W tym przypadku MCU to mikrokontrolerTM4C129ENCPDT wyposażony w rdzeń Cortex-M4F

MEMS - Od angielskiego microelectromechanical system Jest to zintegrowanyukład elektromechaniczny ktoacuterego co najmniej jeden wymiar znajduje się w skalimikro

22 Działanie jednostki inercyjnej

Jednostka inercyjna jest urządzeniem złożonym z kilku czujnikoacutew Najczęściej wszyst-kie zawarte w niej urządzenia wykonane sa w technologi MEMS Często spotykanąw ostatnich latach sytuacją jest że wszystkie te urządzenia znajdują się w jed-nym układzie scalonym Akcelerometry zawierające się w jednostkach inercyjnychwystępują praktycznie tylko w wersji MEMS a podstawy ich działania opisane sąw podsekcji 221 Żyroskopy roacutewnież w zdecydowanej większości występują jakowykonane w tej technologii elementy krzemowe ktoacuterych zasadę działania opisanow podsekcji 222 Jednak okazało się że można roacutewnież spotkać jednostki inercyjnezawierające żyroskopy światłowodowe przedstawione w podsekcji 223 Urządzeniate jednak najczęściej dedykowane są do zastosowań na dużych maszynach takich jakstatki morskie czy samoloty

221 Akcelerometry MEMS

Akcelerometry MEMSmożna dzielić na dwa typy Pierwszym z nich są akcelerometrypiezorezystancyjne [3] Akcelerometry te opierają się o dosyć prostą zasadę działania

10 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Masa odchylająca się pod wpływem siły bezwładności proacutebuje odkształcić piezorezy-story wmontowane w belkę na ktoacuterej jest zaczepiona Mierząc rezystancję wspomnia-nych piezorezystoroacutew można ustalić siłę działającą na masę oraz dzięki znajomościmasy wartość przyspieszenia ktoacuteremu poddawany jest układ Oczywiście wspoacutełcze-sne układy stosują zaawansowane techniki w celu zmniejszenia wpływu przyspieszeńw innych osiach niż badana oraz zabezpieczeniu przed uszkodzeniem wywołanymnadmiernymi przeciążeniami Jednak zasada działania pozostaje ta sama

Drugim rodzajem akcelerometroacutew MEMS są akcelerometry pojemnościowe Ak-celerometr pojemnościowy do swojego działania wymaga dwoacutech elementoacutew elek-trody stałej oraz elektrody połączonej z masą ktoacutera będzie poruszana siłą bezwład-ności Gdy masa zbliża się do elektrody stałej pojemność się zwiększa gdy natomiastsiła bezwładności działa w drugą stronę pojemność się zmniejsza Takie założeniapozwalają na pomiar przyspieszenia w akcelerometrze pojemnościowym

222 Żyroskopy MEMS

Obecnie produkowane żyroskopy MEMS to żyroskopy wibracyjne [3] Ich zasadadziałania jest nieco bardziej skomplikowana niż zasada działania akcelerometroacutewMEMS W układzie takim masę należy wprawić w wibracje wzdłuż jednej z osi (naprzykład x) leżącej na płaszczyźnie zawierającej dwie z nich (na przykład płaszczy-zna xy) W tej sytuacji pomiar możliwy będzie przy obrocie wokoacuteł trzeciej osi (z)Przy obrocie wokoacuteł osi Z czujnika siła Coriolisa przeniesie drgania z osi X na oś Ymierząc amplitudę tych drgań (na przykład za pomocą metody pojemnościowej jakw akcelerometrze pojemnościowym) można ustalić prędkość obrotu wokoacuteł danej osi

223 Żyroskopy światłowodowe

Działanie żyroskopoacutew światłowodowych oparte jest o efekt Sagnaca [12] jest toefekt polegający na przesunięciu w fazie dwuch rozszczepionych wiązek światła bie-gnących po pętli w przeciwnych kierunkach jeżeli układ się obraca Efekt ten jestspowodowany tym że w obracającym się układzie światło lecące w jedną stronę musipokonać dłuższą drogę niż światło lecące w stronę przeciwną Efekt ten stosuje sięw żyroskopach światłowodowych wielokrotnie zwiekszając drogę jaką musi pokonaćświatło poprzez zwiększenie ilości zwojoacutew światłowodu Żyroskopy takie mają bar-dzo wysoką czułość (nawet do jednej dziesięciotysięcznej stopnia na godzinę) orazbardzo niski dryft

23 Działanie magistrali Ethercat

EtherCAT jest standardem stworzonym przez firmę Beckhoff Automation Standardten jest jednak otwarty i powstają otwarte implementacje jego elementoacutew Choćsam standard swoje niższe warstwy opiera na Ethernecie (warstwę fizyczną orazczęść warstwy łącza danych) to wyższe z nich roacuteżnią się między sobą ze względuna zastosowanie na przykład protokołem warstwy aplikacji jest protokoacuteł Can OverEtherCAT opisany w podsekcji 231 W podsekcji 232 znajduje się wprowadzeniedo działania rozproszonego zegara magistrali EtherCAT

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 11

(a) Struktura ramki ethercat źroacutedło [5] (b) Struktura ramki danych źroacutedło [5]

Rysunek 21 Schematy przedstawiające strukturę ramki w komunikacji ethercat

231 COE

CANopen Over EtherCAT [5] jest protokołem warstwy aplikacji działającym w sieciEthercat jest to protokoacuteł komunikacyjny bazujący na systemie CAN Wykorzystanieimplementacji protokołu CoE sprawia że EtherCAT jest kompatybilny z urządze-niami obsługującymi CANopen Aby zrozumieć w jaki sposoacuteb datagramy CANopenprzekazywane są w protokole ethercat musimy przyjrzeć się strukturze jego ramkiTa (pokazana na rysunku 21(a)) zawiera czternastobajtowy nagłoacutewek ramki ether-cat dwubajtowe pole długości wiadomości wiadomość oraz czterobajtową sumękontrolną W polu wiadomości może znajdować się wiele datagramoacutew każdy z nichmoże być adresowany do innego urządzenia w magistrali ethercat Każdy datagramzawiera swoacutej nagłoacutewek ktoacutery adresuje go do danego urządzenia pole danych orazsumę kontrolną Skład pola danych pokazany na rysunku 21(b) a składa się onoz nagłoacutewka ktoacutery między innymi zawiera typ ktoacutery wskazuje na protokoacuteł warstwyaplikacji czyli na przykład CoE Następnie w ramce znajduje się komenda oraz od-powiadające jej dane

232 Synchronizacja zegara

Algorytm synchronizacji czasu w standardzie ethercat oparty jest o standard Pre-cision Time Protocol Jest to standard zaakceptowany przez IEEE o sygnaturzeIEEE 1588-2008 służący do submikrosekundowej synchronizacji czasu między urzą-dzeniami połączonymi w jedną sieć Więcej o działaniu samego protokołu możemyprzeczytać w publikacjach [15] [8] W protokole EtherCAT do synchronizacji zegarawyznaczany jest pierwsze urządzenie podrzędne na magistrali Ponieważ urządzeniapodrzędne magistrali EtherCAT wspierają synchronizacje sprzętową za pomocą pro-tokołu PTP możliwe jest odciążenie węzła głoacutewnego

24 Jednostki inercyjne ze złączami I2C oraz SPI

Szerokie spektrum tych urządzeń korzysta z interfejsoacutew I2C oraz SPI Taki spo-soacuteb podłączenia daje możliwość szybkiego przesyłu danych jednak ograniczeniemw takim przypadku jest odległość na jaka można prowadzić komunikację I2C niema zdefiniowanego maksymalnego zasięgu a maksymalną pojemność linii komuni-kacyjnej na poziomie 400 pF [16] co ogranicza zasięg komunikacji do pojedynczychmetroacutew w warunkach idealnych Magistrala SPI roacutewnież nie podaje maksymalnegozasięgu jednak w optymalnych warunkach jest to ok 2m

12 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Jednostki inercyjne ze złączami I2C oraz SPI są zazwyczaj tańszymi modelamiW większości są oferowane jako układy elektroniczne do montażu powierzchniowegoOto kilka przykładoacutew tego typu jednostek

bull LSM9DS0 - Jednostka inercyjna firmy ST electronics z interfejsem komunika-cyjnym SPII2C wyposażona w akcelerometr żyroskop i magnetometr Mak-symalne proacutebkowanie magnetometru to 100Hz akcelerometru 1600Hz żyro-skop ma maksymalną częstotliwość dostarczania danych 760Hz Jednostka do-starczana jest w obudowie LGA-24 o rozmiarach 4x4x1mm

bull ADIS16362 - Jednostka inercyjna firmy Analog devices jest to jednostka za-wierająca akcelerometr oraz żyroskop (oba troacutejosiowe) Charakteryzuje się onawysoką maksymalną częstotliwością proacutebkowania ponieważ dla obu czujnikoacutewwynosi ona 1200Hz Interfejs komunikacyjny stanowi złącze SPI Urządzeniedostarczane jest w obudowie w kształcie sześcianu 2x2 cm z miejscami naprzykręcenie jej do urządzenia W przeciwieństwie do poprzedniego urządze-nia jednostka posiada program kalibracji

25 Jednostki inercyjne ze złączami przemysłowymi

Inne z tych urządzeń komunikują się za pomocą dedykowanych standardoacutew prze-mysłowych ktoacutere charakteryzują się znacznie większą odpornością na zakłoacuteceniaa także na ciężkie warunki pracy Warto w tym miejscu zaznaczyć że jak dotąd narynku nie ma jednostki inercyjnej ktoacutera komunikowała by się za pomocą magistraliEthercat

Jednostki inercyjne omawiane w tym podrozdziale są urządzeniami ze złączamiprzemysłowymi przez co rozumiemy że złącza te charakteryzują się dużą nieza-wodnością Niestety modele zapewniające taki komfort są zazwyczaj znacznie więk-sze oraz droższe

bull MTi 100-series - Jednostki inercyjne firmy Xsens jest jednostką zawierającąakcelerometr i żyroskop Maksymalna częstotliwość dostarczania danych to2000Hz Jednostka ta komunikuje się za pomocą interfejsoacutew RS232 RS485422 UART USB Urządzenie to jest dostarczone w wodoodpornej obudo-wie mocowanej do urządzenia na śruby z przemysłowym złączem służącym dokomunikacji oraz zasilania Producent deklaruje że urządzenie posiada żyro-skop filtrujący wpływ wibracji

bull KVH 1750 IMU - Jednostka inercyjna firmy KVH jest urządzeniem posiada-jącym żyroskop światłowodowy oraz 3 oddzielne akcelerometry MEMS Inter-fejs komunikacyjny tego modułu to RS-422 Obudowa tej jednostki to waleco średnicy 89 mm i wysokości 74 mm przytwierdzany za pomocą sześciu śrubUrządzenie może produkować dane z częstotliwością do 1000Hz Charaktery-zuje się ono bardzo niskim dryftem żyroskopu Dryft ten typowo musi byćkorygowany za pomocą magnetometru

bull 6DF-1N2-C2-HWL - Jednostka inercyjna firmy Honeywell wyposażona jestw akcelerometr oraz żyroskop (oba troacutejosiowe) Interfejs komunikacyjny tegourządzenia to CAN Maksymalna częstotliwość dostarczania danych to 100Hz

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 13

26 Możliwe zastosowania proponowanego układu

Możliwe zastosowania jednostek inercyjnych w robotyce pojawiają się tam gdziewystępuje ruch inny od jednostajnego prostoliniowego W tym rozdziale rozważymydwa zastosowania w ktoacuterych proponowany układ może pokazać swoje zalety wy-nikające ze sposobu komunikacji W sekcji 261 omoacutewimy przypadek zastosowaniajednostki inercyjnej do wspomagania procesu nawigacji robota mobilnego o napę-dzie roacuteżnicowym Sekcja 262 dotyczyć będzie przypadku użycia takiego urządzeniado korekty odczytu siły z czujnika umieszczonego w nadgarstku manipulatora

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym

W tej sekcji przedstawiony zostanie przykład nawigacji robota mobilnego o napę-dzie roacuteżnicowym Przykładem takiego robota może być robot o konstrukcji takiejjak baza mobilna elektron [10] czyli robot posiadający wiele koacuteł po jednej stro-nie co generuje problemy z odtworzeniem trajektorii ruchu robota przez odczytyz czujnikoacutew odometrycznych Dzieje się tak przez to że budowa robota wprowa-dza poślizgi podczas skrętoacutew Robot taki ze względu na roacuteżne czynniki zewnętrznenp typ podłoża nieroacutewności podłoża skręca w okoacuteł trudnej do ustalenia osi obrotuktoacutera w dodatku może być zmienna (np gdy jedno koło jest na innym typie podłożaniż wszystkie inne) Aby temu zapobiec można użyć do estymacji trajektorii ruchurobota pomiaroacutew pochodzących z czujnika inercyjnego razem z odczytami z senso-roacutew odometrycznych Aby tego dokonać potrzebna jest fuzja odczytoacutew z roacuteżnychczujnikoacutew Istnieje wiele metod podejścia do tego tematu jednak jednym z częściejspotykanych w literaturze jest użycie rozszerzonego filtru Kalmana [18] [17] [4]

262 Korekcja odczytu siły z czujnika nadgarstkowego

Drugim przykładem zastosowania jednostki inercyjnej w robotyce jest korekcja od-czytu z czujnika siły [20] Nie jest to zastosowanie tak szeroko omawiane w literaturzejak nawigacja wspomagana inercyjnie opisana w sekcji 261 Pomimo niewielkiegozainteresowania jest to bardzo ciekawy temat badawczy głoacutewnie ze względu na chęćwyeliminowania bdquopikoacutewrdquo odczytoacutew siły wynikających z nagłej zmiany kierunku ru-chu ramienia bądź też drgnięcia wynikającego z niedoskonałości regulatora Do tegozastosowania konstruuje się obserwator siły oparty o filtr Kalmana [6]

14 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Masa odchylająca się pod wpływem siły bezwładności proacutebuje odkształcić piezorezy-story wmontowane w belkę na ktoacuterej jest zaczepiona Mierząc rezystancję wspomnia-nych piezorezystoroacutew można ustalić siłę działającą na masę oraz dzięki znajomościmasy wartość przyspieszenia ktoacuteremu poddawany jest układ Oczywiście wspoacutełcze-sne układy stosują zaawansowane techniki w celu zmniejszenia wpływu przyspieszeńw innych osiach niż badana oraz zabezpieczeniu przed uszkodzeniem wywołanymnadmiernymi przeciążeniami Jednak zasada działania pozostaje ta sama

Drugim rodzajem akcelerometroacutew MEMS są akcelerometry pojemnościowe Ak-celerometr pojemnościowy do swojego działania wymaga dwoacutech elementoacutew elek-trody stałej oraz elektrody połączonej z masą ktoacutera będzie poruszana siłą bezwład-ności Gdy masa zbliża się do elektrody stałej pojemność się zwiększa gdy natomiastsiła bezwładności działa w drugą stronę pojemność się zmniejsza Takie założeniapozwalają na pomiar przyspieszenia w akcelerometrze pojemnościowym

222 Żyroskopy MEMS

Obecnie produkowane żyroskopy MEMS to żyroskopy wibracyjne [3] Ich zasadadziałania jest nieco bardziej skomplikowana niż zasada działania akcelerometroacutewMEMS W układzie takim masę należy wprawić w wibracje wzdłuż jednej z osi (naprzykład x) leżącej na płaszczyźnie zawierającej dwie z nich (na przykład płaszczy-zna xy) W tej sytuacji pomiar możliwy będzie przy obrocie wokoacuteł trzeciej osi (z)Przy obrocie wokoacuteł osi Z czujnika siła Coriolisa przeniesie drgania z osi X na oś Ymierząc amplitudę tych drgań (na przykład za pomocą metody pojemnościowej jakw akcelerometrze pojemnościowym) można ustalić prędkość obrotu wokoacuteł danej osi

223 Żyroskopy światłowodowe

Działanie żyroskopoacutew światłowodowych oparte jest o efekt Sagnaca [12] jest toefekt polegający na przesunięciu w fazie dwuch rozszczepionych wiązek światła bie-gnących po pętli w przeciwnych kierunkach jeżeli układ się obraca Efekt ten jestspowodowany tym że w obracającym się układzie światło lecące w jedną stronę musipokonać dłuższą drogę niż światło lecące w stronę przeciwną Efekt ten stosuje sięw żyroskopach światłowodowych wielokrotnie zwiekszając drogę jaką musi pokonaćświatło poprzez zwiększenie ilości zwojoacutew światłowodu Żyroskopy takie mają bar-dzo wysoką czułość (nawet do jednej dziesięciotysięcznej stopnia na godzinę) orazbardzo niski dryft

23 Działanie magistrali Ethercat

EtherCAT jest standardem stworzonym przez firmę Beckhoff Automation Standardten jest jednak otwarty i powstają otwarte implementacje jego elementoacutew Choćsam standard swoje niższe warstwy opiera na Ethernecie (warstwę fizyczną orazczęść warstwy łącza danych) to wyższe z nich roacuteżnią się między sobą ze względuna zastosowanie na przykład protokołem warstwy aplikacji jest protokoacuteł Can OverEtherCAT opisany w podsekcji 231 W podsekcji 232 znajduje się wprowadzeniedo działania rozproszonego zegara magistrali EtherCAT

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 11

(a) Struktura ramki ethercat źroacutedło [5] (b) Struktura ramki danych źroacutedło [5]

Rysunek 21 Schematy przedstawiające strukturę ramki w komunikacji ethercat

231 COE

CANopen Over EtherCAT [5] jest protokołem warstwy aplikacji działającym w sieciEthercat jest to protokoacuteł komunikacyjny bazujący na systemie CAN Wykorzystanieimplementacji protokołu CoE sprawia że EtherCAT jest kompatybilny z urządze-niami obsługującymi CANopen Aby zrozumieć w jaki sposoacuteb datagramy CANopenprzekazywane są w protokole ethercat musimy przyjrzeć się strukturze jego ramkiTa (pokazana na rysunku 21(a)) zawiera czternastobajtowy nagłoacutewek ramki ether-cat dwubajtowe pole długości wiadomości wiadomość oraz czterobajtową sumękontrolną W polu wiadomości może znajdować się wiele datagramoacutew każdy z nichmoże być adresowany do innego urządzenia w magistrali ethercat Każdy datagramzawiera swoacutej nagłoacutewek ktoacutery adresuje go do danego urządzenia pole danych orazsumę kontrolną Skład pola danych pokazany na rysunku 21(b) a składa się onoz nagłoacutewka ktoacutery między innymi zawiera typ ktoacutery wskazuje na protokoacuteł warstwyaplikacji czyli na przykład CoE Następnie w ramce znajduje się komenda oraz od-powiadające jej dane

232 Synchronizacja zegara

Algorytm synchronizacji czasu w standardzie ethercat oparty jest o standard Pre-cision Time Protocol Jest to standard zaakceptowany przez IEEE o sygnaturzeIEEE 1588-2008 służący do submikrosekundowej synchronizacji czasu między urzą-dzeniami połączonymi w jedną sieć Więcej o działaniu samego protokołu możemyprzeczytać w publikacjach [15] [8] W protokole EtherCAT do synchronizacji zegarawyznaczany jest pierwsze urządzenie podrzędne na magistrali Ponieważ urządzeniapodrzędne magistrali EtherCAT wspierają synchronizacje sprzętową za pomocą pro-tokołu PTP możliwe jest odciążenie węzła głoacutewnego

24 Jednostki inercyjne ze złączami I2C oraz SPI

Szerokie spektrum tych urządzeń korzysta z interfejsoacutew I2C oraz SPI Taki spo-soacuteb podłączenia daje możliwość szybkiego przesyłu danych jednak ograniczeniemw takim przypadku jest odległość na jaka można prowadzić komunikację I2C niema zdefiniowanego maksymalnego zasięgu a maksymalną pojemność linii komuni-kacyjnej na poziomie 400 pF [16] co ogranicza zasięg komunikacji do pojedynczychmetroacutew w warunkach idealnych Magistrala SPI roacutewnież nie podaje maksymalnegozasięgu jednak w optymalnych warunkach jest to ok 2m

12 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Jednostki inercyjne ze złączami I2C oraz SPI są zazwyczaj tańszymi modelamiW większości są oferowane jako układy elektroniczne do montażu powierzchniowegoOto kilka przykładoacutew tego typu jednostek

bull LSM9DS0 - Jednostka inercyjna firmy ST electronics z interfejsem komunika-cyjnym SPII2C wyposażona w akcelerometr żyroskop i magnetometr Mak-symalne proacutebkowanie magnetometru to 100Hz akcelerometru 1600Hz żyro-skop ma maksymalną częstotliwość dostarczania danych 760Hz Jednostka do-starczana jest w obudowie LGA-24 o rozmiarach 4x4x1mm

bull ADIS16362 - Jednostka inercyjna firmy Analog devices jest to jednostka za-wierająca akcelerometr oraz żyroskop (oba troacutejosiowe) Charakteryzuje się onawysoką maksymalną częstotliwością proacutebkowania ponieważ dla obu czujnikoacutewwynosi ona 1200Hz Interfejs komunikacyjny stanowi złącze SPI Urządzeniedostarczane jest w obudowie w kształcie sześcianu 2x2 cm z miejscami naprzykręcenie jej do urządzenia W przeciwieństwie do poprzedniego urządze-nia jednostka posiada program kalibracji

25 Jednostki inercyjne ze złączami przemysłowymi

Inne z tych urządzeń komunikują się za pomocą dedykowanych standardoacutew prze-mysłowych ktoacutere charakteryzują się znacznie większą odpornością na zakłoacuteceniaa także na ciężkie warunki pracy Warto w tym miejscu zaznaczyć że jak dotąd narynku nie ma jednostki inercyjnej ktoacutera komunikowała by się za pomocą magistraliEthercat

Jednostki inercyjne omawiane w tym podrozdziale są urządzeniami ze złączamiprzemysłowymi przez co rozumiemy że złącza te charakteryzują się dużą nieza-wodnością Niestety modele zapewniające taki komfort są zazwyczaj znacznie więk-sze oraz droższe

bull MTi 100-series - Jednostki inercyjne firmy Xsens jest jednostką zawierającąakcelerometr i żyroskop Maksymalna częstotliwość dostarczania danych to2000Hz Jednostka ta komunikuje się za pomocą interfejsoacutew RS232 RS485422 UART USB Urządzenie to jest dostarczone w wodoodpornej obudo-wie mocowanej do urządzenia na śruby z przemysłowym złączem służącym dokomunikacji oraz zasilania Producent deklaruje że urządzenie posiada żyro-skop filtrujący wpływ wibracji

bull KVH 1750 IMU - Jednostka inercyjna firmy KVH jest urządzeniem posiada-jącym żyroskop światłowodowy oraz 3 oddzielne akcelerometry MEMS Inter-fejs komunikacyjny tego modułu to RS-422 Obudowa tej jednostki to waleco średnicy 89 mm i wysokości 74 mm przytwierdzany za pomocą sześciu śrubUrządzenie może produkować dane z częstotliwością do 1000Hz Charaktery-zuje się ono bardzo niskim dryftem żyroskopu Dryft ten typowo musi byćkorygowany za pomocą magnetometru

bull 6DF-1N2-C2-HWL - Jednostka inercyjna firmy Honeywell wyposażona jestw akcelerometr oraz żyroskop (oba troacutejosiowe) Interfejs komunikacyjny tegourządzenia to CAN Maksymalna częstotliwość dostarczania danych to 100Hz

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 13

26 Możliwe zastosowania proponowanego układu

Możliwe zastosowania jednostek inercyjnych w robotyce pojawiają się tam gdziewystępuje ruch inny od jednostajnego prostoliniowego W tym rozdziale rozważymydwa zastosowania w ktoacuterych proponowany układ może pokazać swoje zalety wy-nikające ze sposobu komunikacji W sekcji 261 omoacutewimy przypadek zastosowaniajednostki inercyjnej do wspomagania procesu nawigacji robota mobilnego o napę-dzie roacuteżnicowym Sekcja 262 dotyczyć będzie przypadku użycia takiego urządzeniado korekty odczytu siły z czujnika umieszczonego w nadgarstku manipulatora

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym

W tej sekcji przedstawiony zostanie przykład nawigacji robota mobilnego o napę-dzie roacuteżnicowym Przykładem takiego robota może być robot o konstrukcji takiejjak baza mobilna elektron [10] czyli robot posiadający wiele koacuteł po jednej stro-nie co generuje problemy z odtworzeniem trajektorii ruchu robota przez odczytyz czujnikoacutew odometrycznych Dzieje się tak przez to że budowa robota wprowa-dza poślizgi podczas skrętoacutew Robot taki ze względu na roacuteżne czynniki zewnętrznenp typ podłoża nieroacutewności podłoża skręca w okoacuteł trudnej do ustalenia osi obrotuktoacutera w dodatku może być zmienna (np gdy jedno koło jest na innym typie podłożaniż wszystkie inne) Aby temu zapobiec można użyć do estymacji trajektorii ruchurobota pomiaroacutew pochodzących z czujnika inercyjnego razem z odczytami z senso-roacutew odometrycznych Aby tego dokonać potrzebna jest fuzja odczytoacutew z roacuteżnychczujnikoacutew Istnieje wiele metod podejścia do tego tematu jednak jednym z częściejspotykanych w literaturze jest użycie rozszerzonego filtru Kalmana [18] [17] [4]

262 Korekcja odczytu siły z czujnika nadgarstkowego

Drugim przykładem zastosowania jednostki inercyjnej w robotyce jest korekcja od-czytu z czujnika siły [20] Nie jest to zastosowanie tak szeroko omawiane w literaturzejak nawigacja wspomagana inercyjnie opisana w sekcji 261 Pomimo niewielkiegozainteresowania jest to bardzo ciekawy temat badawczy głoacutewnie ze względu na chęćwyeliminowania bdquopikoacutewrdquo odczytoacutew siły wynikających z nagłej zmiany kierunku ru-chu ramienia bądź też drgnięcia wynikającego z niedoskonałości regulatora Do tegozastosowania konstruuje się obserwator siły oparty o filtr Kalmana [6]

14 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

(a) Struktura ramki ethercat źroacutedło [5] (b) Struktura ramki danych źroacutedło [5]

Rysunek 21 Schematy przedstawiające strukturę ramki w komunikacji ethercat

231 COE

CANopen Over EtherCAT [5] jest protokołem warstwy aplikacji działającym w sieciEthercat jest to protokoacuteł komunikacyjny bazujący na systemie CAN Wykorzystanieimplementacji protokołu CoE sprawia że EtherCAT jest kompatybilny z urządze-niami obsługującymi CANopen Aby zrozumieć w jaki sposoacuteb datagramy CANopenprzekazywane są w protokole ethercat musimy przyjrzeć się strukturze jego ramkiTa (pokazana na rysunku 21(a)) zawiera czternastobajtowy nagłoacutewek ramki ether-cat dwubajtowe pole długości wiadomości wiadomość oraz czterobajtową sumękontrolną W polu wiadomości może znajdować się wiele datagramoacutew każdy z nichmoże być adresowany do innego urządzenia w magistrali ethercat Każdy datagramzawiera swoacutej nagłoacutewek ktoacutery adresuje go do danego urządzenia pole danych orazsumę kontrolną Skład pola danych pokazany na rysunku 21(b) a składa się onoz nagłoacutewka ktoacutery między innymi zawiera typ ktoacutery wskazuje na protokoacuteł warstwyaplikacji czyli na przykład CoE Następnie w ramce znajduje się komenda oraz od-powiadające jej dane

232 Synchronizacja zegara

Algorytm synchronizacji czasu w standardzie ethercat oparty jest o standard Pre-cision Time Protocol Jest to standard zaakceptowany przez IEEE o sygnaturzeIEEE 1588-2008 służący do submikrosekundowej synchronizacji czasu między urzą-dzeniami połączonymi w jedną sieć Więcej o działaniu samego protokołu możemyprzeczytać w publikacjach [15] [8] W protokole EtherCAT do synchronizacji zegarawyznaczany jest pierwsze urządzenie podrzędne na magistrali Ponieważ urządzeniapodrzędne magistrali EtherCAT wspierają synchronizacje sprzętową za pomocą pro-tokołu PTP możliwe jest odciążenie węzła głoacutewnego

24 Jednostki inercyjne ze złączami I2C oraz SPI

Szerokie spektrum tych urządzeń korzysta z interfejsoacutew I2C oraz SPI Taki spo-soacuteb podłączenia daje możliwość szybkiego przesyłu danych jednak ograniczeniemw takim przypadku jest odległość na jaka można prowadzić komunikację I2C niema zdefiniowanego maksymalnego zasięgu a maksymalną pojemność linii komuni-kacyjnej na poziomie 400 pF [16] co ogranicza zasięg komunikacji do pojedynczychmetroacutew w warunkach idealnych Magistrala SPI roacutewnież nie podaje maksymalnegozasięgu jednak w optymalnych warunkach jest to ok 2m

12 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Jednostki inercyjne ze złączami I2C oraz SPI są zazwyczaj tańszymi modelamiW większości są oferowane jako układy elektroniczne do montażu powierzchniowegoOto kilka przykładoacutew tego typu jednostek

bull LSM9DS0 - Jednostka inercyjna firmy ST electronics z interfejsem komunika-cyjnym SPII2C wyposażona w akcelerometr żyroskop i magnetometr Mak-symalne proacutebkowanie magnetometru to 100Hz akcelerometru 1600Hz żyro-skop ma maksymalną częstotliwość dostarczania danych 760Hz Jednostka do-starczana jest w obudowie LGA-24 o rozmiarach 4x4x1mm

bull ADIS16362 - Jednostka inercyjna firmy Analog devices jest to jednostka za-wierająca akcelerometr oraz żyroskop (oba troacutejosiowe) Charakteryzuje się onawysoką maksymalną częstotliwością proacutebkowania ponieważ dla obu czujnikoacutewwynosi ona 1200Hz Interfejs komunikacyjny stanowi złącze SPI Urządzeniedostarczane jest w obudowie w kształcie sześcianu 2x2 cm z miejscami naprzykręcenie jej do urządzenia W przeciwieństwie do poprzedniego urządze-nia jednostka posiada program kalibracji

25 Jednostki inercyjne ze złączami przemysłowymi

Inne z tych urządzeń komunikują się za pomocą dedykowanych standardoacutew prze-mysłowych ktoacutere charakteryzują się znacznie większą odpornością na zakłoacuteceniaa także na ciężkie warunki pracy Warto w tym miejscu zaznaczyć że jak dotąd narynku nie ma jednostki inercyjnej ktoacutera komunikowała by się za pomocą magistraliEthercat

Jednostki inercyjne omawiane w tym podrozdziale są urządzeniami ze złączamiprzemysłowymi przez co rozumiemy że złącza te charakteryzują się dużą nieza-wodnością Niestety modele zapewniające taki komfort są zazwyczaj znacznie więk-sze oraz droższe

bull MTi 100-series - Jednostki inercyjne firmy Xsens jest jednostką zawierającąakcelerometr i żyroskop Maksymalna częstotliwość dostarczania danych to2000Hz Jednostka ta komunikuje się za pomocą interfejsoacutew RS232 RS485422 UART USB Urządzenie to jest dostarczone w wodoodpornej obudo-wie mocowanej do urządzenia na śruby z przemysłowym złączem służącym dokomunikacji oraz zasilania Producent deklaruje że urządzenie posiada żyro-skop filtrujący wpływ wibracji

bull KVH 1750 IMU - Jednostka inercyjna firmy KVH jest urządzeniem posiada-jącym żyroskop światłowodowy oraz 3 oddzielne akcelerometry MEMS Inter-fejs komunikacyjny tego modułu to RS-422 Obudowa tej jednostki to waleco średnicy 89 mm i wysokości 74 mm przytwierdzany za pomocą sześciu śrubUrządzenie może produkować dane z częstotliwością do 1000Hz Charaktery-zuje się ono bardzo niskim dryftem żyroskopu Dryft ten typowo musi byćkorygowany za pomocą magnetometru

bull 6DF-1N2-C2-HWL - Jednostka inercyjna firmy Honeywell wyposażona jestw akcelerometr oraz żyroskop (oba troacutejosiowe) Interfejs komunikacyjny tegourządzenia to CAN Maksymalna częstotliwość dostarczania danych to 100Hz

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 13

26 Możliwe zastosowania proponowanego układu

Możliwe zastosowania jednostek inercyjnych w robotyce pojawiają się tam gdziewystępuje ruch inny od jednostajnego prostoliniowego W tym rozdziale rozważymydwa zastosowania w ktoacuterych proponowany układ może pokazać swoje zalety wy-nikające ze sposobu komunikacji W sekcji 261 omoacutewimy przypadek zastosowaniajednostki inercyjnej do wspomagania procesu nawigacji robota mobilnego o napę-dzie roacuteżnicowym Sekcja 262 dotyczyć będzie przypadku użycia takiego urządzeniado korekty odczytu siły z czujnika umieszczonego w nadgarstku manipulatora

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym

W tej sekcji przedstawiony zostanie przykład nawigacji robota mobilnego o napę-dzie roacuteżnicowym Przykładem takiego robota może być robot o konstrukcji takiejjak baza mobilna elektron [10] czyli robot posiadający wiele koacuteł po jednej stro-nie co generuje problemy z odtworzeniem trajektorii ruchu robota przez odczytyz czujnikoacutew odometrycznych Dzieje się tak przez to że budowa robota wprowa-dza poślizgi podczas skrętoacutew Robot taki ze względu na roacuteżne czynniki zewnętrznenp typ podłoża nieroacutewności podłoża skręca w okoacuteł trudnej do ustalenia osi obrotuktoacutera w dodatku może być zmienna (np gdy jedno koło jest na innym typie podłożaniż wszystkie inne) Aby temu zapobiec można użyć do estymacji trajektorii ruchurobota pomiaroacutew pochodzących z czujnika inercyjnego razem z odczytami z senso-roacutew odometrycznych Aby tego dokonać potrzebna jest fuzja odczytoacutew z roacuteżnychczujnikoacutew Istnieje wiele metod podejścia do tego tematu jednak jednym z częściejspotykanych w literaturze jest użycie rozszerzonego filtru Kalmana [18] [17] [4]

262 Korekcja odczytu siły z czujnika nadgarstkowego

Drugim przykładem zastosowania jednostki inercyjnej w robotyce jest korekcja od-czytu z czujnika siły [20] Nie jest to zastosowanie tak szeroko omawiane w literaturzejak nawigacja wspomagana inercyjnie opisana w sekcji 261 Pomimo niewielkiegozainteresowania jest to bardzo ciekawy temat badawczy głoacutewnie ze względu na chęćwyeliminowania bdquopikoacutewrdquo odczytoacutew siły wynikających z nagłej zmiany kierunku ru-chu ramienia bądź też drgnięcia wynikającego z niedoskonałości regulatora Do tegozastosowania konstruuje się obserwator siły oparty o filtr Kalmana [6]

14 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Jednostki inercyjne ze złączami I2C oraz SPI są zazwyczaj tańszymi modelamiW większości są oferowane jako układy elektroniczne do montażu powierzchniowegoOto kilka przykładoacutew tego typu jednostek

bull LSM9DS0 - Jednostka inercyjna firmy ST electronics z interfejsem komunika-cyjnym SPII2C wyposażona w akcelerometr żyroskop i magnetometr Mak-symalne proacutebkowanie magnetometru to 100Hz akcelerometru 1600Hz żyro-skop ma maksymalną częstotliwość dostarczania danych 760Hz Jednostka do-starczana jest w obudowie LGA-24 o rozmiarach 4x4x1mm

bull ADIS16362 - Jednostka inercyjna firmy Analog devices jest to jednostka za-wierająca akcelerometr oraz żyroskop (oba troacutejosiowe) Charakteryzuje się onawysoką maksymalną częstotliwością proacutebkowania ponieważ dla obu czujnikoacutewwynosi ona 1200Hz Interfejs komunikacyjny stanowi złącze SPI Urządzeniedostarczane jest w obudowie w kształcie sześcianu 2x2 cm z miejscami naprzykręcenie jej do urządzenia W przeciwieństwie do poprzedniego urządze-nia jednostka posiada program kalibracji

25 Jednostki inercyjne ze złączami przemysłowymi

Inne z tych urządzeń komunikują się za pomocą dedykowanych standardoacutew prze-mysłowych ktoacutere charakteryzują się znacznie większą odpornością na zakłoacuteceniaa także na ciężkie warunki pracy Warto w tym miejscu zaznaczyć że jak dotąd narynku nie ma jednostki inercyjnej ktoacutera komunikowała by się za pomocą magistraliEthercat

Jednostki inercyjne omawiane w tym podrozdziale są urządzeniami ze złączamiprzemysłowymi przez co rozumiemy że złącza te charakteryzują się dużą nieza-wodnością Niestety modele zapewniające taki komfort są zazwyczaj znacznie więk-sze oraz droższe

bull MTi 100-series - Jednostki inercyjne firmy Xsens jest jednostką zawierającąakcelerometr i żyroskop Maksymalna częstotliwość dostarczania danych to2000Hz Jednostka ta komunikuje się za pomocą interfejsoacutew RS232 RS485422 UART USB Urządzenie to jest dostarczone w wodoodpornej obudo-wie mocowanej do urządzenia na śruby z przemysłowym złączem służącym dokomunikacji oraz zasilania Producent deklaruje że urządzenie posiada żyro-skop filtrujący wpływ wibracji

bull KVH 1750 IMU - Jednostka inercyjna firmy KVH jest urządzeniem posiada-jącym żyroskop światłowodowy oraz 3 oddzielne akcelerometry MEMS Inter-fejs komunikacyjny tego modułu to RS-422 Obudowa tej jednostki to waleco średnicy 89 mm i wysokości 74 mm przytwierdzany za pomocą sześciu śrubUrządzenie może produkować dane z częstotliwością do 1000Hz Charaktery-zuje się ono bardzo niskim dryftem żyroskopu Dryft ten typowo musi byćkorygowany za pomocą magnetometru

bull 6DF-1N2-C2-HWL - Jednostka inercyjna firmy Honeywell wyposażona jestw akcelerometr oraz żyroskop (oba troacutejosiowe) Interfejs komunikacyjny tegourządzenia to CAN Maksymalna częstotliwość dostarczania danych to 100Hz

Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu 13

26 Możliwe zastosowania proponowanego układu

Możliwe zastosowania jednostek inercyjnych w robotyce pojawiają się tam gdziewystępuje ruch inny od jednostajnego prostoliniowego W tym rozdziale rozważymydwa zastosowania w ktoacuterych proponowany układ może pokazać swoje zalety wy-nikające ze sposobu komunikacji W sekcji 261 omoacutewimy przypadek zastosowaniajednostki inercyjnej do wspomagania procesu nawigacji robota mobilnego o napę-dzie roacuteżnicowym Sekcja 262 dotyczyć będzie przypadku użycia takiego urządzeniado korekty odczytu siły z czujnika umieszczonego w nadgarstku manipulatora

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym

W tej sekcji przedstawiony zostanie przykład nawigacji robota mobilnego o napę-dzie roacuteżnicowym Przykładem takiego robota może być robot o konstrukcji takiejjak baza mobilna elektron [10] czyli robot posiadający wiele koacuteł po jednej stro-nie co generuje problemy z odtworzeniem trajektorii ruchu robota przez odczytyz czujnikoacutew odometrycznych Dzieje się tak przez to że budowa robota wprowa-dza poślizgi podczas skrętoacutew Robot taki ze względu na roacuteżne czynniki zewnętrznenp typ podłoża nieroacutewności podłoża skręca w okoacuteł trudnej do ustalenia osi obrotuktoacutera w dodatku może być zmienna (np gdy jedno koło jest na innym typie podłożaniż wszystkie inne) Aby temu zapobiec można użyć do estymacji trajektorii ruchurobota pomiaroacutew pochodzących z czujnika inercyjnego razem z odczytami z senso-roacutew odometrycznych Aby tego dokonać potrzebna jest fuzja odczytoacutew z roacuteżnychczujnikoacutew Istnieje wiele metod podejścia do tego tematu jednak jednym z częściejspotykanych w literaturze jest użycie rozszerzonego filtru Kalmana [18] [17] [4]

262 Korekcja odczytu siły z czujnika nadgarstkowego

Drugim przykładem zastosowania jednostki inercyjnej w robotyce jest korekcja od-czytu z czujnika siły [20] Nie jest to zastosowanie tak szeroko omawiane w literaturzejak nawigacja wspomagana inercyjnie opisana w sekcji 261 Pomimo niewielkiegozainteresowania jest to bardzo ciekawy temat badawczy głoacutewnie ze względu na chęćwyeliminowania bdquopikoacutewrdquo odczytoacutew siły wynikających z nagłej zmiany kierunku ru-chu ramienia bądź też drgnięcia wynikającego z niedoskonałości regulatora Do tegozastosowania konstruuje się obserwator siły oparty o filtr Kalmana [6]

14 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

26 Możliwe zastosowania proponowanego układu

Możliwe zastosowania jednostek inercyjnych w robotyce pojawiają się tam gdziewystępuje ruch inny od jednostajnego prostoliniowego W tym rozdziale rozważymydwa zastosowania w ktoacuterych proponowany układ może pokazać swoje zalety wy-nikające ze sposobu komunikacji W sekcji 261 omoacutewimy przypadek zastosowaniajednostki inercyjnej do wspomagania procesu nawigacji robota mobilnego o napę-dzie roacuteżnicowym Sekcja 262 dotyczyć będzie przypadku użycia takiego urządzeniado korekty odczytu siły z czujnika umieszczonego w nadgarstku manipulatora

261 Nawigacja Robota Mobilnego o napędzie roacuteżnicowym

W tej sekcji przedstawiony zostanie przykład nawigacji robota mobilnego o napę-dzie roacuteżnicowym Przykładem takiego robota może być robot o konstrukcji takiejjak baza mobilna elektron [10] czyli robot posiadający wiele koacuteł po jednej stro-nie co generuje problemy z odtworzeniem trajektorii ruchu robota przez odczytyz czujnikoacutew odometrycznych Dzieje się tak przez to że budowa robota wprowa-dza poślizgi podczas skrętoacutew Robot taki ze względu na roacuteżne czynniki zewnętrznenp typ podłoża nieroacutewności podłoża skręca w okoacuteł trudnej do ustalenia osi obrotuktoacutera w dodatku może być zmienna (np gdy jedno koło jest na innym typie podłożaniż wszystkie inne) Aby temu zapobiec można użyć do estymacji trajektorii ruchurobota pomiaroacutew pochodzących z czujnika inercyjnego razem z odczytami z senso-roacutew odometrycznych Aby tego dokonać potrzebna jest fuzja odczytoacutew z roacuteżnychczujnikoacutew Istnieje wiele metod podejścia do tego tematu jednak jednym z częściejspotykanych w literaturze jest użycie rozszerzonego filtru Kalmana [18] [17] [4]

262 Korekcja odczytu siły z czujnika nadgarstkowego

Drugim przykładem zastosowania jednostki inercyjnej w robotyce jest korekcja od-czytu z czujnika siły [20] Nie jest to zastosowanie tak szeroko omawiane w literaturzejak nawigacja wspomagana inercyjnie opisana w sekcji 261 Pomimo niewielkiegozainteresowania jest to bardzo ciekawy temat badawczy głoacutewnie ze względu na chęćwyeliminowania bdquopikoacutewrdquo odczytoacutew siły wynikających z nagłej zmiany kierunku ru-chu ramienia bądź też drgnięcia wynikającego z niedoskonałości regulatora Do tegozastosowania konstruuje się obserwator siły oparty o filtr Kalmana [6]

14 Rozdział 2 Wstęp teoretyczny przegląd dostępnych rozwiązań oraz możliwe zastosowania układu

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Rozdział 3

Opis budowanego układu

W ramach tej pracy powstał układ elektroniczny umożliwiający komunikację mię-dzy urządzeniem jednostki inercyjnej a magistralą ethercat Zaroacutewno projekt płytekdrukowanych dwoacutech rozłącznych modułoacutew opisanych w sekcji 31 jak i projekt sche-matyczny skutkujący układem sygnałoacutew na każdym ze złącz opisanych w sekcji 32pochłonęły wiele pracy Pogodzenie wszystkich wymagań zaroacutewno mechanicznychjak i elektrycznych czy tych dotyczących możliwości ponownego wykorzystania mo-dułu głoacutewnego stanowiły dla autora wyzwanie

31 Opis Układu

Opracowany układ składa się z dwoacutech niezależnych modułoacutew połączonych przez złą-cza o wysokiej gęstości Moduł głoacutewny opisany jest w sekcji 311 a moduł jednostkiinercyjnej w sekcji 312 Konstrukcja taka pozwala na uniwersalne wykorzystaniemodułu głoacutewnego poprzez wymianę zamontowanych modułoacutew np zamiast modułujednostki inercyjnej można zaprojektować sterownik silnikoacutew lub inne urządzeniewymagające komunikacji za pośrednictwem EtherCATrsquoa Do modułu głoacutewnego pod-łączony jest układ odpowiadający za komunikację z magistralą EtherCAT opisanyjest on w sekcji 313 Moduł głoacutewny przedstawiony jest na rysunku 31(a) a modułjednostki inercyjnej na rysunku 31(b)

(a) płytka modułu głoacutewnego (b) płytka modułu jednostki inercyjnej

Rysunek 31 Wizualizacja płytek

Rozdział 3 Opis budowanego układu 15

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

311 Moduł głoacutewny

Moduł głoacutewny schematycznie przedstawiony na rysunku 32 oparty jest o procesorTM4C129EN Odpowiada on za komunikację z urządzeniem obsługującym magi-stralę EtherCAT W tym przypadku rolę tę odgrywa układ Anybus B40 EtherCAT313 Mikrokontroler TM4C129EN został wybrany z powodu posiadania interfejsuEPI (External peripherial interface) Jest to roacutewnoległy interfejs używany do komu-nikacji z urządzeniem Anybus B40 może on pracować z częstotliwością 30 MHz coprzy ośmiobitowej szerokości magistrali daje transfer na poziomie 30 MBs Jegopodłączenie do mikrokontrolera opisane jest w sekcji 325 Dodatkowo na złączauniwersalne wyprowadzone są wszystkie inne porty mikrokontrolera oraz zasilaniei masa układu

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

Rysunek 32 Rysunek płytki głoacutewnej wraz z podanymi wymiarami

312 Moduł jednostki inercyjnej (IMU)

Moduł jednostki inercyjnej zawiera samą jednostkę inercyjną ADIS16362 podłą-czoną do złącza płytka-płytka Jednostka inercyjna komunikuje się z procesorem zapomocą interfejsu SPI IMU jest zintegrowanym urządzeniem cyfrowym ktoacutere po-siada funkcje sprawdzania swojego stanu oraz autokalibracji Opis połączeń znajdujesię w tablicy 31

Tablica 31 Opis podłączeń do jednostki inercyjnejPin jednostki inercyjnej Pin procesora Opis

DIO3 NC ndashSCLK PA2 SPI ClockDIN PA4 SPI data inputDIO1 NC ndashDIO2 NC ndashDIO4 PB2 Sync Clock InputDOUT PA5 SPI data outputCS PA3 SPI chip select

RESET PB1 not reset

16 Rozdział 3 Opis budowanego układu

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

313 Anybus M40 EtherCAT

Anybus B40 EtherCAT przedstawiony na zdjęciu 33(a) jest modułem pozwalają-cym na szybką komunikację z masterem EtherCAT Układ ten posiada dwa złącza(jedno do połączenia interfejsu sieciowego oraz jedno do komunikacji z mikrokon-trolerem) Są one przedstawione na rysunku 33(b)

(a) Zdjęcie modułu odpowiadającego za komu-nikację EtherCAT

(b) Rysunek złączy modułu Anybus B40 źroacute-dło [2]

Rysunek 33 Wizualizacja płytek

32 Opis złącz

Urządzenie zawiera zestaw złącz o roacuteżnym przeznaczeniu Złącza międzypłytkowemają zapewnić komunikację między modułami (głoacutewnym oraz jednostki inercyjnej)Są to dwa podłużne złącza zawierające po 40 stykoacutew ich opis znajduje się w sek-cji 321 Złącze zasilania umieszczone jest na płytce modułu jednostki inercyjnejOpis tego złącza znajduje się w sekcji 322 Następne złącza znajdujące się na płytcemodułu głoacutewnego to złącza służące do podłączenia kabla ethernetowego Umożliwiato komunikację za pomocą EtherCATrsquoa Opis tych złącz znajduje się w sekcji 323Złącze programowania to standardowe złącze jtag o rastrze 127 mm znajduje sięono na płytce modułu głoacutewnego i jest opisane w sekcji 324 Ostatnim opisywanymzłączem jest to łączące procesor z układem Anybus B40 Opis tego złącza znajdujesię w sekcji 325

321 Złącze międzypłytkowe

Złącze międzypłytkowe służy do połączenia obu modułoacutew Są to dwa gniazda umiesz-czone na płycie modułu głoacutewnego z czego każde posiada piny numerowane tak jakna rysunku 34 Zastosowane złącza to TE CONNECTIVITY AMP 5179030-1Na module znajdziemy dwa tego typu złącza numerowane jako J6 oraz J7 jak narysunku 32

Rozdział 3 Opis budowanego układu 17

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Rysunek 34 Standard numeracji na złączu międzypłytkowym

Opis wyprowadzeń złącz przedstawiony jest w tabeli 32 Wszystkie nazwy pinoacutewprzedstawione są tak jak w mikrokontrolerze TM4C1294NCPDT [9] firmy TexasInstruments ktoacuterego ten moduł używa

Tablica 32 Opis podłączeń w złączu międzypłytkowymJ6 J7 J6 J7

numer pinu nazwa pinu nazwa pinu numer pinu nazwa pinu nazwa pinuzłącza złącza

1 PL6 3v3 21 PD2 5V2 PB1 GND 22 PD1 5V3 PC0 GND 23 PD7 PA24 PP3 PL7 24 PD6 PH35 PP5 PB2 25 PD5 PH26 PN2 PM4 26 PD4 PA37 PN3 PM5 27 PE5 PA48 PN5 PM6 28 PE4 PF19 PH1 PK5 29 PB4 PF310 PH0 PK7 30 PB5 PG311 PJ1 PQ5 31 PP0 PG412 PP1 PG6 32 PJ0 PG713 PQ3 PG5 33 PN4 PQ614 PE0 PG2 34 PN1 PM715 PE1 PF4 35 PN0 PK616 PE2 PF2 36 PP4 PK417 PE3 PF0 37 PQ4 PL518 PQ2 PA5 38 PC1 GND19 PD0 5V 39 PC2 GND20 PD3 5V 40 PC3 3v3

322 Złącze zasilania

Złącze zasilania umieszczone na płytce jednostki inercyjnej jest złączem typu micro-fit 30 Numeracja połączeń przedstawiona jest na rysunku 35 a opis połączeń w ta-beli 34 Układ zasilany jest za pomocą napięcia 5V plusmn5 źroacutedło napięcia powinno

18 Rozdział 3 Opis budowanego układu

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

mieć wydajność na poziomie co najmniej 11A Taka wydajność zapewni działaniezestawu obu opisanych modułoacutew Sam moduł głoacutewny wymaga 1A prądu

1112

1314

Rysunek 35 Schemat złącza zasilania

Tablica 33 Opis połączeń złącza zasilaniaPin Nazwa Opis1 +5 Napięcie zasilające +5V plusmn52 NC3 NC4 GND Wspoacutelna masa

323 Złącze interfejsu EtherCAT

Na płytce modułu głoacutewnego znajdują się dwa złącza do podłączania przewodoacutewethernetowych etykietowanych jako J4 i J5 jak na rysunku 36(b) Numeracja pinoacutewzobrazowana jest na rysunku 36(a) Złącze J4 jest złączem wyjściowym a złącze J5złączem wejściowym Oznacza to że do gniazda J5 należy podłączyć interfejs komu-nikacyjny urządzenia nadrzędnego lub złącza wyjściowego poprzedniego urządzeniapodrzędnego

(a) numeracja pinoacutew

A

A

B

B

C

C

D

D

E

E

F

F

G

G

H

H

1 1

2 2

3 3

4 4

5 5

6 6

B1

J5J4

R77

R66

R55

R1

R2

R5

R6

R7

C66

C55

C44

C3

C4

C7

Cld

o

CV1

CV2

CV3

CV4CV5CV6CV7

CV

8

CV9

CV10

CV11

CV12

CV

13

R11

R12

R13R14

DL3

DL4

NONAME1

J2

View from Top side (Scale = 2)

X1

U1

R20

R21

R33R34

R35

Rp

g

Rp

u

Rs

Cx2

Cx1

C1C2

C5C6

C21

Cin

Co

ut

CVref1

CVref2

R15

R16

R17

R18

U2

DL1 DL2

1

T2 T1

View from Bottom side (Scale = 2)

40m

m

43mm

4mm

14m

m

484

4mm

5294mm

195

5mm

49mm51mm

Oslash3mm

Oslash08mm

Oslash12mm

Oslash3mm202

mm

Oslash12mm

Oslash08mm

33mm

A3

UNLESS OTHERWISE SPECIFIEDDIMENSIONS ARE IN MILLIMETERSSURFACE FINISHTOLERANCES LINEAR ANGULAR

FINISH DEBURR AND BREAK SHARP EDGES

DRAW

CHKD

APPVD

MFG

QA

NAME SIGNATURE DATA

MATERIAL

TITLE

DO NOT SCALE DRAWING REVISION

DWG N0

WEIGHT SCALE 11 SHEET 1 OF 2

J6

J7

J4J5

J8

J9

(b) położenie złącz na płytce

Rysunek 36 Numeracja pinoacutew oraz położenie złącz na płytce

Rozdział 3 Opis budowanego układu 19

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Tablica 34 Opis połączeń złącza EtherCATPin Nazwa Opis1 Tx+ Transmit positive2 Tx- Transmit negative3 Rx+ Receive positive4 Rx- Receive negative5 Ekran Ekran przewodu

324 Złącze Jtag

Złącze Jtag znajduje się na płytce modułu głoacutewnego jest ono oznaczone jako J2 na32 Jest to złącze typu Cortex Debug Connector Standard ten jest zdefiniowanyw dokumencie [11]

Rysunek 37 Schemat złącza jtag

325 Złącze Anybus B40-CPU

Podłączenie jednostki obsługującej protokoacuteł EtherCAT do mikrokontorlera odbywasię za pomocą złącza hosta tego układu (J8 na rysunku 32) Numeracja tego złączapokazana jest na rysunku 38 a sygnały podłączone do mikrokontrolera opisane sąw tabeli 35

Rysunek 38 Numeracja złącza hosta źroacutedło [2]

20 Rozdział 3 Opis budowanego układu

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Tablica 35 Opis połączeń złącza EtherCATnumer pinu złącza Nazwa pinu B40 Nazwa pinu CPU

03 RESET PA004 A0 PA605 A1 PA706 A2 PG107 A3 PG009 A4 PM310 A5 PM211 A6 PM112 A7 PM013 A8 PL015 A9 PL116 A10 PL217 A11 PL319 A12 PQ020 A13 PQ121 D7 PK722 D6 PK623 D5 PK525 D4 PK426 D3 PK327 D2 PK229 D1 PK131 D0 PK036 CS PL437 WEWELCT PP238 IRQPA PB039 OE PB354 MI0SYNC PA1

Rozdział 3 Opis budowanego układu 21

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

22 Rozdział 3 Opis budowanego układu

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Rozdział 4

Opis oprogramowania

W ramach niniejszej pracy zostały wytworzone dwa programy Pierwszy z nich jestto firmware wgrywany na mikrokontroler Oprogramowanie to opisane w sekcji 42jest odpowiedzialne za interfejsowanie jednostki inercyjnej z kontrolerem magistraliethercat Drugą częścią oprogramowania stworzoną w ramach pracy jest sterow-nik włączony w strukturę OROCOS Jego działanie opisane w sekcji 43 polega naudostępnieniu pomiaroacutew z jednostki inercyjnej do użytku zaroacutewno wewnątrz opro-gramowania uruchomionego w reżimie czasu rzeczywistego pod kontrolą OROCOSjak i poza nim w strukturze ROS

41 Protokoły komunikacyjne

W tym rozdziale omoacutewione zostaną protokoły komunikacyjne dzięki ktoacuterym reali-zowana jest komunikacja z jednostką nadrzędną stworzonym urządzeniem Najpierwopisane zostaną założenia standardu EtherCAT Miały one niebagatelny wpływ nawyboacuter sposobu komunikacji a te najistotniejsze opisane są w sekcji 411 Natomiastprotokoacuteł Can over EtherCAT został wybrany ze względu na łatwość jego użytkowa-nia Jego opis znajduje się w sekcji 412

411 Standard EtherCAT

Głoacutewnym założeniem magistrali EtherCAT było osiągnięcie kroacutetkich cykli komuni-kacyjnych (lt 100micros) oraz małych zmian opoacuteźnienia (lt1 micros) aby osiągnąć dokładnąsynchronizację urządzeń podłączonych do sieci Urządzenia w magistrali EtherCATdzielimy na urządzenia podrzędne oraz nadrzędne Urządzenia podrzędne połączoneszeregowo kolejno odbierają pakiet i w locie uzupełniają żądane wartości Następniepaczka przekazywana jest od urządzenia do urządzenia po czym wraca do węzłanadrzędnego

412 Can over EtherCAT

CAN over EtherCAT jest protokołem opartym o mechanizmy zdefiniowane w stan-dardzie CANopen Rcopy (EN 50325-4) Pozwala on na mapowanie elementoacutew danychz urządzeń podrzędnych do obiektoacutew na urządzeniu nadrzędnym Istnieją dwa typyobiektoacutew PDO (Proces Data Object) oraz SDO (Service Data Object) pierwszesłużą do przesyłania danych w czasie rzeczywistym między urządzeniami podrzęd-nymi a nadrzędnym (np przesyłanie pomiaroacutew jednostki inercyjnej) drugi służy doustawień konfiguracyjnych układu W układzie dostępne jest 6 obiektoacutew PDO

Rozdział 4 Opis oprogramowania 23

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

42 Oprogramowanie MCU

Oprogramowanie MCU wytworzone w ramach niniejszego projektu służy do pośred-niczenia między układem jednostki inercyjnej a modułem Anybus ktoacutery odpowiadaza sprzętową obsługę magistrali ethercat Oprogramowanie pozwalające obsłużyćmaszynę stanoacutew zawartą w module Anybus zostało dostarczone przez producentamodułu i jest opisane w podsekcji 422 Wykonana w tej pracy została więc ta częśćoprogramowania ktoacutera odpowiedzialna jest za komunikację z modułami na pozio-mie sprzętu i jest omoacutewiona w podsekcji 421 Zaimplementowany został roacutewnieżmechanizm obsługi jednostki inercyjnej i mechanizm pozwalający na odpowiedniąobsługę pomiaru w trybie synchronizowanym (Nie znajdował się on w sterownikuponieważ mechanizm ten silnie zależy od wykonywanych zadań) Te dwa ostatniemechanizmy zaimplementowane są za pomocą przerwań opisanych w podsekcji 423

421 Konfiguracja peryferioacutew

Implementacja najniższej warstwy oprogramowania czyli obsługi wejść wyjść mi-krokontrolera może być logicznie podzielona na dwie części Pierwszą z nich jestkonfiguracja wejść wyjść służących do komunikacji z jednostką inercyjną Komunika-cja z jednostką inercyjną odbywa się za pomocą interfejsu SPI oraz dwoacutech sygnałoacutewsterujących Jeden z nich służy jednostce inercyjnej do sygnalizowania że pomiarzostał zakończony drugi zaś służy do zlecenia pomiaru jednostce inercyjnej

Druga porcja wejść i wyjść wykorzystywana jest przez moduł kontroli Ether-CATrsquoa Urządzenie to jest obsługiwane za pomocą magistrali EPI Ta jest skonfiguro-wana tak aby pracować w trybie 8-bitowym z dwunastoma bitami adresu Magistralata działając z prędkością trzydziestu MHz zapewnia maksymalną przepustowość napoziomie 30 MBs Dodatkowo do kontroli układu wykorzystywane są sygnały re-setIRQSYNC Sygnał reset pozwala na wyzerowanie maszyny stanu modułu Any-bus natomiast linia IRQ pozwala modułowi zgłaszać przerwania ostatni sygnałSYNC służy roacutewnież do zgłaszania przerwań Przerwania modułu zgłaszane za po-mocą sygnału IRQ są żądaniami komunikacji najczęściej odczytuzapisu ProcessData odczytu zapisu Service Data czy żądania zmiany stanu modułu np zmianytrybu synchronizacji Przerwanie zgłaszane przez sygnał SYNC jest przerwaniemwskazującym punkt synchronicznego odczytu wejść (inicjacji pomiaru) bądź zapisuwyjść (np zadania wartości na silniki w przypadku gdyby był to sterownik silnikoacuteww obecnej implementacji brak jest wyjść ktoacutere można ustawić)

Z sygnałem SYNC wiąże się kilka pojęć ktoacutere definiują sposoacuteb działania od-czytuzapisu z synchronizacją Pojęcia te zilustrowane są na rysunku 41 a opisaneponiżej

bull Cycle time - czyli minimalny czas całego cyklu działania (w tym przypadkuminimalny czas trwania cyklu to 1 ms)

bull Output Processing Time - minimalny czas przetwarzania danych do wystawie-nia na wyjścia

bull Output Valid - czas od impulsu synchronizacji do ustawienia zadanych danychna wyjściu układu

bull Input Valid - Czas potrzebny od sygnału synchronizacji do czasu pomiaru

24 Rozdział 4 Opis oprogramowania

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

bull Input Processing Time - czas potrzebny od wykonania pomiaru do wysłaniago do modułu Anybus

Rysunek 41 Zależności czasowe dla sygnału synchronizacji

422 Sterownik Anybus

Drugim elementem oprogramowania jest biblioteka dostarczona przez producentaPomimo że jest to produkt w miarę kompletny to należy go skonfigurować aby dzia-łał w trybie odpowiadającym konfiguracji sprzętowej układu W naszym przypadkużądanym trybem jest komunikacja za pomocą szyny roacutewnoległej (w mikrokontrolerzeEPI) Dzięki temu możemy mapować pamięć modułu dostarczonego przez Anybusdo przestrzeni adresowej procesora co zapewnia nam prostotę w dostępie do zaso-boacutew tego urządzenia Kolejnym ważnym parametrem bez ktoacuterego nie możliwa byłabypraca w trybie synchronizowanego odczytu jest komunikacja za pomocą przerwańPo skonfigurowaniu tego parametru Anybus może zażądać komunikacji z naszymmodułem w odpowiednim dla niego czasie (to moduł Anybus zarządza synchroniza-cją czasu z masterem ethercat)

Wreszcie wymagana jest konfiguracja mapowania obiektoacutew za pomocą ktoacuterychukład przesyła dane

Konfiguracja biblioteki sterownika odbywa się na kilku poziomach Pierwszymz nich jest konfiguracja obiektoacutew za pomocą ktoacuterych będziemy przesyłać dane przezprotokoacuteł COE

Obiekty służące do przesyłania danych między skonstruowanym urządzeniema węzłem nadrzędnym komunikacji ethercat dzielą się na dwa typy Pierwszym z nichjest typ PDO czyli Process Data Object Jest to typ danych służący do komunikacjiw reżimie czasu rzeczywistego W wykonanej aplikacji dostępnych jest sześć obiek-toacutew tego typu Ich spis znajduje się w tabeli 41 Są to oczywiście wartości zmierzonekolejno przyspieszeń liniowych oraz prędkości kątowych Obroacutebka tych danych w mi-krokontrolerze sprowadza się jedynie do rzutowania ich na liczbę całkowitą ze zna-kiem Oznacza to że przyspieszenia wysyłane są w formacie 0333mg (tysięcznychprzyspieszenia ziemskiego) na bit Format prędkości kątowej zależy od konfiguracjiżyroskopu w tym przypadku dostępne są 3 zakresy pomiarowe prędkości kątowejPierwszy to zakres plusmn300s drugi plusmn150s oraz trzeci plusmn75s Pierwsze ustawie-nie wymaga jedynie rejestru opisanego w tabeli 42 jako Gyro range Settings Abyustawić najwyższy zakres pomiaru należy ustawić w tym rejestrze wartość 4 Dla

Rozdział 4 Opis oprogramowania 25

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

niższych zakresoacutew pomiarowych są to odpowiednio wartości 2 dla zakresu plusmn150soraz 1 dla zakresu plusmn75s Dla utrzymania dokładności zakresy te wymagają roacutew-nież ustawienia filtru cyfrowego Filtr cyfrowy jednostki inercyjnej konfiguruje sięza pomocą pola Digital Filter Settings opisanego w tabeli 42 Dla zakresu plusmn150swartość tego rejestru musi być większa bądź roacutewna 2 a dla zakresu plusmn75s większabądź roacutewna 4

Tablica 41 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa Opis Typ Adres

danychAcceleration X Wartość przyspieszenia w osi X 16b int 0x2065Acceleration Y Wartość przyspieszenia w osi Y 16b int 0x2066Acceleration Z Wartość przyspieszenia w osi Z 16b int 0x2067Rotation Speed X Wartość prędkości kontowej wokoacuteł osi X 16b int 0x2068Rotation Speed Y Wartość prędkości kontowej wokoacuteł osi Y 16b int 0x2069Rotation Speed Z Wartość prędkości kontowej wokoacuteł osi Z 16b int 0x206a

Obiekty typu SDO są obiektami służącymi do niecyklicznej komunikacji z urzą-dzeniem Używa się ich zazwyczaj do konfiguracji urządzenia podrzędnego lub spora-dycznego pobierania od niego informacji W implementacji urządzenia podrzędnegoethercat znajduje się wiele obiektoacutew SDO W większości służą one do konfigura-cji urządzenia SDO zaimplementowane w ramach pracy służą przede wszystkimdo konfiguracji oraz sprawdzania stanu jednostki inercyjnej Są one przedstawionew tabeli 42

Trzy pierwsze z nich to pola umożliwiające odczyt temperatury żyroskopoacutew poje-dynczych osi Dla tych wpisoacutew liczba 0 oznacza 25C a każda kolejna wartość wnosizmianę temperatury o 0 136C Pola te mają możliwość odczytu ale z oczywistychwzględoacutew nie można do nich zapisać Kolejne trzy wpisy w tabeli odwołując się doopcji sprawdzania oraz ustawienia wartości kompensujących odchylenie w pomia-rze żyroskopu Wartości od 7 do 9 to wartości kompensacji odczytu przyspieszenialiniowego

Wartość na pozycji dziesiątej to wartość zmieniająca ustawienia filtroacutew cyfro-wych jednostki inercyjnej Filty cyfrowe jednostki inercyjnej służą do zmniejszeniawpływu zakłoacuteceń na pomiar struktura filtrowania przedstawiona jest na rysunku42(a) na tym schemacie filtr cyfrowy wizualizowany jest przez dwa ostatnie ele-menty w ścieżce przetwarzania sygnału można roacutewnież zauważyć że ustawienie filtru(N) wpływa zaroacutewno na filtrowanie sygnału akcelerometru jak i żyroskopu Usta-wienia opisywanego rejestru są wykładnikiem dla poziomu filtrowania Zwiększonefiltrowanie oproacutecz zmniejszenia wpływu zakłoacuteceń zmniejsza roacutewnież możliwości do-starczenia sygnału o wysokiej częstotliwości Tłumienie filtroacutew roacuteżnych poziomoacutewprzedstawione jest na rysunku 42(b) Na osi pionowej oznaczone jest tłumienie sy-gnału w danej częstotliwości Na osi poziomej oznaczona jest częstotliwość względemczęstotliwości proacutebkowania Przykładowo dla domyślnej częstotliwości proacutebkowania(8192 SPS) przy ustawieniu do obiektu SDO bdquoDigital Filter Settingsrdquo wartości 4 coprzekłada się na poziom filtru roacutewny 16 tłumienie sygnału zaczyna znacznie rosnąćokoło wartości 16 Hz

Kolejna wartość czyli Gyro Range Settings to wpis pozwalający na ustawieniezakresu pomiarowego dla żyroskopu Dostępne są trzy zakresy pomiarowe plusmn75splusmn150s plusmn300s Odpowiadają im wartości odpowiednio 1 2 4 Dla zakresuplusmn75s wymagane jest aby wartość wpisana w pole Digital Filter Settings była

26 Rozdział 4 Opis oprogramowania

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Tablica 42 Opis Obiektoacutew SDO dostępnych w urządzeniuNazwa Opis Typ da-

nychAdres

Gyro Temperature X Wartość temperatury żyro-skopu w osi X

16b uint 0x21FC

Gyro Temperature Y Wartość temperatury żyro-skopu w osi Y

16b uint 0x21FD

Gyro Temperature Z Wartość temperatury żyro-skopu w osi Z

16b uint 0x21FE

Gyro bias offset X Wartość korekty odchyleniażyroskopu w osi X

16b uint 0x21FF

Gyro bias offset Y Wartość korekty odchyleniażyroskopu w osi Y

16b uint 0x2200

Gyro bias offset Z Wartość korekty odchyleniażyroskopu w osi Z

16b uint 0x2201

Accelerometer bias offset X Wartość korekty odchyleniaakcelerometru w osi X

16b uint 0x2202

Accelerometer bias offset Y Wartość korekty odchyleniaakcelerometru w osi Y

16b uint 0x2203

Accelerometer bias offset Z Wartość korekty odchyleniaakcelerometru w osi Z

16b uint 0x2204

Digital Filter Settings Wartość filtru 16b uint 0x2205Gyro Range Settings Zakres żyroskopu 16b uint 0x2206Autonull Gyro Bias Procedura kalibracji żyro-

skopubool 0x2207

Restore Factory Calibration Przywroacuteć ustawienia fa-bryczne

bool 0x2208

Precision Autonull Bias procedura dokładnej kalibra-cji żyroskopu

bool 0x2209

Product ID Kod produktu 16b uint 0x220ASerial Number Numer seryjny produktu 16b uint 0x220B

nie mniejsza niż 4 dla zakresu plusmn150s jest to odpowiednio 2 Zakres plusmn300s nieposiada takich ograniczeń

AutoNull Gyro Bias obiekt ktoacuterego ustawienie powoduje uruchomienie proce-dury kalibracji parametroacutew Gyro Bias jednostka inercyjna powinna znajdować sięwtedy w bezruchu Kolejna Opcja służy do odzyskiwania ustawień fabrycznych off-setoacutew ale roacutewnież zakresu pomiarowego akcelerometru oraz ustawień wewnętrznegocyfrowego filtru Ostatnie dwie pozycje to numeryczny kod produktu oraz numerseryjny

Drugim mniej abstrakcyjnym poziomem na ktoacuterym należy skonfigurować biblio-tekę dostarczoną wraz z modułem Anybus jest poziom definicji preprocesora w jed-nym z plikoacutew nagłoacutewkowych Służą one do sygnalizacji bibliotece jakie są możliwościkomunikacyjne między mikroprocesorem a modułem Anybus Stałe wykorzystywanew wytworzonym oprogramowaniu przedstawione są w tabeli 43 a listę wszystkichmożliwości konfiguracyjnych można znaleźć w [1]

Po inicjalizacji wejść wyjść procesora program przechodzi do głoacutewnej pętliktoacutera zajmuje się śledzeniem maszyny stanu zawartej w module Anybus a przed-stawionej na rysunku 43 Reszta zadań została oddelegowana do obsługi w prze-rwaniach

Rozdział 4 Opis oprogramowania 27

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Tablica 43 Opis Obiektoacutew PDO dostępnych w urządzeniuNazwa OpisABCC_USER_MAX_NUM_APPL_CMDS Ilość komend ktoacutere można wysłać bez

otrzymania odpowiedziABCC_USER_MAX_NUM_ABCC_CMDS Ilość komend ktoacutere można odebrać

bez wysłania odpowiedziABCC_USER_MAX_MSG_SIZE Wielkość największej używanej w pro-

gramie odpowiedzi Gdy nie jestznana największa odpowiedź jaka bę-dzie nadawana należy ustawić naj-większa obsługiwaną wartość tj 1524bajty

ABCC_USER_MAX_PROCESS_DATA_SIZE Wielkość największej obiektoacutew PDO

używanych podczas komunikacjiABCC_USER_ERR_REPORTING_ENABLED Odblokowuje funkcję raportującą

błędy (ktoacuterą należy zaimplementowaćw systemie)

ABCC_USER_DEBUG_ERR_ENABLED Odblokowuje wypisywanie informacji

debugujących po wywołaniu funkcjibłędu ABCC_CbfDriverError()

ABCC_USER_DEBUG_PRINT Przypisanie funkcji wypisywania in-formacji debugowych

ABCC_USER_POLL_WRPD Jeżeli stała jest zdefiniowana za każ-dym razem gdy uruchomiona zosta-nie funkcja ABCC_RunDriver() uru-chomiona zostanie roacutewnież funkcjaodświeżająca WRPD Czyli obiektyktoacutere są PDO i są dostępne do za-pisu(odebrania od mastera)

ABCC_USER_DRV_PARALLEL Sterownik będzie używany w trybieroacutewnoległym z obsługą zdarzeniową

ABCC_USER_INT_ENABLED Odblokowuje obsługę przerwań ze-wnętrznych od Anybus

28 Rozdział 4 Opis oprogramowania

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Nazwa OpisABCC_USER_INT_ENABLE_MASK Określa możliwe typy przerwań wartość jest sumą

logiczą flagABP_INTMASK_RDPDIEN - Odblokowuje czy-tanie PDO w przerwaniuABP_INTMASK_STATUSIEN - Odblokowujesprawdzanie statusu urządzenia w przerwaniuABP_INTMASK_RDMSGIEN - Odblokowujeodczyt wiadomości w przerwaniuABP_INTMASK_WRMSGIEN - Odblokowujezapis wiadomości w przerwaniuABP_INTMASK_ANBRIEN - Odblokowujeprzerwanie zmiany statusu przez moduł Anybus

ABCC_USER_MEMORY_MAPPED_ACCESS Odblokowuje tryb mapowania pamięci Anybus do

przestrzeni adresowej mikrokontrolera co znacznieułatwia komunikację

ABCC_USER_PARALLEL_BASE_ADR Zawiera adres pamięci mapowanej do mikrokon-

trolera (0xA0000000)

(a) Schemat układu filtroacutew cyfrowych Źroacutedło[19]

(b) Wykres tłumienia filtroacutew cyfrowych Źroacutedło[19]

Rysunek 42 Filtry cyfrowe jednostki inercyjnej

Rozdział 4 Opis oprogramowania 29

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Rysunek 43 Maszyna stanu jednostki Anybus Źroacutedło [2]

423 Przerwania

Do mikrokontrolera doprowadzone są 3 linie przerwań zewnętrznych Linia prze-rwania komunikacji modułu Anybus zarządza komunikacją z urządzeniem AnybusSchemat tej procedury przedstawiony jest na rysunku 44 Po odebraniu przerwa-nia na tej linii w pierwszej kolejności odczytywany jest z urządzenia Anybus rejestrmoacutewiący o przyczynie przerwania Gdy ta zostanie ustalona z modułu Anybus od-czytywane są (lub zapisywane) odpowiednie wartości jeżeli zachodzi taka potrzebawywoływane są roacutewnież funkcje specyficzne dla danego typu komunikacji Obsługarożnych typoacutew komunikacji jest dostarczona przez bibliotekę obsługującą modułAnybus

Żądanie komunikacji anybus

Sprawdzenie czy sterownik oczekuje na gotowość anybus do komunikacji

Ustaw Anybus gotowy do komunikacji

[Oczekuje]

Określ typy komunikatoacutew obsługiwanych w przerwaniach

[Nie oczekuje]

Odczytaj typ komunikatu

Czy komunikat obsługiwany w przerwaniuOdbierz komunikat

TakNie

Przetwoacuterz komunikat

Rysunek 44 Obsługa przerwania komunikacji z Anybus

30 Rozdział 4 Opis oprogramowania

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Druga linia przerwania jest linią pochodzącą od jednostki inercyjnej Sygnałprzerwania na tej linii sygnalizuje możliwość odebrania danych Procedura odbie-rania danych wykonuje się roacutewnież w przerwaniu dzieje się tak ponieważ ważnejest natychmiastowy ich odbioacuter i przepisanie do odpowiednich adresoacutew skąd zostanąwysłane do modułu Anybus co pokazane jest na schemacie 45

Dane dostępne Zablokuj przerwania Odczytaj dane Odblokuj przerwania

Rysunek 45 Obsługa przerwania komunikacji z jednostką inercyjną

Ostatnią linią przerwania jaka została doprowadzona do mikrokontrolera jest li-nia sygnału SYNC przedstawiona na rysunku 46 Sygnał ten jest odpowiedzialny zaobsługę mechanizmoacutew związanych z komunikacją synchroniczną Ponieważ w przed-stawianej aplikacji nie występują sygnały wyjściowe do wysterowania zaimplemen-towana jest tylko część odpowiadająca za pozyskanie pomiaru z jednostki inercyjnejProcedura przerwania wygląda tak jak na rysunku

Żądanie synchronizacji Czy czas Input Valid jest zerowy

Konfiguruj Timer

Nie

Tak

Uruchom Timer

Timer zakończył odliczanie

Zleć Pomiar do IMU

Rysunek 46 Obsługa przerwania sygnału synchronizacji

Pierwszym krokiem procedury obsługi przerwania jest sprawdzenie czy czas In-put Valid jest zerowy (domyślnie jest ustawiany na 0) jeżeli jest inaczej urucha-miamy Timer w trybie pojedynczego zliczania na czas Input Valid Jeżeli czas tenjest zerowy lub bo odliczeniu czasu za pomocą timera zlecamy pomiar jednostceinercyjnej Zlecenie pomiaru wygląda w następujący sposoacuteb Najpierw ustawiamysygnał zlecenia pomiaru w stan wysoki następnie ustawiamy Timer na odliczenie 5us i opuszczamy przerwanie w ktoacuterym byliśmy (przerwanie może pochodzić z sy-gnału SYNC lub timera inicjowanego gdy czas Input Valid jest roacuteżny od zera) Poodliczeniu pięciu mikro sekund Timer wywoła przerwanie w ktoacuterym sygnał zleceniapomiaru ustawiamy na poziomie niskim i opuszczamy przerwanie

43 Sterownik w strukturze OROCOS

Druga część oprogramowania została napisana w celu skomunikowania pomiędzymagistralą ethercat a innymi programami działającymi w ramach struktury ramo-wej ROS i OROCOS Jest to prosty sterownik działający w strefie użytkownikaoraz opierający się o pakiet ec_hardware zawierający się w oprogramowaniu ir-pOS Pakiet ec_hardware jest sterownikiem dostosowanym do użycia przez sys-tem OROCOS Klasa sterownika jednostki inercyjnej dziedziczy po klasie ECDri-

Rozdział 4 Opis oprogramowania 31

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

ver co pozwala na komunikację z programem mastera Ethercat oraz na tworze-nie jego instancji podczas działania systemu przez komponent nazywający się roacutew-nież ECHardware Najważniejsze zadania sterownika przedstawione zostały w li-stingu 41 Pierwsza z nich nosi nazwę configureHook jest to funkcja uruchamianapodczas konfiguracji usług oprogramowania OROCOS ta funkcja w pierwszej kolej-ności przypisuje obiekty adresom PDO (inicjalizacja adresoacutew odbywa się w konstruk-torze) po czym wczytuje z przekazanej jej konfiguracji parametry zakresu pomiaro-wego oraz ustawień filtru zapisuje mnożnik prędkości kątowej i wysyła konfiguracjedo urządzenia Wartości uruchomieniowe tych parametroacutew znajdują się w pliku o roz-szerzeniu YAML o nazwie master_configyaml Druga funkcja uruchamiana w każ-dym cyklu sterowania ma za zadanie pobrać wartości mierzone z jednostki inercyjnejNastępnie dokonuje konwersji do formatu [ms2] zapisuje do obiektu wiadomościstandardowego typu sensor_msgsImu Typ tej wiadomości jest standardowym ty-pem znajdującym się w systemie ROS służącym do opisywania pomiaroacutew jednostekinercyjnych Przed wysłaniem wiadomości na specjalny temat zostaje ona opatrzonastemplem czasowym co umożliwia poacuteźniejsza identyfikację momentu otrzymania da-nych nawet poza środowiskiem czasu rzeczywistego jakie zapewnia OROCOS Pouruchomieniu sterownika jego działanie przedstawione na rysunku 47 sprowadza siędo odpytywania za pomocą mastera ethercata jednostki inercyjnej o dane oraz eks-portowaniu tych danych do środowiska ROS dla dowolnego podsystemu sterowaniado użycia

Listing 41 prototypy kluczowych funkcji sterownika imubool IMUDriver conf igureHook ( const YAML Node ampc fg ) uint16_t range = 0x2 uint16_t f i l t e r = 0x4 rangeSca l e = Pre150degpsec

thisminusgtaddPDOEntry(ampacceleration_x_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_y_pdo_ ) thisminusgtaddPDOEntry(ampacceleration_z_pdo_ ) thisminusgtaddPDOEntry(amprotation_x_pdo_ ) thisminusgtaddPDOEntry(amprotation_y_pdo_ ) thisminusgtaddPDOEntry(amprotation_z_pdo_ ) thisminusgtaddPDOEntry(ampcontrol1_pdo_ )

i f ( c f g [ range ] ) range = c fg [ range ] asltint gt()i f ( range==1)rangeSca l e = Pre75degpsec i f ( range==2)rangeSca l e = Pre150degpsec i f ( range==4)rangeSca l e = Pre300degpsec

i f ( c f g [ f i l t e r ] ) f i l t e r = c f g [ f i l t e r ] asltint gt()

slave_minusgtaddSDOConfig (0 x2205 0 f i l t e r ) slave_minusgtaddSDOConfig (0 x2206 0 range ) return true

32 Rozdział 4 Opis oprogramowania

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

void IMUDriver updateInputs ( ) int16_t ax ay az rx ry rz sensor_msgs Imu wr

ax = acceleration_x_pdo_ read ( ) ay = acceleration_y_pdo_ read ( ) az = acceleration_z_pdo_ read ( )

rx = rotation_x_pdo_ read ( ) ry = rotation_y_pdo_ read ( ) r z = rotation_z_pdo_ read ( )

wr l i n e a r_a c c e l e r a t i o n x = ( static_castltdoublegt(ax ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n y = ( static_castltdoublegt(ay ) accSca l e )lowast g wr l i n e a r_a c c e l e r a t i o n z = ( static_castltdoublegt(az ) accSca l e )lowast g

wr angu la r_ve loc i ty x = static_castltdoublegt(rx ) rangeSca l e wr angu la r_ve loc i ty y = static_castltdoublegt(ry ) rangeSca l e wr angu la r_ve loc i ty z = static_castltdoublegt(rz ) rangeSca l e

wr header stamp = rt t_ro s c l o ck host_now ( ) port_imu_msr_outport_ wr i t e (wr )

Podsystem sterowania

Jednostka inercyjna rzeczywisty

receptor

OROCOSowySterownik IMU

Ethercat Master

ROS

OROCOSWirtualny receptor

Jednostka inercyjna

Rysunek 47 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej

Rozdział 4 Opis oprogramowania 33

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

34 Rozdział 4 Opis oprogramowania

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Rozdział 5

Badania weryfikacyjne

Weryfikacja działania układu została przeprowadzona z użyciem robota IRP-6 bę-dącego na wyposażeniu laboratorium Zespołu Programowania Robotoacutew i SystemoacutewRozpoznających działającego na Wydziale Elektroniki i Technik Informacyjnych Po-litechniki Warszawskiej [14] Środowisko eksperymentalne wykorzystane na potrzebytej pracy opisane jest w sekcji 51 Następnie w sekcji 52 omoacutewiony zostanie projektoraz cel przeprowadzonych eksperymentoacutew Wyniki eksperymentoacutew opisane w sek-cji 53 rzucą nieco światła na możliwości zbudowanego układu Wreszcie w sekcji54 podsumujemy wyniki eksperymentoacutew oraz zaznaczymy mocne i słabsze stronyzbudowanego układu

51 Opis stanowiska eksperymentalnego

Jednostka inercyjna została zamontowana na ramieniu robota irp6 Jest to roboto siedmiu stopniach swobody dzięki dodatkowemu torowi jezdnemu Robot ten jestroacutewnież wyposażony w nadgarstkowy czujnik siły Uproszczony schemat manipula-tora wraz z zbliżeniem na miejsce montowania jednostki inercyjnej przedstawionyjest na rysunku 51 Robot ten sterowany jest za pomocą komputera PC komunikują-cego się ze sterownikami silnikoacutew za pomocą magistrali Ethercat oraz protokołu CanOver Ethercat Sterowanie odbywa się pod kontrolą oprogramowania IRpOS [13]Oprogramowanie to można potraktować jako wirtualny receptor bądź jako oprogra-mowanie warstwy abstrahującej sprzęt Częstotliwość pętli sterowania robota to 500[Hz] pomiar przyspieszeń za pomocą jednostki inercyjnej oraz pomiar sił odbywasię w każdym cyklu pętli sterowania

52 Projekt eksperymentoacutew

Eksperymenty zaprojektowane oraz przeprowadzone na robocie irp6 miały na celuweryfikację działania układu sprawdzenie wpływu filtrowania sygnału na odczytyjednostki inercyjnej oraz sprawdzenie przydatności tego układu do zadania korekcjiodczytu siły w czujniku nadgarstkowym robota na ktoacuterym został zainstalowany

Do badań użyto dwoacutech typoacutew trajektorii testowych Obie z nich zakładały stałąorientację końcoacutewki robota tak aby oś Z chwytaka skierowana była w doacuteł jak narysunku 52

Trajektoria typu pierwszego była zaplanowana tak aby z osobna we wszystkichosiach poruszać końcoacutewką robota po sinusoidzie następnie po kilku okresach sinusazatrzymać się i natychmiast zacząć odtwarzać ten ruch w drugą stronę Trajektoria

Rozdział 5 Badania weryfikacyjne 35

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

2

20

Wrist(W)

Sensor(S)

Base(0)

ZX

Gripper

(G)

Z

IMU(M)

Force

Rysunek 51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20]

typu drugiego polegała na poruszaniu się po okręgu leżącym kolejno w płaszczy-znach yz xy xz i tak jak poprzednio po wykonaniu kilku okręgoacutew końcoacutewka masię zatrzymać i zacząć poruszać się w drugą stronę a potem przejść do kolejnejpłaszczyzny

Zaplanowane trajektorie nie zmieniały orientacji chwytaka (a także czujnika siłyi jednostki inercyjnej) co pozwoliło na założenie że wektor przyspieszenia grawita-cyjnego działa w kierunku i ze zwrotem zgodnym z wersorem Z6 Ponieważ jednostkainercyjna ma inną orientację niż czujnik siły pomiar musi zostać poddany przekształ-ceniu ktoacutere pokazane jest we wzorach 51 52 Następnie należało usunąć wpływgrawitacji na pomiar przyspieszenia co pokazują wzory 53 54

R =

0 0 10 minus1 01 0 0

(51)

Ag = R lowast Ai (52)

G =

00g

(53)

Agg = Ag minusG (54)

Następnym krokiem testowania było sprawdzenie czy odczytane w ten sposoacuteb danemogą posłużyć do korekcji siły Model estymowanej siły jest przedstawiony we wzo-rze 55 Aby to stwierdzić należy najpierw ustalić masę tej części chwytaka ktoacuteraznajduje się za czujnikiem siły (w kierunku i o zwrocie osi Z6) Estymacja ta zostaładokonana za pomocą najmniejszych kwadratoacutew Według liniowej wersji tej metodymasę można wyznaczyć ze wzoru 56 Gdzie As to wektor wybranych pomiaroacutew

36 Rozdział 5 Badania weryfikacyjne

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

przyspieszenia w wybranej osi a Fo jest wektorem siły mierzonej w odpowiedniejosi oraz odpowiadających pomiarom przyspieszenia Każdy z pomiaroacutew ma stem-pel czasowy mierzony w nanosekundach Z tego powodu do obliczeń używany byłostatni dostępny pomiar przyspieszenia (dla każdego pomiaru siły)

Fest = minusm lowast Agg (55)

m = minus(ATs lowast As)

minus1 lowast ATs lowast Fo (56)

Z6

X6Y6

Rysunek 52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej

53 Wyniki eksperymentoacutew

Przedstawione w tej sekcji wyniki opierają się na trzech seriach pomiarowych prze-prowadzonych z udziałem czujnika siły oraz jednostki inercyjnej Pełne przebiegi dlatych serii można zobaczyć w dodatku B Na wykresach 53 54 55 przedstawionezostały pomiary z czujnika siły w osi x oraz pomiary jednostki inercyjnej roacutewnieżw osi x (po obroacuteceniu układu wspoacutełrzędnych) kolejnych serii pomiarowych Pierw-sza z nich 53 jest pobrana z użyciem trajektorii typu pierwszego opisanej w sekcji52 oraz z ustawieniami filtru jednostki inercyjnej na minimalnym poziomie (war-tość pola Digital Filter Settings roacutewna 1) Odczyt ten nosi widoczne ślady podziałuna trzy części ( ich granice znajdują się w okolicach 21 i 42 sekundy pomiaru)Pierwsza z tych części to ta w ktoacuterej robot porusza końcoacutewką wzdłuż osi w ktoacuterejwykonywany jest pomiar widoczny tam jest wyraźnie działanie wpływu siły iner-cyjnej związanej z zadaną trajektorią Około dziesiątej sekundy pomiaru występujeznaczny skok zmierzonej siły jest to efekt nagłej zmiany kierunku poruszania sięrobota Druga sekcja jest okresem w ktoacuterym robot poruszał się wzdłuż osi y w tejsekcji nie występują znaczne skoki mierzonej (w osi x) wartości siły ani przyspie-szenia Trzecia sekcja pokazuje wzrost wartości mierzonych na obu sensorach co

Rozdział 5 Badania weryfikacyjne 37

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

oznacza że ruch w osi z nie jest idealnie odseparowany od ruchu w osi x Druga seriapomiarowa 54 oparta została o taką samą trajektorię Jedyną roacuteżnicą było tu zasto-sowanie większej wartości filtrowania dla jednostki inercyjnej (wartość pola DigitalFilter Settings roacutewna 4) W tym przypadku widać że choć sekcja druga i trzeciapomiaru w przypadku czujnika siły wygląda niemali identycznie jak w poprzednimprzykładzie to pomiar uzyskany z jednostki inercyjnej jest praktycznie zerowy Filtrusunął większość sygnału wysokoczęstotliwościowego ktoacutery pojawiał się w poprzed-nim przykładzie Sytuacja ta miała roacutewnież wpływ na usunięcie znacznej części pikuwystępującego w okolicach dziesiątej sekundy pomiaru Oznacza to że pomiar niejest przydatny do korekcji odczytu siły ponieważ w tym zadaniu potrzebny jest od-czyt umożliwiający stwierdzenie piku pochodzącego na przykład z zatrzymania sięmanipulatora Dlatego też w dalszej części rozważań przyjrzymy się jedynie pomia-rom zebranym z niskim ustawieniem poziomu filtracji sygnału Trzecia seria 55 jestserią testową Ponieważ w każdym momencie ruchu końcoacutewka robota porusza sięw dwoacutech osiach wyznaczenie siły korygującej siłę inercyjną powinno być znacznietrudniejsze Decydujące dla wynikoacutew pracy jest stwierdzenie czy pomiar pochodzącyz jednostki inercyjnej jest na tyle szybki aby niwelować piki pomiaru siły takie jakpokazane w powyższych przykładach

Wyznaczenie masy chwytaka odbyło się za pomocą danych z każdej osi pierwszejprzedstawionej serii pomiarowej Dane wybrane do wyznaczania masy chwytaka zo-stały wybrane tak aby zmniejszyć udział szumoacutew pomiarowych w tym procesie Więcw danej osi zostały uwzględnione jedynie pomiary gdy robot poruszał się wzdłuż tejosi (od t = 0s do t asymp 21s z osi x t asymp 21s do t asymp 42s z osi y t asymp 42s do t asymp 65s z osiz) W ten sposoacuteb masa została oszacowana na m asymp 13547[kg]

Dzięki aproksymacji masy chwytaka możliwe było wyznaczenie siły bezwładnościjaka powinna na niego działać na podstawie pomiaroacutew jednostki inercyjnej Pierw-szym przykładem jest obliczenie i korekcja odczytu siły podczas ruchu wzdłuż osi x56 Na rysunku 56(a) widać że przebieg ktoacutery w idealnych warunkach powinienbyć sinusoidą jest w pewnym stopniu zniekształcony Jednak estymowana siła poka-zana na rysunku 56(b) roacutewnież posiada sporą dozę zakłoacuteceń Zakłoacutecenia te w dużejczęści nie wynikają z szumu własnego czujnika tylko ze sposobu sterowania mani-pulatorem Na przykład duża doza tego szumu ma okres około 50[ms] co odpowiadaokresowi zadawania kolejnych punktoacutew docelowych przez skrypt generujący trajek-torie Roacuteżnica tych dwoacutech sił pokazana jest na rysunku 56(c) Na tym rysunku od

38 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres oczytu przyspieszenia w osi x

Rysunek 54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Wykres odczytu siły w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) Wykres odczytu przyspieszenia w osi x

Rysunek 55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej

Rozdział 5 Badania weryfikacyjne 39

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(a) Siła zmieżona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(b) Siła obliczona w osi x

czas (ns) 109

0 1 2 3 4 5 6 7 8 9 10

-3

-2

-1

0

1

2

3

(c) Siła skorygowana w osi x

Rysunek 56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

44 46 48 5 52 54 56 58 6 62 64-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 57 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y

razu można zobaczyć że amplituda sygnału znacznie spadła maksymalna wartośćw tej sekcji spadła z 3 [N] do 15 [N]

Kolejnym ciekawym przykładem jest ten część trajektorii w ktoacuterej robot poruszasię po płaszczyźnie prostopadłej do osi x w ktoacuterej pomiar przedstawiamy W tymprzykładzie wydaje się że siła estymowana jest niedoszacowana co widać szczegoacutelniew skokach siły takich jak ta w t = 64s

Przykładem dobitnie pokazującym że sam układ jest na tyle szybki aby zniwelo-wać skoki siły bezwładności spowodowane sterowaniem robota jest ten widoczny narysunku 58 Na rysunku przedstawiającym silę odczytaną z nadgarstkowego czuj-nika siły 58(a) widać jak duże wartości może osiągać siła bezwładności w kroacutetkimczasie W tym przypadku siła estymowana 58(b) ma bardzo podobne wartości dotych zmierzonych z czego wynika siła skorygowana ktoacutera swoacutej pik ma na pozio-mie minus5[N ] W miejscu w ktoacuterym oryginalny odczyt siły wynosił minus15[N ] co dajeredukcję o około 66

Następne dwa przykłady pochodzą nie z trajektorii uczącej lecz z trajektoriitestowej ktoacutera przedstawiona została na rysunku 55 Przykład ten (widoczny narysunku 59) obejmuje okres od 27 do 32 sekundy pomiaru i jak wszystkie inne zostałon pobrany z danych z osi x N fragment ruchu po płaszczyźnie XY oraz momentzatrzymania robota Siła estymowana (rysunek 59(b)) roacuteżni się od siły zmierzonej(rysunek 59(a)) niedoszacowanymi skokami siły w momencie zatrzymania się ro-bota co najlepiej widać na wykresie siły skorygowanej 59(c) Oproacutecz tego roacuteżnicesą niewielkie

Ostatni przykład(rysunek B3) to pochodzący z między 54 a 55 sekundy po-miar W tej sekcji sygnał jest silnie zakłoacutecany przez sprzężenia z ruchu w drugiejosi Pomiar siły przedstawiony dla tego segmentu na rysunku B3(g) jest na tylezniekształcony że posiada pik o wartości prawie 5[N ] tam gdzie powinno być jego

40 Rozdział 5 Badania weryfikacyjne

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

101 102 103 104 105 106 107 108 109-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 58 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-niu manipulatora

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(b) Siła obliczona w osi x

czas (ns) 1010

28 285 29 295 3 305 31 315 32-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

Rysunek 59 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xywraz z momentem zatrzymania manipulatora

minimum Siła estymowana (a więc roacutewnież pomiar przyspieszenia) przedstawionana rysunku B3(h) zdaje się być mniej obciążona tym zakłoacuteceniem co widać nawykresie siły skorygowanej (rysunek B3(i)) ta posiada maksima lokalne tam gdziepowinny być minima trajektorii W tej części trajektorii maksymalne pomiary siłzmniejszyły się nieznacznie w stosunku do wartości zmierzonych

Przedstawione przykłady wskazują na to że jednostka inercyjna może być zasto-sowana do zadania korekcji odczytu siły (lub odczytu siły uogoacutelnionej) czujnika nad-garstkowego Są to jednak jedynie przykłady działania w poszczegoacutelnych sytuacjachW tabeli 51 przedstawiono zaś statystykę dotyczącą całych trajektorii Wartości teoparte są o długość wektoroacutew siły zmierzonej oraz skorygowanej Biorą one więc poduwagę wszystkie trzy osie w ktoacuterych dokonany był pomiar Trajektoria pierwsza tota przedstawiona na rysunku 53 jako że służyła ona do wyznaczenia parametrumasy chwytaka robota spodziewane było znaczne poprawienie odczytu siły Okazuje

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(a) Siła zmieżona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(b) Siła obliczona w osi x

czas (ns) 1010

55 555 56 565 57 575-5

-4

-3

-2

-1

0

1

2

3

4

5

(c) Siła skorygowana w osi x

Rysunek 510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz

Rozdział 5 Badania weryfikacyjne 41

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

się że roacuteżnica wartości średniej siły w tej serii pomiarowej spadła o nieco ponad 05[N] Pomimo niewielkiego jak by się mogło wydawać jej spadku średnia wartość wy-nosi niewiele ponad 1 [N] Inaczej sprawa ma się co do wartości maksymalnej bo tuwyniki wskazują na dużą poprawę bo spadek ma wartość około 93 [N] Trajektoriadruga to ta przedstawiona na rysunku 55 Musimy zauważyć że ze względu na ruchwzdłuż dwoacutech osi wartość średnia pomiaru jest większa a maksymalna jest prawieidentyczna W tym przypadku spadek wartości średniej w skorygowanym odczyciesiły wynioacutesł około 074 [N] czyli nieco więcej niż w trajektorii pierwszej Roacuteżnica sił(zmierzonej i skorygowanej) maksymalnych wynosi natomiast około 8 [N] to wyniknieco mniejszy niż w przebiegu pierwszym

Tablica 51 Statystyka dotycząca długości wektoroacutew siłyNazwa opisywanej wartości Trajektoria 1 Trajektoria 2Średnia długość wektora siły zmierzonej 16219 21895Średnia długość wektora siły skorygowanej 11002 14455Maksymalna długość wektora siły zmierzonej 245354 245398Maksymalna długość wektora siły skorygowanej 152327 164627

54 Podsumowanie

W ostatnich latach dzięki eksplozji dostępnej mocy obliczeniowej w robotyce moż-liwa jest implementacja coraz bardziej skomplikowanych pętli sterowania Wrazz tym otwierają się możliwości dla przetwarzania coraz większej ilości danych Za-chęca to do konstruowania systemoacutew robotycznych pozwalających na większą inte-rakcję z otoczeniem Do tworzenia takich systemoacutew potrzebne są rozwiązania sprzę-towe oraz programowe pozwalające na przewidywalną komunikacje wraz z możliwo-ścią synchronizacji momentu pomiaru Takie urządzenia choć pojawiają się to przedewszystkim w segmencie serwo-sterownikoacutew Rynek czujnikoacutew szczegoacutelnie z interfej-sem EtherCat jest wciąż wąski

W ramach tej pracy został zaprojektowany zrealizowany oraz oprogramowanyukład pośredniczący w komunikacji między jednostką inercyjną a kontrolerem urzą-dzenia podrzędnego magistrali EtherCAT

Przeprowadzono badania dowodzące że wykonany układ ma możliwości aby byćzastosowany do korekcji pomiaru siły w czujniku nadgarstkowym robota Wyniki ba-dań potwierdzają przydatność układu do zadania korekcji pomiaru siły Układ zacho-wuje się stabilnie istnieje możliwość przeprowadzania pomiaroacutew w sposoacuteb synchro-nizowany jest łatwy w integracji z systemem robotycznym dzięki standardowemusposobowi komunikacji Minusem tego rozwiązania jest koszt układu potrzebnegodo komunikacji EtherCAT

42 Rozdział 5 Badania weryfikacyjne

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

Bibliografia

[1] HMS Industrial Networks AB Anybus CompactCom 40 Driver User ManualDocId HMSI-27-225

[2] HMS Industrial Networks AB Anybus CompactCom B40 Design Gu-ide httpwwwanybuscomuploadAnybus-CompactCom20B4020Brick-4697-Anybus_CompactCom_B40_design_guidepdf

[3] Minhang Bao Analysis and design principles of MEMS devices Elsevier 2005

[4] C Boucher A Lahrech J-C Noyer Non-linear filtering for land vehicle naviga-tion with gps outage Systems Man and Cybernetics 2004 IEEE InternationalConference on wolumen 2 strony 1321ndash1325 IEEE 2004

[5] Yongming Chen Hua Chen Mingzhong Zhang Yin Li The relevant researchof coe protocol in ethercat industrial ethernet Intelligent Computing and In-telligent Systems (ICIS) 2010 IEEE International Conference on wolumen 2strony 67ndash70 IEEE 2010

[6] Javier Gaacutemez Garciacutea Anders Robertsson Juan Goacutemez Ortega Rolf Johans-son Force and acceleration sensor fusion for compliant robot motion controlRobotics and Automation 2005 ICRA 2005 Proceedings of the 2005 IEEEInternational Conference on strony 2709ndash2714 IEEE 2005

[7] Ethercat Technology Group Ethercat Slave Implementation Gu-ide httpwwwethercatorgpdfenglishETG2200_V2i0i0_SlaveImplementationGuidepdf

[8] Jiho Han Deog-Kyoon Jeong A practical implementation of ieee 1588-2008transparent clock for distributed measurement and control systems IEEEtransactions on instrumentation and measurement 59(2)433ndash439 2010

[9] Texas Instruments Incorporated TivaTM TM4C1294NCPDT Microcontrol-ler DATA SHEET httpwwwticomlitdssymlinktm4c1294ncpdtpdf

[10] Bartosz Kaczor Budowa platformy mobilnej o napędzie roacuteżnicowym Bachelorrsquosthesis WEiTI 2015

[11] ARM Ltd Specyfikacja złącz Jtag httpinfocenterarmcomhelptopiccomarmdocfaqsattached13634cortex_debug_connectorspdf

[12] Robert P Moeller William K Burns Low noise fiber gyroscope system whichincludes excess noise subtraction Lipiec 19 1994 US Patent 5331404

Bibliografia 43

[13] Warsaw University of Technology IRpOS software wiki page httpsgithubcomRCPRG-ros-pkgirp6_robotwiki

[14] Warsaw University of Technology Robot Programing and Pattern RecognitionGroup httprobotykaiapwedupl

[15] Ruxandra Lupas Scheiterer Chongning Na Dragan Obradovic Guumlnter SteindlSynchronization performance of the precision time protocol in industrial auto-mation networks IEEE Transactions on Instrumentation and Measurement58(6)1849ndash1857 2009

[16] NXP Semiconductors UM10204 I2C-bus specification and user manual httpwwwnxpcomdocumentsuser_manualUM10204pdf

[17] Jakub Simanek Michal Reinstein Vladimir Kubelka Evaluation of the ekf-based estimation architectures for data fusion in mobile robots IEEEASMETransactions on Mechatronics 20(2)985ndash990 2015

[18] Salah Sukkarieh Low cost high integrity aided inertial navigation systems forautonomous land vehicles Praca doktorska The University of Sydney 2000

[19] Linear Technology ADIS16362 data sheet httpwwwanalogcommediaentechnical-documentationdata-sheetsADIS16362pdf

[20] Tomasz Winiarski Konrad Banachowicz System akwizycji skorygowanej siłyuogoacutelnionej kontaktu robota manipulacyjnego z otoczeniem Pomiary Automa-tyka Robotyka (2)390ndash394 2013 The acquisition system of general force ofcontact between robotic manipulator and the environment (in Polish)

44 Bibliografia

Spis rysunkoacutew

21 Schematy przedstawiające strukturę ramki w komunikacji ethercat 12

31 Wizualizacja płytek 1532 Rysunek płytki głoacutewnej wraz z podanymi wymiarami 1633 Wizualizacja płytek 1734 Standard numeracji na złączu międzypłytkowym 1835 Schemat złącza zasilania 1936 Numeracja pinoacutew oraz położenie złącz na płytce 1937 Schemat złącza jtag 2038 Numeracja złącza hosta źroacutedło [2] 20

41 Zależności czasowe dla sygnału synchronizacji 2542 Filtry cyfrowe jednostki inercyjnej 2943 Maszyna stanu jednostki Anybus Źroacutedło [2] 3044 Obsługa przerwania komunikacji z Anybus 3045 Obsługa przerwania komunikacji z jednostką inercyjną 3146 Obsługa przerwania sygnału synchronizacji 3147 Komunikacja OROCOSrsquoowego sterownika jednostki inercyjnej 33

51 Schemat maniulatora IRP6 wraz z miejscem montowania jednostkiinercyjnej Źroacutedło [20] 36

52 Schemat manipulatora IRP6 w pozycji początkowej do trajektorii te-stowej 37

53 Trajektoria typu pierwszego z minimalnym ustawieniem filtrowaniaw jednostce inercyjnej 38

54 Trajektoria typu pierwszego ze średnim ustawieniem filtrowania w jed-nostce inercyjnej 39

55 Trajektoria typu drugiego z minimalnym ustawieniem filtrowania w jed-nostce inercyjnej 39

56 Część trajektorii uczącej pokazująca ruch wzdłuż osi x 4057 Część trajektorii uczącej pokazująca wibracje związane z ruchem osi y 4058 Część trajektorii uczącej pokazująca skok siły przy nagłym zatrzyma-

niu manipulatora 4159 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xy

wraz z momentem zatrzymania manipulatora 41510 Część trajektorii weryfikującej pokazująca ruch po płaszczyźnie xz 41

B1 Trajektoria ucząca z minimalnym zastosowaniem filtru 58B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu 59B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru 60

Spis rysunkoacutew 45

46 Spis rysunkoacutew

Spis tablic

31 Opis podłączeń do jednostki inercyjnej 1632 Opis podłączeń w złączu międzypłytkowym 1833 Opis połączeń złącza zasilania 1934 Opis połączeń złącza EtherCAT 2035 Opis połączeń złącza EtherCAT 21

41 Opis Obiektoacutew PDO dostępnych w urządzeniu 2642 Opis Obiektoacutew SDO dostępnych w urządzeniu 2743 Opis Obiektoacutew PDO dostępnych w urządzeniu 28

51 Statystyka dotycząca długości wektoroacutew siły 42

Spis tablic 47

48 Spis tablic

Spis Dodatkoacutew

Dodatek A 51

Dodatek B 57

Spis tablic 49

50 Spis tablic

Dodatek A

Schematy modułoacutew urządzenia

Dodatek A Schematy modułoacutew urządzenia 51

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

A

nybu

s-B

ase

SchD

ocD

raw

n B

y

~RES

ET10

3

A0

~WEH

DIP

1_0

104

A1

DIP

1_1

105

A2

DIP

1_2

106

A3

DIP

1_3

107

A4

DIP

1_4

109

A5

DIP

1_5

110

A6

DIP

1_6

111

A7

DIP

1_7

112

A8

~LD

~SS

113

A9

SCLK

115

A10

DO

MIS

O11

6

A11

DIM

OSI

117

A12

ASM

_RX

119

A13

ASM

_TX

120

D7

DIP

2_7

121

D6

DIP

2_6

122

D5

DIP

2_5

123

D4

DIP

2_4

125

D3

DIP

2_3

126

D2

DIP

2_2

127

D1

DIP

2_1

129

MD

013

0D

0D

IP2_

013

1O

M0

132

OM

113

3

OM

213

5~C

S13

6~W

E~W

ELC

T13

7~I

RQ

PA

138

~OE

139

~LED

4AD

1514

1~L

ED4B

D14

142

~LED

3AD

1314

3

~LED

3BD

1214

5LE

D2A

D11

146

LED

2BD

1014

7LE

D1A

D9

148

LED

1BD

814

9

RX

ASM

_RX

151

TXA

SM_T

XO

M3

152

MI1

153

MI0

SY

NC

154

Hos

t Con

nect

or

B Any

bus_

CC

_B40

PK0

18

PK1

19

PK2

20

PK3

21

PC7

22

PC6

23

PC5

24

PC4

25

Dat

aBus

Pin

sU

1E

TM4C

1292

NC

PDTI

3R

PA6

40

PA7

41

PG0

49PG

150

PB3

92

PQ0

5

PQ1

6

PM3

75

PM2

76

PM1

77

PM0

78

PL0

81

PL1

82

PL2

83

PL3

84

PL4

85

PP2

103

Adr

esB

us P

ins

U1F

TM4C

1292

NC

PDTI

3R

D0

D1

D2

D3

D4

D5

D6

D7

D0

D1

D2

D3

D4

D5

D6

D7A

0A

1A

2A

3A

4A

5A

6A

7A

8A

9A

10A

11A

12A

13

A0

A1

A2

A3

A4

A5

A6

A7

A8

A9

A10

A11

A12

A13

PA0

33

PA1

34

PB0

95

U1A

TM4C

1292

NC

PDTI

3R

GN

D

R33

10k

R34

10k

R35

10k

Any

bus

Page

15

Any

bus

uC c

onne

ctio

n

1

Bar

osz

Kac

zor

1

4

PI0103

PI0104

PI0105

PI0106

PI0107

PI0109

PI0110

PI0111

PI0112

PI0113

PI0115

PI0116

PI0117

PI0119

PI0120

PI0121

PI0122

PI0123

PI0125

PI0126

PI0127

PI0129

PI0130

PI0131

PI0132

PI0133

PI0135

PI0136

PI0137

PI0138

PI0139

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

COB

PIR3301 PIR3302 CO

R33

PIR3401 PIR3402 COR34

PIR3501 PIR3502 CO

R35

PIU1033

PIU1034

PIU1095 COU1

A

PIU1018

PIU1019

PIU1020

PIU1021

PIU1022

PIU1023

PIU1024

PIU1025

COU1

E PIU105

PIU106

PIU1040

PIU1041

PIU1049

PIU1050

PIU1075

PIU1076

PIU1077

PIU1078

PIU1081

PIU1082

PIU1083

PIU1084

PIU1085

PIU1092

PIU10103

COU1

F

PI0104

PIU1040

NLA0

PI0105

PIU1041

NLA1

PI0106

PIU1050

NLA2

PI0107

PIU1049

NLA3

PI0109

PIU1075

NLA4

PI0110

PIU1076

NLA5

PI0111

PIU1077

NLA6

PI0112

PIU1078

NLA7

PI0113

PIU1081

NLA8

PI0115

PIU1082

NLA9

PI0116

PIU1083

NLA10

PI0117

PIU1084

NLA11

PI0119

PIU105

NLA12

PI0120

PIU106

NLA13

PI0131

PIU1018

NLD0

PI0129

PIU1019

NLD1

PI0127

PIU1020

NLD2

PI0126

PIU1021

NLD3 PI0125

PIU1022

NLD4

PI0123

PIU1023

NLD5 PI0122

PIU1024

NLD6

PI0121

PIU1025

NLD7

PIR3301 PIR3401

PIR3501

PI0103

PIU1033

PI0130

PI0132

PIR3302

PI0133

PIR3402

PI0135

PIR3502

PI0136

PIU1085

PI0137

PIU10103

PI0138

PIU1095

PI0139

PIU1092

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

PIU1034

52 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

C

onne

ctor

sSch

Doc

Dra

wn

By

PQ2

11

PQ3

27

PQ5

57

PQ6

58

PM7

71PM

672

PM5

73PM

474

PL5

86

PL7

93PL

694

PQ4

102

PP3

104

PP4

105

PP5

106

PN0

107

PN1

108

PN2

109

PN3

110

PN4

111

PN5

112

PP0

118

PP1

119

U1B

TM4C

1292

NC

PDTI

3R

PD0

1

PD1

2

PD2

3

PD3

4

PE3

12PE

213

PE1

14PE

015

PH0

29

PH1

30

PH2

31

PH3

32

PA2

35

PA3

36

PA4

37

PA5

38

PF0

42

PF1

43

PF2

44

PF3

45

PF4

46

PG2

51

PG3

52

PG4

53

PG5

54

PG6

55

PG7

56

PK7

59PK

660

PK5

61PK

462

PB2

91PB

196

PC3_

TDO

_SW

O97

PC2_

TDI

98PC

1_TM

S_SW

DIO

99PC

0_TC

K_S

WC

LK10

0

PJ0

116

PJ1

117

PB5

120

PB4

121

PE4

123

PE5

124

PD4

125

PD5

126

PD6

127

PD7

128

U1G

TM4C

1292

NC

PDTI

3R

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PH0

PH1

PH2

PH3

PJ0

PJ1

PK4

PK5

PK6

PK7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

PC0

PC1

PC2

PC3

3V3

GN

D

RST

92

101

74

83 5

6

J2 Pin

200

mm

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

Con

nect

or s

chem

a

1

Bar

tosz

Kac

zor

2

4

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIJ201

PIJ202

PIJ203

PIJ204

PIJ205

PIJ206

PIJ207

PIJ208

PIJ209

PIJ2010

COJ2

PIU1011

PIU1027

PIU1057

PIU1058

PIU1071

PIU1072

PIU1073

PIU1074

PIU1086

PIU1093

PIU1094

PIU10102

PIU10104

PIU10105

PIU10106

PIU10107

PIU10108

PIU10109

PIU10110

PIU10111

PIU10112

PIU10118

PIU10119

COU1B

PIU101

PIU102

PIU103

PIU104

PIU1012

PIU1013

PIU1014

PIU1015

PIU1029

PIU1030

PIU1031

PIU1032

PIU1035

PIU1036

PIU1037

PIU1038

PIU1042

PIU1043

PIU1044

PIU1045

PIU1046

PIU1051

PIU1052

PIU1053

PIU1054

PIU1055

PIU1056

PIU1059

PIU1060

PIU1061

PIU1062

PIU1091

PIU1096

PIU1097

PIU1098

PIU1099

PIU10100

PIU10116

PIU10117

PIU10120

PIU10121

PIU10123

PIU10124

PIU10125

PIU10126

PIU10127

PIU10128

COU1G

PI1041

PI1080

PIJ201

PI1059

PI1060

PI1061

PI1062

PI1042

PI1043

PI1078

PI1079

PIJ202

PIJ203

PIJ205

PIJ204

PI1063

PIU1035

NLPA2

PI1066

PIU1036

NLPA3

PI1067

PIU1037

NLPA4

PI1058

PIU1038

NLPA5

PI102

PIU1096

NLPB1

PI1045

PIU1091

NLPB2

PI1029

PIU10121

NLPB4

PI1030

PIU10120

NLPB5

PI103

PIJ209

PIU10100

NLPC0

PI1038

PIJ2010

PIU1099

NLPC

1

PI1039

PIJ207

PIU1098

NLPC2

PI1040

PIJ208

PIU1097

NLPC3

PI1019

PIU101

NLPD

0

PI1022

PIU102

NLPD1

PI1021

PIU103

NLPD

2

PI1020

PIU104

NLPD3

PI1026

PIU10125

NLPD

4

PI1025

PIU10126

NLPD5

PI1024

PIU10127

NLPD

6

PI1023

PIU10128

NLPD

7

PI1014

PIU1015

NLPE0

PI1015

PIU1014

NLPE1

PI1016

PIU1013

NLPE2

PI1017

PIU1012

NLPE3

PI1028

PIU10123

NLPE4

PI1027

PIU10124

NLPE5

PI1057

PIU1042

NLPF0

PI1068

PIU1043

NLPF1

PI1056

PIU1044

NLPF2

PI1069

PIU1045

NLPF3

PI1055

PIU1046

NLPF4

PI1054

PIU1051

NLPG2

PI1070

PIU1052

NLPG3

PI1071

PIU1053

NLPG4

PI1053

PIU1054

NLPG5

PI1052

PIU1055

NLPG6

PI1072

PIU1056

NLPG7

PI1010

PIU1029

NLPH0

PI109

PIU1030

NLPH1

PI1065

PIU1031

NLPH2

PI1064

PIU1032

NLPH3

PI1032

PIU10116

NLPJ0

PI1011

PIU10117

NLPJ1

PI1076

PIU1062

NLPK4

PI1049

PIU1061

NLPK5

PI1075

PIU1060

NLPK6

PI1050

PIU1059

NLPK7

PI1077

PIU1086

NLPL5

PI101

PIU1094

NLPL6

PI1044

PIU1093

NLPL7

PI1046

PIU1074

NLPM

4

PI1047

PIU1073

NLPM

5

PI1048

PIU1072

NLPM

6

PI1074

PIU1071

NLPM

7

PI1035

PIU10107

NLPN0

PI1034

PIU10108

NLPN1

PI106

PIU10109

NLPN2

PI107

PIU10110

NLPN3

PI1033

PIU10111

NLPN4

PI108

PIU10112

NLPN5

PI1031

PIU10118

NLPP0

PI1012

PIU10119

NLPP1

PI104

PIU10104

NLPP3

PI1036

PIU10105

NLPP4

PI105

PIU10106

NLPP5

PI1018

PIU1011

NLPQ

2

PI1013

PIU1027

NLPQ3

PI1037

PIU10102

NLPQ

4

PI1051

PIU1057

NLPQ5

PI1073

PIU1058

NLPQ

6

PIJ206

NLRST

Dodatek A Schematy modułoacutew urządzenia 53

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

N

etw

ork

SchD

ocD

raw

n B

y

B_1

PR

XP

3

B_1

CEN

SD

A4

B_1

NR

XN

5

B_2

PSD

P7

B_2

CEN

SC

L8

B_2

NS

DN

9

B_3

PTX

EN11

B_3

CEN

12

B_3

NT

XD

IS13

B_4

PTX

P15

B_4

CEN

~B

USP

16

B_4

NT

XN

17

NW

_LED

4B19

NW

_LED

4A20

NW

_LED

3B21

NW

_LED

3A22

NW

_LED

2B23

NW

_LED

2A24

NW

_LED

1B25

NW

_LED

1A26

A_1

PR

XP

29A

_1C

ENS

DA

30A

_1N

RX

N31

A_2

PSD

P33

A_2

CEN

SC

L34

A_2

NS

DN

35

A_3

PTX

EN37

A_3

CEN

38A

_3N

TX

DIS

39

A_4

PTX

P41

A_4

CEN

~B

USP

42A

_4N

TX

N43

C_T

X45

C_R

X46

C_T

XEN

47~C

_BU

SP48

GA

TE1

49G

ATE

250

Net

wor

k C

onne

ctor

A Any

bus_

CC

_B40

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T1 47

uHB_

1PB_

1CEN

B_1N

B_1P

B_1C

EN

B_1N

B_2P

B_2C

ENB_

2N

B_2P

B_2C

EN

B_2N

16V

100n

F

C1

16V

100n

F

C2

GN

DS

PE

2kV

1nF

C3

R1

75R

275

R5

1M2k

V1n

F

C4 PE

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T2 47

uH

A_1

P

A_1

CEN

A_1

N

A_2

P

A_2

CEN

A_2

N

16V

100n

F

C5

16V

100n

F

C6

GN

DS

PE

2kV

1nF

C7

R6

75R

775

A_1

PA

_1C

ENA

_1N

A_2

PA

_2C

ENA

_2N

DL1

R18

220

R17

220

R16

220

R15

220

R14

220

R13

220

R12

220

R11

220

DL2

DL3

DL4

GN

DS

3210 4

J4 Clic

k_M

ate

5x1

3210 4

J5 Clic

k_M

ate

5x1

Net

wor

k co

nnec

tor

1

3

4B

arto

sz k

aczo

r

PI03

PI04

PI05

PI07

PI08

PI09

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PI020

PI021

PI022

PI023

PI024

PI025

PI026

PI029

PI030

PI031

PI033

PI034

PI035

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

COA

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIC301 PIC302

COC3

PIC401 PIC402

COC4

PIC501 PIC502

COC5

PIC601 PIC602

COC6

PIC701 PIC702

COC7

PIDL101 PIDL102

PIDL103 PIDL104 CODL1

PIDL201 PIDL202

PIDL203 PIDL204 CODL2

PIDL301 PIDL302

PIDL303 PIDL304 CODL3

PIDL401 PIDL402

PIDL403 PIDL404 CODL4

PIJ400

PIJ401

PIJ402

PIJ403

PIJ404

COJ4 PIJ500

PIJ501

PIJ502

PIJ503

PIJ504

COJ5

PIR101 PIR102 COR

1 PIR201 PIR202 COR

2

PIR501 PIR502 COR

5

PIR601 PIR602 COR

6 PIR701 PIR702 COR

7

PIR1101 PIR1102 COR1

1

PIR1201 PIR1202 COR12

PIR1301 PIR1302 COR13

PIR1401 PIR1402 COR14

PIR1501 PIR1502 COR15

PIR1601 PIR1602 COR16

PIR1701 PIR1702 COR17 PIR1801 PIR1802

COR1

8

PIT101

PIT102

PIT103

PIT104

PIT105

PIT106

PIT107

PIT108

PIT109

PIT1010

PIT1011

PIT1012

COT1

PIT201

PIT202

PIT203

PIT204

PIT205

PIT206

PIT207

PIT208

PIT209

PIT2010

PIT2011

PIT2012

COT2

PI030

PIC501

PIT203

NLA01CEN

PI031

PIT202

NLA0

1N

PI029

PIT201

NLA0

1P

PI034

PIC601

PIT204

NLA02CEN

PI035

PIT206

NLA0

2N

PI033

PIT205

NLA0

2P

PI04

PIC101

PIT103

NLB01CEN

PI05

PIT102

NLB0

1N

PI03

PIT101

NLB0

1P

PI08

PIC201

PIT104

NLB02CEN

PI09

PIT106

NLB0

2N

PI07

PIT105

NLB0

2P

PIC102 PIC202

PIC502 PIC602

PIDL101 PIDL103

PIDL201 PIDL203

PIDL301 PIDL303

PIDL401 PIDL403

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PIR1102

PI020

PIR1202

PI021

PIR1302

PI022

PIR1402

PI023

PIR1502

PI024

PIR1602 PI025

PIR1702 PI026

PIR1802

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

PIC301 PIR101

PIR201

PIC401

PIJ404 PIJ504

PIR502

PIC701 PIR601

PIR701

PIDL102

PIR1701

PIDL104

PIR1801

PIDL202

PIR1501

PIDL204

PIR1601

PIDL302

PIR1301

PIDL304

PIR1401

PIDL402

PIR1101

PIDL404

PIR1201

PIJ400

PIT1012

PIJ401

PIT1011

PIJ402

PIT108

PIJ403

PIT107

PIJ500

PIT2012

PIJ501

PIT2011

PIJ502

PIT208

PIJ503

PIT207 PIR102

PIT1010

PIR202

PIT109

PIR602

PIT2010

PIR702

PIT209

PIC302

PIC402

PIC702

PIR501

54 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

P

ower

Supp

lyS

chD

ocD

raw

n B

y

5V GN

D

GN

D

3V3

GN

DS

GN

DS

3V3 R

pu10

k 16V

10nF

C21

GN

DS

Rs

470

TAC

TD60

B1

GN

DS

16V

10uF

Cin

R20

510k

Rpg

499k

R21

162k

16V

22uF

Cou

t

16V

4uF

Cld

o

3V3

GN

DS

GN

DS

16V

12pF

Cx2

16V

12pF

Cx1

GN

DS

3V3

1

GN

D2

GN

D6

GN

D10

GN

D14

GN

D18

3V3

27

GN

D28

GN

D32

GN

D36

GN

D40

GN

D44

3V3

51

GN

D52

Pow

er p

ins

3V3

101

GN

D10

2

GN

D10

8

GN

D11

4

GN

D11

8

GN

D12

4

GN

D12

8

GN

D13

4G

ND

140

GN

D14

4G

ND

150

3V3

155

GN

D15

6

C Any

bus_

CC

_B40

3V3

GN

DS

GN

DG

ND

S

WA

KE_

N64

HIB

_N65

XO

SC0

66

XO

SC1

67

RST

_N70

OSC

088

OSC

189

U1C

TM4C

1292

NC

PDTI

3R

VD

D7

VD

DA

8

VR

EFA

+9

GN

DA

10

VD

D16

GN

D17

VD

D26

VD

D28

VD

D39

VD

D47

GN

D48

VD

D63

VB

AT

68

VD

D69

VD

D79

GN

D80

VD

DC

87

VD

D90

VD

D10

1

VD

D11

3

GN

D11

4

VD

DC

115

VD

D12

2

U1D

TM4C

1292

NC

PDTI

3R

RST

GN

DX

18M

Hz

GN

D

EN1

PG2

VIN

3

VIN

4G

ND

5G

ND

6FB

7V

OU

T8

TPS8

2085

GN

D9

U2

TPS8

2085

GN

D

R66

500k

2kV

1nF

C55 PE

GN

DR

5550

0k2k

V1n

F

C44 PE

GN

D

R77

500k

2kV

1nF

C66 PE

GN

D

16V

100n

F

CV

116

V10

0nF

CV

216

V10

0nF

CV

316

V10

0nF

CV

4

3V3

16V

100n

F

CV

516

V10

0nF

CV

616

V10

0nF

CV

716

V10

0nF

CV

816

V10

0nF

CV

916

V10

0nF

CV

1016

V10

0nF

CV

1116

V10

0nF

CV

1216

V10

0nF

CV

13

GN

D

16V

1uF

CV

ref1

16V

10nF

CV

ref2

GN

DG

ND Po

wer

supp

ly

1

4

4 B

arto

sz K

aczo

r

PI01

PI02

PI06

PI010

PI014

PI018

PI027

PI028

PI032

PI036

PI040

PI044

PI051

PI052

PI0101

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0155

PI0156

COC

PIB101

PIB102

PIB103 PI

B104 COB1

PIC2101 PIC2102

COC2

1

PIC4401 PIC4402

COC44

PIC5501 PIC5502

COC55

PIC6601 PIC6602

COC66

PICin01 PICin02

COCin

PICldo01 PICldo02

COCl

do

PICout01 PICout02

COCo

ut

PICV101 PICV102

COCV1

PICV201 PICV202

COCV

2 PICV301 PICV302

COCV

3 PICV401 PICV402

COCV

4 PICV501 PICV502

COCV5

PICV601 PICV602

COCV

6 PICV701 PICV702

COCV

7 PICV801 PICV802

COCV

8 PICV901 PICV902

COCV

9 PICV1001 PICV1002

COCV10

PICV1101 PICV1102

COCV11

PICV1201 PICV1202

COCV12

PICV1301 PICV1302

COCV13

PICVref101 PICVref102

COCV

ref1

PICVref201 PICVref202

COCVref2

PICx101 PICx102

COCx1

PICx201 PICx202

COCx2

PIR2001 PIR2002 COR20

PIR2

101

PIR2

102 COR

21

PIR5501 PIR5502 COR55

PIR6601 PIR6602 COR

66

PIR7701 PIR7702 CO

R77

PIRpg01 PIRpg02 CO

Rpg

PIRpu01 PIRpu02 CORpu

PIRs01

PIRs

02

CORs

PIU1064

PIU1065

PIU1066

PIU1067

PIU1070

PIU1088

PIU1089

COU1

C

PIU107

PIU108

PIU109

PIU1010

PIU1016

PIU1017

PIU1026

PIU1028

PIU1039

PIU1047

PIU1048

PIU1063

PIU1068

PIU1069

PIU1079

PIU1080

PIU1087

PIU1090

PIU10101

PIU10113

PIU10114

PIU10115

PIU10122

COU1

D

PIU201

PIU202

PIU203

PIU204

PIU205

PIU206

PIU207

PIU208

PIU209

COU2

PIX101 PIX102 PIX10GND

PIX10GND2 COX

1

PI01

PI027

PI051

PI0101

PI0155

PICout01

PICV101 PICV201

PICV301 PICV401

PICV501 PICV601

PICV701 PICV801

PICV901 PICV1001

PICV1101 PICV1201

PICV1301

PICVref101 PICVref201

PIR2002 PIRpg02

PIRpu02

PIU107

PIU108

PIU109

PIU1016

PIU1026

PIU1028

PIU1039

PIU1047

PIU1063

PIU1068

PIU1069

PIU1079

PIU1090

PIU10101

PIU10113

PIU10122

PIU208

PICin01

PIU201

PIU203

PIU204

PI02

PI06

PI010

PI014

PI018

PI028

PI032

PI036

PI040

PI044

PI052

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0156

PIB101

PIB102

PIC2102

PIC4401 PIC5501

PIC6601

PICin02

PICldo02

PICout02

PICV102 PICV202

PICV302 PICV402

PICV502 PICV602

PICV702 PICV802

PICV902 PICV1002

PICV1102 PICV1202

PICV1302

PICVref102 PICVref202

PICx102 PICx202

PIR2

101

PIR5502 PIR6602

PIR7702

PIU1010

PIU1017

PIU1048

PIU1064

PIU1080

PIU1088

PIU10114

PIU205

PIU206

PIU209

PIX10GND

PIX10GND2

PIB103 PI

B104 PIRs01

PICldo01

PIU1087

PIU10115

PICx101

PIU1067

PIX101

PICx201

PIU1066

PIX102

PIR2001 PIR2

102

PIU207

PIRpg01

PIU202

PIU1065

PIU1089

PIC4402 PIC5502

PIC6602

PIR5501 PIR6601

PIR7701

PIC2101 PIRpu01 PI

Rs02

PIU1070

NLRST

Dodatek A Schematy modułoacutew urządzenia 55

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-15

Shee

t o

fFi

le

CU

sers

IM

UO

ldS

chD

ocD

raw

n B

y

8642 16141210 18 20 22 24

17 19 21 239 11 13 157531J5 H

eade

r 12x

2

5V

GN

D

5V

16V

100n

F

C1

16V

100n

F

C2

GN

DRS

TCSD

OU

TD

IO4

DIO

3SC

LKD

IND

IO1

DIO

2

PA2

PA3

PA4

PA5

PB1

PB2

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

1 23 4

J2

5V

GN

D

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIJ201

PIJ202

PIJ203

PIJ204

COJ2

PIJ501

PIJ502

PIJ503

PIJ504

PIJ505

PIJ506

PIJ507

PIJ508

PIJ509

PIJ5

010

PIJ5011

PIJ5012

PIJ5013

PIJ5014

PIJ5

015

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

COJ5

PI1041

PI1080

PI1059

PI1060

PI1061

PI1062

PIC101 PIC201

PIJ201

PIJ5

010

PIJ5011

PIJ5012

PI1066

PIJ506

NLCS

NLPA3

PI1067

PIJ505

NLDIN

NLPA4

PIJ507

NLDI

O1 PIJ509

NLDI

O2 PIJ501

NLDI

O3

PI1045

PIJ502

NLDI

O4

NLPB2

PI1058

PIJ504

NLDOUT

NLPA5

PI1042

PI1043

PI1078

PI1079

PIC102 PIC202

PIJ204

PIJ5013

PIJ5014

PIJ5

015

PIJ202

PIJ203

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

PI1063

PIJ503

NLPA2

NLSCLK

PI102

PIJ508

NLPB1

NLRS

T

PI1029

NLPB4

PI1030

NLPB5

PI103

NLPC0

PI1038

NLPC1

PI1039

NLPC2

PI1040

NLPC3

PI1019

NLPD0

PI1022

NLPD1

PI1021

NLPD2

PI1020

NLPD3

PI1026

NLPD4

PI1025

NLPD5

PI1024

NLPD6

PI1023

NLPD7

PI1014

NLPE0

PI1015

NLPE

1

PI1016

NLPE2

PI1017

NLPE3

PI1028

NLPE4

PI1027

NLPE

5

PI1057

NLPF

0

PI1068

NLPF

1

PI1056

NLPF2

PI1069

NLPF3

PI1055

NLPF

4

PI1054

NLPG2

PI1070

NLPG3

PI1071

NLPG4

PI1053

NLPG5

PI1052

NLPG6

PI1072

NLPG7

PI1010

NLPH0

PI109

NLPH1

PI1065

NLPH2

PI1064

NLPH3

PI1032

NLPJ0

PI1011

NLPJ1

PI1076

NLPK4

PI1049

NLPK5

PI1075

NLPK6

PI1050

NLPK7

PI1077

NLPL5

PI101

NLPL6

PI1044

NLPL7

PI1046

NLPM

4

PI1047

NLPM

5

PI1048

NLPM

6

PI1074

NLPM

7

PI1035

NLPN0

PI1034

NLPN1

PI106

NLPN2

PI107

NLPN3

PI1033

NLPN4

PI108

NLPN5

PI1031

NLPP0

PI1012

NLPP1

PI104

NLPP3

PI1036

NLPP4

PI105

NLPP5

PI1018

NLPQ2

PI1013

NLPQ3

PI1037

NLPQ4

PI1051

NLPQ5

PI1073

NLPQ6

56 Dodatek A Schematy modułoacutew urządzenia

Dodatek B

Wykresy pobranych trajektorii

Dodatek B Wykresy pobranych trajektorii 57

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B1 Trajektoria ucząca z minimalnym zastosowaniem filtru

58 Dodatek B Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

46 Spis rysunkoacutew

Spis tablic

31 Opis podłączeń do jednostki inercyjnej 1632 Opis podłączeń w złączu międzypłytkowym 1833 Opis połączeń złącza zasilania 1934 Opis połączeń złącza EtherCAT 2035 Opis połączeń złącza EtherCAT 21

41 Opis Obiektoacutew PDO dostępnych w urządzeniu 2642 Opis Obiektoacutew SDO dostępnych w urządzeniu 2743 Opis Obiektoacutew PDO dostępnych w urządzeniu 28

51 Statystyka dotycząca długości wektoroacutew siły 42

Spis tablic 47

48 Spis tablic

Spis Dodatkoacutew

Dodatek A 51

Dodatek B 57

Spis tablic 49

50 Spis tablic

Dodatek A

Schematy modułoacutew urządzenia

Dodatek A Schematy modułoacutew urządzenia 51

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

A

nybu

s-B

ase

SchD

ocD

raw

n B

y

~RES

ET10

3

A0

~WEH

DIP

1_0

104

A1

DIP

1_1

105

A2

DIP

1_2

106

A3

DIP

1_3

107

A4

DIP

1_4

109

A5

DIP

1_5

110

A6

DIP

1_6

111

A7

DIP

1_7

112

A8

~LD

~SS

113

A9

SCLK

115

A10

DO

MIS

O11

6

A11

DIM

OSI

117

A12

ASM

_RX

119

A13

ASM

_TX

120

D7

DIP

2_7

121

D6

DIP

2_6

122

D5

DIP

2_5

123

D4

DIP

2_4

125

D3

DIP

2_3

126

D2

DIP

2_2

127

D1

DIP

2_1

129

MD

013

0D

0D

IP2_

013

1O

M0

132

OM

113

3

OM

213

5~C

S13

6~W

E~W

ELC

T13

7~I

RQ

PA

138

~OE

139

~LED

4AD

1514

1~L

ED4B

D14

142

~LED

3AD

1314

3

~LED

3BD

1214

5LE

D2A

D11

146

LED

2BD

1014

7LE

D1A

D9

148

LED

1BD

814

9

RX

ASM

_RX

151

TXA

SM_T

XO

M3

152

MI1

153

MI0

SY

NC

154

Hos

t Con

nect

or

B Any

bus_

CC

_B40

PK0

18

PK1

19

PK2

20

PK3

21

PC7

22

PC6

23

PC5

24

PC4

25

Dat

aBus

Pin

sU

1E

TM4C

1292

NC

PDTI

3R

PA6

40

PA7

41

PG0

49PG

150

PB3

92

PQ0

5

PQ1

6

PM3

75

PM2

76

PM1

77

PM0

78

PL0

81

PL1

82

PL2

83

PL3

84

PL4

85

PP2

103

Adr

esB

us P

ins

U1F

TM4C

1292

NC

PDTI

3R

D0

D1

D2

D3

D4

D5

D6

D7

D0

D1

D2

D3

D4

D5

D6

D7A

0A

1A

2A

3A

4A

5A

6A

7A

8A

9A

10A

11A

12A

13

A0

A1

A2

A3

A4

A5

A6

A7

A8

A9

A10

A11

A12

A13

PA0

33

PA1

34

PB0

95

U1A

TM4C

1292

NC

PDTI

3R

GN

D

R33

10k

R34

10k

R35

10k

Any

bus

Page

15

Any

bus

uC c

onne

ctio

n

1

Bar

osz

Kac

zor

1

4

PI0103

PI0104

PI0105

PI0106

PI0107

PI0109

PI0110

PI0111

PI0112

PI0113

PI0115

PI0116

PI0117

PI0119

PI0120

PI0121

PI0122

PI0123

PI0125

PI0126

PI0127

PI0129

PI0130

PI0131

PI0132

PI0133

PI0135

PI0136

PI0137

PI0138

PI0139

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

COB

PIR3301 PIR3302 CO

R33

PIR3401 PIR3402 COR34

PIR3501 PIR3502 CO

R35

PIU1033

PIU1034

PIU1095 COU1

A

PIU1018

PIU1019

PIU1020

PIU1021

PIU1022

PIU1023

PIU1024

PIU1025

COU1

E PIU105

PIU106

PIU1040

PIU1041

PIU1049

PIU1050

PIU1075

PIU1076

PIU1077

PIU1078

PIU1081

PIU1082

PIU1083

PIU1084

PIU1085

PIU1092

PIU10103

COU1

F

PI0104

PIU1040

NLA0

PI0105

PIU1041

NLA1

PI0106

PIU1050

NLA2

PI0107

PIU1049

NLA3

PI0109

PIU1075

NLA4

PI0110

PIU1076

NLA5

PI0111

PIU1077

NLA6

PI0112

PIU1078

NLA7

PI0113

PIU1081

NLA8

PI0115

PIU1082

NLA9

PI0116

PIU1083

NLA10

PI0117

PIU1084

NLA11

PI0119

PIU105

NLA12

PI0120

PIU106

NLA13

PI0131

PIU1018

NLD0

PI0129

PIU1019

NLD1

PI0127

PIU1020

NLD2

PI0126

PIU1021

NLD3 PI0125

PIU1022

NLD4

PI0123

PIU1023

NLD5 PI0122

PIU1024

NLD6

PI0121

PIU1025

NLD7

PIR3301 PIR3401

PIR3501

PI0103

PIU1033

PI0130

PI0132

PIR3302

PI0133

PIR3402

PI0135

PIR3502

PI0136

PIU1085

PI0137

PIU10103

PI0138

PIU1095

PI0139

PIU1092

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

PIU1034

52 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

C

onne

ctor

sSch

Doc

Dra

wn

By

PQ2

11

PQ3

27

PQ5

57

PQ6

58

PM7

71PM

672

PM5

73PM

474

PL5

86

PL7

93PL

694

PQ4

102

PP3

104

PP4

105

PP5

106

PN0

107

PN1

108

PN2

109

PN3

110

PN4

111

PN5

112

PP0

118

PP1

119

U1B

TM4C

1292

NC

PDTI

3R

PD0

1

PD1

2

PD2

3

PD3

4

PE3

12PE

213

PE1

14PE

015

PH0

29

PH1

30

PH2

31

PH3

32

PA2

35

PA3

36

PA4

37

PA5

38

PF0

42

PF1

43

PF2

44

PF3

45

PF4

46

PG2

51

PG3

52

PG4

53

PG5

54

PG6

55

PG7

56

PK7

59PK

660

PK5

61PK

462

PB2

91PB

196

PC3_

TDO

_SW

O97

PC2_

TDI

98PC

1_TM

S_SW

DIO

99PC

0_TC

K_S

WC

LK10

0

PJ0

116

PJ1

117

PB5

120

PB4

121

PE4

123

PE5

124

PD4

125

PD5

126

PD6

127

PD7

128

U1G

TM4C

1292

NC

PDTI

3R

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PH0

PH1

PH2

PH3

PJ0

PJ1

PK4

PK5

PK6

PK7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

PC0

PC1

PC2

PC3

3V3

GN

D

RST

92

101

74

83 5

6

J2 Pin

200

mm

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

Con

nect

or s

chem

a

1

Bar

tosz

Kac

zor

2

4

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIJ201

PIJ202

PIJ203

PIJ204

PIJ205

PIJ206

PIJ207

PIJ208

PIJ209

PIJ2010

COJ2

PIU1011

PIU1027

PIU1057

PIU1058

PIU1071

PIU1072

PIU1073

PIU1074

PIU1086

PIU1093

PIU1094

PIU10102

PIU10104

PIU10105

PIU10106

PIU10107

PIU10108

PIU10109

PIU10110

PIU10111

PIU10112

PIU10118

PIU10119

COU1B

PIU101

PIU102

PIU103

PIU104

PIU1012

PIU1013

PIU1014

PIU1015

PIU1029

PIU1030

PIU1031

PIU1032

PIU1035

PIU1036

PIU1037

PIU1038

PIU1042

PIU1043

PIU1044

PIU1045

PIU1046

PIU1051

PIU1052

PIU1053

PIU1054

PIU1055

PIU1056

PIU1059

PIU1060

PIU1061

PIU1062

PIU1091

PIU1096

PIU1097

PIU1098

PIU1099

PIU10100

PIU10116

PIU10117

PIU10120

PIU10121

PIU10123

PIU10124

PIU10125

PIU10126

PIU10127

PIU10128

COU1G

PI1041

PI1080

PIJ201

PI1059

PI1060

PI1061

PI1062

PI1042

PI1043

PI1078

PI1079

PIJ202

PIJ203

PIJ205

PIJ204

PI1063

PIU1035

NLPA2

PI1066

PIU1036

NLPA3

PI1067

PIU1037

NLPA4

PI1058

PIU1038

NLPA5

PI102

PIU1096

NLPB1

PI1045

PIU1091

NLPB2

PI1029

PIU10121

NLPB4

PI1030

PIU10120

NLPB5

PI103

PIJ209

PIU10100

NLPC0

PI1038

PIJ2010

PIU1099

NLPC

1

PI1039

PIJ207

PIU1098

NLPC2

PI1040

PIJ208

PIU1097

NLPC3

PI1019

PIU101

NLPD

0

PI1022

PIU102

NLPD1

PI1021

PIU103

NLPD

2

PI1020

PIU104

NLPD3

PI1026

PIU10125

NLPD

4

PI1025

PIU10126

NLPD5

PI1024

PIU10127

NLPD

6

PI1023

PIU10128

NLPD

7

PI1014

PIU1015

NLPE0

PI1015

PIU1014

NLPE1

PI1016

PIU1013

NLPE2

PI1017

PIU1012

NLPE3

PI1028

PIU10123

NLPE4

PI1027

PIU10124

NLPE5

PI1057

PIU1042

NLPF0

PI1068

PIU1043

NLPF1

PI1056

PIU1044

NLPF2

PI1069

PIU1045

NLPF3

PI1055

PIU1046

NLPF4

PI1054

PIU1051

NLPG2

PI1070

PIU1052

NLPG3

PI1071

PIU1053

NLPG4

PI1053

PIU1054

NLPG5

PI1052

PIU1055

NLPG6

PI1072

PIU1056

NLPG7

PI1010

PIU1029

NLPH0

PI109

PIU1030

NLPH1

PI1065

PIU1031

NLPH2

PI1064

PIU1032

NLPH3

PI1032

PIU10116

NLPJ0

PI1011

PIU10117

NLPJ1

PI1076

PIU1062

NLPK4

PI1049

PIU1061

NLPK5

PI1075

PIU1060

NLPK6

PI1050

PIU1059

NLPK7

PI1077

PIU1086

NLPL5

PI101

PIU1094

NLPL6

PI1044

PIU1093

NLPL7

PI1046

PIU1074

NLPM

4

PI1047

PIU1073

NLPM

5

PI1048

PIU1072

NLPM

6

PI1074

PIU1071

NLPM

7

PI1035

PIU10107

NLPN0

PI1034

PIU10108

NLPN1

PI106

PIU10109

NLPN2

PI107

PIU10110

NLPN3

PI1033

PIU10111

NLPN4

PI108

PIU10112

NLPN5

PI1031

PIU10118

NLPP0

PI1012

PIU10119

NLPP1

PI104

PIU10104

NLPP3

PI1036

PIU10105

NLPP4

PI105

PIU10106

NLPP5

PI1018

PIU1011

NLPQ

2

PI1013

PIU1027

NLPQ3

PI1037

PIU10102

NLPQ

4

PI1051

PIU1057

NLPQ5

PI1073

PIU1058

NLPQ

6

PIJ206

NLRST

Dodatek A Schematy modułoacutew urządzenia 53

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

N

etw

ork

SchD

ocD

raw

n B

y

B_1

PR

XP

3

B_1

CEN

SD

A4

B_1

NR

XN

5

B_2

PSD

P7

B_2

CEN

SC

L8

B_2

NS

DN

9

B_3

PTX

EN11

B_3

CEN

12

B_3

NT

XD

IS13

B_4

PTX

P15

B_4

CEN

~B

USP

16

B_4

NT

XN

17

NW

_LED

4B19

NW

_LED

4A20

NW

_LED

3B21

NW

_LED

3A22

NW

_LED

2B23

NW

_LED

2A24

NW

_LED

1B25

NW

_LED

1A26

A_1

PR

XP

29A

_1C

ENS

DA

30A

_1N

RX

N31

A_2

PSD

P33

A_2

CEN

SC

L34

A_2

NS

DN

35

A_3

PTX

EN37

A_3

CEN

38A

_3N

TX

DIS

39

A_4

PTX

P41

A_4

CEN

~B

USP

42A

_4N

TX

N43

C_T

X45

C_R

X46

C_T

XEN

47~C

_BU

SP48

GA

TE1

49G

ATE

250

Net

wor

k C

onne

ctor

A Any

bus_

CC

_B40

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T1 47

uHB_

1PB_

1CEN

B_1N

B_1P

B_1C

EN

B_1N

B_2P

B_2C

ENB_

2N

B_2P

B_2C

EN

B_2N

16V

100n

F

C1

16V

100n

F

C2

GN

DS

PE

2kV

1nF

C3

R1

75R

275

R5

1M2k

V1n

F

C4 PE

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T2 47

uH

A_1

P

A_1

CEN

A_1

N

A_2

P

A_2

CEN

A_2

N

16V

100n

F

C5

16V

100n

F

C6

GN

DS

PE

2kV

1nF

C7

R6

75R

775

A_1

PA

_1C

ENA

_1N

A_2

PA

_2C

ENA

_2N

DL1

R18

220

R17

220

R16

220

R15

220

R14

220

R13

220

R12

220

R11

220

DL2

DL3

DL4

GN

DS

3210 4

J4 Clic

k_M

ate

5x1

3210 4

J5 Clic

k_M

ate

5x1

Net

wor

k co

nnec

tor

1

3

4B

arto

sz k

aczo

r

PI03

PI04

PI05

PI07

PI08

PI09

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PI020

PI021

PI022

PI023

PI024

PI025

PI026

PI029

PI030

PI031

PI033

PI034

PI035

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

COA

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIC301 PIC302

COC3

PIC401 PIC402

COC4

PIC501 PIC502

COC5

PIC601 PIC602

COC6

PIC701 PIC702

COC7

PIDL101 PIDL102

PIDL103 PIDL104 CODL1

PIDL201 PIDL202

PIDL203 PIDL204 CODL2

PIDL301 PIDL302

PIDL303 PIDL304 CODL3

PIDL401 PIDL402

PIDL403 PIDL404 CODL4

PIJ400

PIJ401

PIJ402

PIJ403

PIJ404

COJ4 PIJ500

PIJ501

PIJ502

PIJ503

PIJ504

COJ5

PIR101 PIR102 COR

1 PIR201 PIR202 COR

2

PIR501 PIR502 COR

5

PIR601 PIR602 COR

6 PIR701 PIR702 COR

7

PIR1101 PIR1102 COR1

1

PIR1201 PIR1202 COR12

PIR1301 PIR1302 COR13

PIR1401 PIR1402 COR14

PIR1501 PIR1502 COR15

PIR1601 PIR1602 COR16

PIR1701 PIR1702 COR17 PIR1801 PIR1802

COR1

8

PIT101

PIT102

PIT103

PIT104

PIT105

PIT106

PIT107

PIT108

PIT109

PIT1010

PIT1011

PIT1012

COT1

PIT201

PIT202

PIT203

PIT204

PIT205

PIT206

PIT207

PIT208

PIT209

PIT2010

PIT2011

PIT2012

COT2

PI030

PIC501

PIT203

NLA01CEN

PI031

PIT202

NLA0

1N

PI029

PIT201

NLA0

1P

PI034

PIC601

PIT204

NLA02CEN

PI035

PIT206

NLA0

2N

PI033

PIT205

NLA0

2P

PI04

PIC101

PIT103

NLB01CEN

PI05

PIT102

NLB0

1N

PI03

PIT101

NLB0

1P

PI08

PIC201

PIT104

NLB02CEN

PI09

PIT106

NLB0

2N

PI07

PIT105

NLB0

2P

PIC102 PIC202

PIC502 PIC602

PIDL101 PIDL103

PIDL201 PIDL203

PIDL301 PIDL303

PIDL401 PIDL403

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PIR1102

PI020

PIR1202

PI021

PIR1302

PI022

PIR1402

PI023

PIR1502

PI024

PIR1602 PI025

PIR1702 PI026

PIR1802

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

PIC301 PIR101

PIR201

PIC401

PIJ404 PIJ504

PIR502

PIC701 PIR601

PIR701

PIDL102

PIR1701

PIDL104

PIR1801

PIDL202

PIR1501

PIDL204

PIR1601

PIDL302

PIR1301

PIDL304

PIR1401

PIDL402

PIR1101

PIDL404

PIR1201

PIJ400

PIT1012

PIJ401

PIT1011

PIJ402

PIT108

PIJ403

PIT107

PIJ500

PIT2012

PIJ501

PIT2011

PIJ502

PIT208

PIJ503

PIT207 PIR102

PIT1010

PIR202

PIT109

PIR602

PIT2010

PIR702

PIT209

PIC302

PIC402

PIC702

PIR501

54 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

P

ower

Supp

lyS

chD

ocD

raw

n B

y

5V GN

D

GN

D

3V3

GN

DS

GN

DS

3V3 R

pu10

k 16V

10nF

C21

GN

DS

Rs

470

TAC

TD60

B1

GN

DS

16V

10uF

Cin

R20

510k

Rpg

499k

R21

162k

16V

22uF

Cou

t

16V

4uF

Cld

o

3V3

GN

DS

GN

DS

16V

12pF

Cx2

16V

12pF

Cx1

GN

DS

3V3

1

GN

D2

GN

D6

GN

D10

GN

D14

GN

D18

3V3

27

GN

D28

GN

D32

GN

D36

GN

D40

GN

D44

3V3

51

GN

D52

Pow

er p

ins

3V3

101

GN

D10

2

GN

D10

8

GN

D11

4

GN

D11

8

GN

D12

4

GN

D12

8

GN

D13

4G

ND

140

GN

D14

4G

ND

150

3V3

155

GN

D15

6

C Any

bus_

CC

_B40

3V3

GN

DS

GN

DG

ND

S

WA

KE_

N64

HIB

_N65

XO

SC0

66

XO

SC1

67

RST

_N70

OSC

088

OSC

189

U1C

TM4C

1292

NC

PDTI

3R

VD

D7

VD

DA

8

VR

EFA

+9

GN

DA

10

VD

D16

GN

D17

VD

D26

VD

D28

VD

D39

VD

D47

GN

D48

VD

D63

VB

AT

68

VD

D69

VD

D79

GN

D80

VD

DC

87

VD

D90

VD

D10

1

VD

D11

3

GN

D11

4

VD

DC

115

VD

D12

2

U1D

TM4C

1292

NC

PDTI

3R

RST

GN

DX

18M

Hz

GN

D

EN1

PG2

VIN

3

VIN

4G

ND

5G

ND

6FB

7V

OU

T8

TPS8

2085

GN

D9

U2

TPS8

2085

GN

D

R66

500k

2kV

1nF

C55 PE

GN

DR

5550

0k2k

V1n

F

C44 PE

GN

D

R77

500k

2kV

1nF

C66 PE

GN

D

16V

100n

F

CV

116

V10

0nF

CV

216

V10

0nF

CV

316

V10

0nF

CV

4

3V3

16V

100n

F

CV

516

V10

0nF

CV

616

V10

0nF

CV

716

V10

0nF

CV

816

V10

0nF

CV

916

V10

0nF

CV

1016

V10

0nF

CV

1116

V10

0nF

CV

1216

V10

0nF

CV

13

GN

D

16V

1uF

CV

ref1

16V

10nF

CV

ref2

GN

DG

ND Po

wer

supp

ly

1

4

4 B

arto

sz K

aczo

r

PI01

PI02

PI06

PI010

PI014

PI018

PI027

PI028

PI032

PI036

PI040

PI044

PI051

PI052

PI0101

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0155

PI0156

COC

PIB101

PIB102

PIB103 PI

B104 COB1

PIC2101 PIC2102

COC2

1

PIC4401 PIC4402

COC44

PIC5501 PIC5502

COC55

PIC6601 PIC6602

COC66

PICin01 PICin02

COCin

PICldo01 PICldo02

COCl

do

PICout01 PICout02

COCo

ut

PICV101 PICV102

COCV1

PICV201 PICV202

COCV

2 PICV301 PICV302

COCV

3 PICV401 PICV402

COCV

4 PICV501 PICV502

COCV5

PICV601 PICV602

COCV

6 PICV701 PICV702

COCV

7 PICV801 PICV802

COCV

8 PICV901 PICV902

COCV

9 PICV1001 PICV1002

COCV10

PICV1101 PICV1102

COCV11

PICV1201 PICV1202

COCV12

PICV1301 PICV1302

COCV13

PICVref101 PICVref102

COCV

ref1

PICVref201 PICVref202

COCVref2

PICx101 PICx102

COCx1

PICx201 PICx202

COCx2

PIR2001 PIR2002 COR20

PIR2

101

PIR2

102 COR

21

PIR5501 PIR5502 COR55

PIR6601 PIR6602 COR

66

PIR7701 PIR7702 CO

R77

PIRpg01 PIRpg02 CO

Rpg

PIRpu01 PIRpu02 CORpu

PIRs01

PIRs

02

CORs

PIU1064

PIU1065

PIU1066

PIU1067

PIU1070

PIU1088

PIU1089

COU1

C

PIU107

PIU108

PIU109

PIU1010

PIU1016

PIU1017

PIU1026

PIU1028

PIU1039

PIU1047

PIU1048

PIU1063

PIU1068

PIU1069

PIU1079

PIU1080

PIU1087

PIU1090

PIU10101

PIU10113

PIU10114

PIU10115

PIU10122

COU1

D

PIU201

PIU202

PIU203

PIU204

PIU205

PIU206

PIU207

PIU208

PIU209

COU2

PIX101 PIX102 PIX10GND

PIX10GND2 COX

1

PI01

PI027

PI051

PI0101

PI0155

PICout01

PICV101 PICV201

PICV301 PICV401

PICV501 PICV601

PICV701 PICV801

PICV901 PICV1001

PICV1101 PICV1201

PICV1301

PICVref101 PICVref201

PIR2002 PIRpg02

PIRpu02

PIU107

PIU108

PIU109

PIU1016

PIU1026

PIU1028

PIU1039

PIU1047

PIU1063

PIU1068

PIU1069

PIU1079

PIU1090

PIU10101

PIU10113

PIU10122

PIU208

PICin01

PIU201

PIU203

PIU204

PI02

PI06

PI010

PI014

PI018

PI028

PI032

PI036

PI040

PI044

PI052

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0156

PIB101

PIB102

PIC2102

PIC4401 PIC5501

PIC6601

PICin02

PICldo02

PICout02

PICV102 PICV202

PICV302 PICV402

PICV502 PICV602

PICV702 PICV802

PICV902 PICV1002

PICV1102 PICV1202

PICV1302

PICVref102 PICVref202

PICx102 PICx202

PIR2

101

PIR5502 PIR6602

PIR7702

PIU1010

PIU1017

PIU1048

PIU1064

PIU1080

PIU1088

PIU10114

PIU205

PIU206

PIU209

PIX10GND

PIX10GND2

PIB103 PI

B104 PIRs01

PICldo01

PIU1087

PIU10115

PICx101

PIU1067

PIX101

PICx201

PIU1066

PIX102

PIR2001 PIR2

102

PIU207

PIRpg01

PIU202

PIU1065

PIU1089

PIC4402 PIC5502

PIC6602

PIR5501 PIR6601

PIR7701

PIC2101 PIRpu01 PI

Rs02

PIU1070

NLRST

Dodatek A Schematy modułoacutew urządzenia 55

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-15

Shee

t o

fFi

le

CU

sers

IM

UO

ldS

chD

ocD

raw

n B

y

8642 16141210 18 20 22 24

17 19 21 239 11 13 157531J5 H

eade

r 12x

2

5V

GN

D

5V

16V

100n

F

C1

16V

100n

F

C2

GN

DRS

TCSD

OU

TD

IO4

DIO

3SC

LKD

IND

IO1

DIO

2

PA2

PA3

PA4

PA5

PB1

PB2

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

1 23 4

J2

5V

GN

D

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIJ201

PIJ202

PIJ203

PIJ204

COJ2

PIJ501

PIJ502

PIJ503

PIJ504

PIJ505

PIJ506

PIJ507

PIJ508

PIJ509

PIJ5

010

PIJ5011

PIJ5012

PIJ5013

PIJ5014

PIJ5

015

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

COJ5

PI1041

PI1080

PI1059

PI1060

PI1061

PI1062

PIC101 PIC201

PIJ201

PIJ5

010

PIJ5011

PIJ5012

PI1066

PIJ506

NLCS

NLPA3

PI1067

PIJ505

NLDIN

NLPA4

PIJ507

NLDI

O1 PIJ509

NLDI

O2 PIJ501

NLDI

O3

PI1045

PIJ502

NLDI

O4

NLPB2

PI1058

PIJ504

NLDOUT

NLPA5

PI1042

PI1043

PI1078

PI1079

PIC102 PIC202

PIJ204

PIJ5013

PIJ5014

PIJ5

015

PIJ202

PIJ203

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

PI1063

PIJ503

NLPA2

NLSCLK

PI102

PIJ508

NLPB1

NLRS

T

PI1029

NLPB4

PI1030

NLPB5

PI103

NLPC0

PI1038

NLPC1

PI1039

NLPC2

PI1040

NLPC3

PI1019

NLPD0

PI1022

NLPD1

PI1021

NLPD2

PI1020

NLPD3

PI1026

NLPD4

PI1025

NLPD5

PI1024

NLPD6

PI1023

NLPD7

PI1014

NLPE0

PI1015

NLPE

1

PI1016

NLPE2

PI1017

NLPE3

PI1028

NLPE4

PI1027

NLPE

5

PI1057

NLPF

0

PI1068

NLPF

1

PI1056

NLPF2

PI1069

NLPF3

PI1055

NLPF

4

PI1054

NLPG2

PI1070

NLPG3

PI1071

NLPG4

PI1053

NLPG5

PI1052

NLPG6

PI1072

NLPG7

PI1010

NLPH0

PI109

NLPH1

PI1065

NLPH2

PI1064

NLPH3

PI1032

NLPJ0

PI1011

NLPJ1

PI1076

NLPK4

PI1049

NLPK5

PI1075

NLPK6

PI1050

NLPK7

PI1077

NLPL5

PI101

NLPL6

PI1044

NLPL7

PI1046

NLPM

4

PI1047

NLPM

5

PI1048

NLPM

6

PI1074

NLPM

7

PI1035

NLPN0

PI1034

NLPN1

PI106

NLPN2

PI107

NLPN3

PI1033

NLPN4

PI108

NLPN5

PI1031

NLPP0

PI1012

NLPP1

PI104

NLPP3

PI1036

NLPP4

PI105

NLPP5

PI1018

NLPQ2

PI1013

NLPQ3

PI1037

NLPQ4

PI1051

NLPQ5

PI1073

NLPQ6

56 Dodatek A Schematy modułoacutew urządzenia

Dodatek B

Wykresy pobranych trajektorii

Dodatek B Wykresy pobranych trajektorii 57

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B1 Trajektoria ucząca z minimalnym zastosowaniem filtru

58 Dodatek B Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

Spis tablic

31 Opis podłączeń do jednostki inercyjnej 1632 Opis podłączeń w złączu międzypłytkowym 1833 Opis połączeń złącza zasilania 1934 Opis połączeń złącza EtherCAT 2035 Opis połączeń złącza EtherCAT 21

41 Opis Obiektoacutew PDO dostępnych w urządzeniu 2642 Opis Obiektoacutew SDO dostępnych w urządzeniu 2743 Opis Obiektoacutew PDO dostępnych w urządzeniu 28

51 Statystyka dotycząca długości wektoroacutew siły 42

Spis tablic 47

48 Spis tablic

Spis Dodatkoacutew

Dodatek A 51

Dodatek B 57

Spis tablic 49

50 Spis tablic

Dodatek A

Schematy modułoacutew urządzenia

Dodatek A Schematy modułoacutew urządzenia 51

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

A

nybu

s-B

ase

SchD

ocD

raw

n B

y

~RES

ET10

3

A0

~WEH

DIP

1_0

104

A1

DIP

1_1

105

A2

DIP

1_2

106

A3

DIP

1_3

107

A4

DIP

1_4

109

A5

DIP

1_5

110

A6

DIP

1_6

111

A7

DIP

1_7

112

A8

~LD

~SS

113

A9

SCLK

115

A10

DO

MIS

O11

6

A11

DIM

OSI

117

A12

ASM

_RX

119

A13

ASM

_TX

120

D7

DIP

2_7

121

D6

DIP

2_6

122

D5

DIP

2_5

123

D4

DIP

2_4

125

D3

DIP

2_3

126

D2

DIP

2_2

127

D1

DIP

2_1

129

MD

013

0D

0D

IP2_

013

1O

M0

132

OM

113

3

OM

213

5~C

S13

6~W

E~W

ELC

T13

7~I

RQ

PA

138

~OE

139

~LED

4AD

1514

1~L

ED4B

D14

142

~LED

3AD

1314

3

~LED

3BD

1214

5LE

D2A

D11

146

LED

2BD

1014

7LE

D1A

D9

148

LED

1BD

814

9

RX

ASM

_RX

151

TXA

SM_T

XO

M3

152

MI1

153

MI0

SY

NC

154

Hos

t Con

nect

or

B Any

bus_

CC

_B40

PK0

18

PK1

19

PK2

20

PK3

21

PC7

22

PC6

23

PC5

24

PC4

25

Dat

aBus

Pin

sU

1E

TM4C

1292

NC

PDTI

3R

PA6

40

PA7

41

PG0

49PG

150

PB3

92

PQ0

5

PQ1

6

PM3

75

PM2

76

PM1

77

PM0

78

PL0

81

PL1

82

PL2

83

PL3

84

PL4

85

PP2

103

Adr

esB

us P

ins

U1F

TM4C

1292

NC

PDTI

3R

D0

D1

D2

D3

D4

D5

D6

D7

D0

D1

D2

D3

D4

D5

D6

D7A

0A

1A

2A

3A

4A

5A

6A

7A

8A

9A

10A

11A

12A

13

A0

A1

A2

A3

A4

A5

A6

A7

A8

A9

A10

A11

A12

A13

PA0

33

PA1

34

PB0

95

U1A

TM4C

1292

NC

PDTI

3R

GN

D

R33

10k

R34

10k

R35

10k

Any

bus

Page

15

Any

bus

uC c

onne

ctio

n

1

Bar

osz

Kac

zor

1

4

PI0103

PI0104

PI0105

PI0106

PI0107

PI0109

PI0110

PI0111

PI0112

PI0113

PI0115

PI0116

PI0117

PI0119

PI0120

PI0121

PI0122

PI0123

PI0125

PI0126

PI0127

PI0129

PI0130

PI0131

PI0132

PI0133

PI0135

PI0136

PI0137

PI0138

PI0139

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

COB

PIR3301 PIR3302 CO

R33

PIR3401 PIR3402 COR34

PIR3501 PIR3502 CO

R35

PIU1033

PIU1034

PIU1095 COU1

A

PIU1018

PIU1019

PIU1020

PIU1021

PIU1022

PIU1023

PIU1024

PIU1025

COU1

E PIU105

PIU106

PIU1040

PIU1041

PIU1049

PIU1050

PIU1075

PIU1076

PIU1077

PIU1078

PIU1081

PIU1082

PIU1083

PIU1084

PIU1085

PIU1092

PIU10103

COU1

F

PI0104

PIU1040

NLA0

PI0105

PIU1041

NLA1

PI0106

PIU1050

NLA2

PI0107

PIU1049

NLA3

PI0109

PIU1075

NLA4

PI0110

PIU1076

NLA5

PI0111

PIU1077

NLA6

PI0112

PIU1078

NLA7

PI0113

PIU1081

NLA8

PI0115

PIU1082

NLA9

PI0116

PIU1083

NLA10

PI0117

PIU1084

NLA11

PI0119

PIU105

NLA12

PI0120

PIU106

NLA13

PI0131

PIU1018

NLD0

PI0129

PIU1019

NLD1

PI0127

PIU1020

NLD2

PI0126

PIU1021

NLD3 PI0125

PIU1022

NLD4

PI0123

PIU1023

NLD5 PI0122

PIU1024

NLD6

PI0121

PIU1025

NLD7

PIR3301 PIR3401

PIR3501

PI0103

PIU1033

PI0130

PI0132

PIR3302

PI0133

PIR3402

PI0135

PIR3502

PI0136

PIU1085

PI0137

PIU10103

PI0138

PIU1095

PI0139

PIU1092

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

PIU1034

52 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

C

onne

ctor

sSch

Doc

Dra

wn

By

PQ2

11

PQ3

27

PQ5

57

PQ6

58

PM7

71PM

672

PM5

73PM

474

PL5

86

PL7

93PL

694

PQ4

102

PP3

104

PP4

105

PP5

106

PN0

107

PN1

108

PN2

109

PN3

110

PN4

111

PN5

112

PP0

118

PP1

119

U1B

TM4C

1292

NC

PDTI

3R

PD0

1

PD1

2

PD2

3

PD3

4

PE3

12PE

213

PE1

14PE

015

PH0

29

PH1

30

PH2

31

PH3

32

PA2

35

PA3

36

PA4

37

PA5

38

PF0

42

PF1

43

PF2

44

PF3

45

PF4

46

PG2

51

PG3

52

PG4

53

PG5

54

PG6

55

PG7

56

PK7

59PK

660

PK5

61PK

462

PB2

91PB

196

PC3_

TDO

_SW

O97

PC2_

TDI

98PC

1_TM

S_SW

DIO

99PC

0_TC

K_S

WC

LK10

0

PJ0

116

PJ1

117

PB5

120

PB4

121

PE4

123

PE5

124

PD4

125

PD5

126

PD6

127

PD7

128

U1G

TM4C

1292

NC

PDTI

3R

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PH0

PH1

PH2

PH3

PJ0

PJ1

PK4

PK5

PK6

PK7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

PC0

PC1

PC2

PC3

3V3

GN

D

RST

92

101

74

83 5

6

J2 Pin

200

mm

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

Con

nect

or s

chem

a

1

Bar

tosz

Kac

zor

2

4

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIJ201

PIJ202

PIJ203

PIJ204

PIJ205

PIJ206

PIJ207

PIJ208

PIJ209

PIJ2010

COJ2

PIU1011

PIU1027

PIU1057

PIU1058

PIU1071

PIU1072

PIU1073

PIU1074

PIU1086

PIU1093

PIU1094

PIU10102

PIU10104

PIU10105

PIU10106

PIU10107

PIU10108

PIU10109

PIU10110

PIU10111

PIU10112

PIU10118

PIU10119

COU1B

PIU101

PIU102

PIU103

PIU104

PIU1012

PIU1013

PIU1014

PIU1015

PIU1029

PIU1030

PIU1031

PIU1032

PIU1035

PIU1036

PIU1037

PIU1038

PIU1042

PIU1043

PIU1044

PIU1045

PIU1046

PIU1051

PIU1052

PIU1053

PIU1054

PIU1055

PIU1056

PIU1059

PIU1060

PIU1061

PIU1062

PIU1091

PIU1096

PIU1097

PIU1098

PIU1099

PIU10100

PIU10116

PIU10117

PIU10120

PIU10121

PIU10123

PIU10124

PIU10125

PIU10126

PIU10127

PIU10128

COU1G

PI1041

PI1080

PIJ201

PI1059

PI1060

PI1061

PI1062

PI1042

PI1043

PI1078

PI1079

PIJ202

PIJ203

PIJ205

PIJ204

PI1063

PIU1035

NLPA2

PI1066

PIU1036

NLPA3

PI1067

PIU1037

NLPA4

PI1058

PIU1038

NLPA5

PI102

PIU1096

NLPB1

PI1045

PIU1091

NLPB2

PI1029

PIU10121

NLPB4

PI1030

PIU10120

NLPB5

PI103

PIJ209

PIU10100

NLPC0

PI1038

PIJ2010

PIU1099

NLPC

1

PI1039

PIJ207

PIU1098

NLPC2

PI1040

PIJ208

PIU1097

NLPC3

PI1019

PIU101

NLPD

0

PI1022

PIU102

NLPD1

PI1021

PIU103

NLPD

2

PI1020

PIU104

NLPD3

PI1026

PIU10125

NLPD

4

PI1025

PIU10126

NLPD5

PI1024

PIU10127

NLPD

6

PI1023

PIU10128

NLPD

7

PI1014

PIU1015

NLPE0

PI1015

PIU1014

NLPE1

PI1016

PIU1013

NLPE2

PI1017

PIU1012

NLPE3

PI1028

PIU10123

NLPE4

PI1027

PIU10124

NLPE5

PI1057

PIU1042

NLPF0

PI1068

PIU1043

NLPF1

PI1056

PIU1044

NLPF2

PI1069

PIU1045

NLPF3

PI1055

PIU1046

NLPF4

PI1054

PIU1051

NLPG2

PI1070

PIU1052

NLPG3

PI1071

PIU1053

NLPG4

PI1053

PIU1054

NLPG5

PI1052

PIU1055

NLPG6

PI1072

PIU1056

NLPG7

PI1010

PIU1029

NLPH0

PI109

PIU1030

NLPH1

PI1065

PIU1031

NLPH2

PI1064

PIU1032

NLPH3

PI1032

PIU10116

NLPJ0

PI1011

PIU10117

NLPJ1

PI1076

PIU1062

NLPK4

PI1049

PIU1061

NLPK5

PI1075

PIU1060

NLPK6

PI1050

PIU1059

NLPK7

PI1077

PIU1086

NLPL5

PI101

PIU1094

NLPL6

PI1044

PIU1093

NLPL7

PI1046

PIU1074

NLPM

4

PI1047

PIU1073

NLPM

5

PI1048

PIU1072

NLPM

6

PI1074

PIU1071

NLPM

7

PI1035

PIU10107

NLPN0

PI1034

PIU10108

NLPN1

PI106

PIU10109

NLPN2

PI107

PIU10110

NLPN3

PI1033

PIU10111

NLPN4

PI108

PIU10112

NLPN5

PI1031

PIU10118

NLPP0

PI1012

PIU10119

NLPP1

PI104

PIU10104

NLPP3

PI1036

PIU10105

NLPP4

PI105

PIU10106

NLPP5

PI1018

PIU1011

NLPQ

2

PI1013

PIU1027

NLPQ3

PI1037

PIU10102

NLPQ

4

PI1051

PIU1057

NLPQ5

PI1073

PIU1058

NLPQ

6

PIJ206

NLRST

Dodatek A Schematy modułoacutew urządzenia 53

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

N

etw

ork

SchD

ocD

raw

n B

y

B_1

PR

XP

3

B_1

CEN

SD

A4

B_1

NR

XN

5

B_2

PSD

P7

B_2

CEN

SC

L8

B_2

NS

DN

9

B_3

PTX

EN11

B_3

CEN

12

B_3

NT

XD

IS13

B_4

PTX

P15

B_4

CEN

~B

USP

16

B_4

NT

XN

17

NW

_LED

4B19

NW

_LED

4A20

NW

_LED

3B21

NW

_LED

3A22

NW

_LED

2B23

NW

_LED

2A24

NW

_LED

1B25

NW

_LED

1A26

A_1

PR

XP

29A

_1C

ENS

DA

30A

_1N

RX

N31

A_2

PSD

P33

A_2

CEN

SC

L34

A_2

NS

DN

35

A_3

PTX

EN37

A_3

CEN

38A

_3N

TX

DIS

39

A_4

PTX

P41

A_4

CEN

~B

USP

42A

_4N

TX

N43

C_T

X45

C_R

X46

C_T

XEN

47~C

_BU

SP48

GA

TE1

49G

ATE

250

Net

wor

k C

onne

ctor

A Any

bus_

CC

_B40

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T1 47

uHB_

1PB_

1CEN

B_1N

B_1P

B_1C

EN

B_1N

B_2P

B_2C

ENB_

2N

B_2P

B_2C

EN

B_2N

16V

100n

F

C1

16V

100n

F

C2

GN

DS

PE

2kV

1nF

C3

R1

75R

275

R5

1M2k

V1n

F

C4 PE

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T2 47

uH

A_1

P

A_1

CEN

A_1

N

A_2

P

A_2

CEN

A_2

N

16V

100n

F

C5

16V

100n

F

C6

GN

DS

PE

2kV

1nF

C7

R6

75R

775

A_1

PA

_1C

ENA

_1N

A_2

PA

_2C

ENA

_2N

DL1

R18

220

R17

220

R16

220

R15

220

R14

220

R13

220

R12

220

R11

220

DL2

DL3

DL4

GN

DS

3210 4

J4 Clic

k_M

ate

5x1

3210 4

J5 Clic

k_M

ate

5x1

Net

wor

k co

nnec

tor

1

3

4B

arto

sz k

aczo

r

PI03

PI04

PI05

PI07

PI08

PI09

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PI020

PI021

PI022

PI023

PI024

PI025

PI026

PI029

PI030

PI031

PI033

PI034

PI035

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

COA

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIC301 PIC302

COC3

PIC401 PIC402

COC4

PIC501 PIC502

COC5

PIC601 PIC602

COC6

PIC701 PIC702

COC7

PIDL101 PIDL102

PIDL103 PIDL104 CODL1

PIDL201 PIDL202

PIDL203 PIDL204 CODL2

PIDL301 PIDL302

PIDL303 PIDL304 CODL3

PIDL401 PIDL402

PIDL403 PIDL404 CODL4

PIJ400

PIJ401

PIJ402

PIJ403

PIJ404

COJ4 PIJ500

PIJ501

PIJ502

PIJ503

PIJ504

COJ5

PIR101 PIR102 COR

1 PIR201 PIR202 COR

2

PIR501 PIR502 COR

5

PIR601 PIR602 COR

6 PIR701 PIR702 COR

7

PIR1101 PIR1102 COR1

1

PIR1201 PIR1202 COR12

PIR1301 PIR1302 COR13

PIR1401 PIR1402 COR14

PIR1501 PIR1502 COR15

PIR1601 PIR1602 COR16

PIR1701 PIR1702 COR17 PIR1801 PIR1802

COR1

8

PIT101

PIT102

PIT103

PIT104

PIT105

PIT106

PIT107

PIT108

PIT109

PIT1010

PIT1011

PIT1012

COT1

PIT201

PIT202

PIT203

PIT204

PIT205

PIT206

PIT207

PIT208

PIT209

PIT2010

PIT2011

PIT2012

COT2

PI030

PIC501

PIT203

NLA01CEN

PI031

PIT202

NLA0

1N

PI029

PIT201

NLA0

1P

PI034

PIC601

PIT204

NLA02CEN

PI035

PIT206

NLA0

2N

PI033

PIT205

NLA0

2P

PI04

PIC101

PIT103

NLB01CEN

PI05

PIT102

NLB0

1N

PI03

PIT101

NLB0

1P

PI08

PIC201

PIT104

NLB02CEN

PI09

PIT106

NLB0

2N

PI07

PIT105

NLB0

2P

PIC102 PIC202

PIC502 PIC602

PIDL101 PIDL103

PIDL201 PIDL203

PIDL301 PIDL303

PIDL401 PIDL403

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PIR1102

PI020

PIR1202

PI021

PIR1302

PI022

PIR1402

PI023

PIR1502

PI024

PIR1602 PI025

PIR1702 PI026

PIR1802

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

PIC301 PIR101

PIR201

PIC401

PIJ404 PIJ504

PIR502

PIC701 PIR601

PIR701

PIDL102

PIR1701

PIDL104

PIR1801

PIDL202

PIR1501

PIDL204

PIR1601

PIDL302

PIR1301

PIDL304

PIR1401

PIDL402

PIR1101

PIDL404

PIR1201

PIJ400

PIT1012

PIJ401

PIT1011

PIJ402

PIT108

PIJ403

PIT107

PIJ500

PIT2012

PIJ501

PIT2011

PIJ502

PIT208

PIJ503

PIT207 PIR102

PIT1010

PIR202

PIT109

PIR602

PIT2010

PIR702

PIT209

PIC302

PIC402

PIC702

PIR501

54 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

P

ower

Supp

lyS

chD

ocD

raw

n B

y

5V GN

D

GN

D

3V3

GN

DS

GN

DS

3V3 R

pu10

k 16V

10nF

C21

GN

DS

Rs

470

TAC

TD60

B1

GN

DS

16V

10uF

Cin

R20

510k

Rpg

499k

R21

162k

16V

22uF

Cou

t

16V

4uF

Cld

o

3V3

GN

DS

GN

DS

16V

12pF

Cx2

16V

12pF

Cx1

GN

DS

3V3

1

GN

D2

GN

D6

GN

D10

GN

D14

GN

D18

3V3

27

GN

D28

GN

D32

GN

D36

GN

D40

GN

D44

3V3

51

GN

D52

Pow

er p

ins

3V3

101

GN

D10

2

GN

D10

8

GN

D11

4

GN

D11

8

GN

D12

4

GN

D12

8

GN

D13

4G

ND

140

GN

D14

4G

ND

150

3V3

155

GN

D15

6

C Any

bus_

CC

_B40

3V3

GN

DS

GN

DG

ND

S

WA

KE_

N64

HIB

_N65

XO

SC0

66

XO

SC1

67

RST

_N70

OSC

088

OSC

189

U1C

TM4C

1292

NC

PDTI

3R

VD

D7

VD

DA

8

VR

EFA

+9

GN

DA

10

VD

D16

GN

D17

VD

D26

VD

D28

VD

D39

VD

D47

GN

D48

VD

D63

VB

AT

68

VD

D69

VD

D79

GN

D80

VD

DC

87

VD

D90

VD

D10

1

VD

D11

3

GN

D11

4

VD

DC

115

VD

D12

2

U1D

TM4C

1292

NC

PDTI

3R

RST

GN

DX

18M

Hz

GN

D

EN1

PG2

VIN

3

VIN

4G

ND

5G

ND

6FB

7V

OU

T8

TPS8

2085

GN

D9

U2

TPS8

2085

GN

D

R66

500k

2kV

1nF

C55 PE

GN

DR

5550

0k2k

V1n

F

C44 PE

GN

D

R77

500k

2kV

1nF

C66 PE

GN

D

16V

100n

F

CV

116

V10

0nF

CV

216

V10

0nF

CV

316

V10

0nF

CV

4

3V3

16V

100n

F

CV

516

V10

0nF

CV

616

V10

0nF

CV

716

V10

0nF

CV

816

V10

0nF

CV

916

V10

0nF

CV

1016

V10

0nF

CV

1116

V10

0nF

CV

1216

V10

0nF

CV

13

GN

D

16V

1uF

CV

ref1

16V

10nF

CV

ref2

GN

DG

ND Po

wer

supp

ly

1

4

4 B

arto

sz K

aczo

r

PI01

PI02

PI06

PI010

PI014

PI018

PI027

PI028

PI032

PI036

PI040

PI044

PI051

PI052

PI0101

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0155

PI0156

COC

PIB101

PIB102

PIB103 PI

B104 COB1

PIC2101 PIC2102

COC2

1

PIC4401 PIC4402

COC44

PIC5501 PIC5502

COC55

PIC6601 PIC6602

COC66

PICin01 PICin02

COCin

PICldo01 PICldo02

COCl

do

PICout01 PICout02

COCo

ut

PICV101 PICV102

COCV1

PICV201 PICV202

COCV

2 PICV301 PICV302

COCV

3 PICV401 PICV402

COCV

4 PICV501 PICV502

COCV5

PICV601 PICV602

COCV

6 PICV701 PICV702

COCV

7 PICV801 PICV802

COCV

8 PICV901 PICV902

COCV

9 PICV1001 PICV1002

COCV10

PICV1101 PICV1102

COCV11

PICV1201 PICV1202

COCV12

PICV1301 PICV1302

COCV13

PICVref101 PICVref102

COCV

ref1

PICVref201 PICVref202

COCVref2

PICx101 PICx102

COCx1

PICx201 PICx202

COCx2

PIR2001 PIR2002 COR20

PIR2

101

PIR2

102 COR

21

PIR5501 PIR5502 COR55

PIR6601 PIR6602 COR

66

PIR7701 PIR7702 CO

R77

PIRpg01 PIRpg02 CO

Rpg

PIRpu01 PIRpu02 CORpu

PIRs01

PIRs

02

CORs

PIU1064

PIU1065

PIU1066

PIU1067

PIU1070

PIU1088

PIU1089

COU1

C

PIU107

PIU108

PIU109

PIU1010

PIU1016

PIU1017

PIU1026

PIU1028

PIU1039

PIU1047

PIU1048

PIU1063

PIU1068

PIU1069

PIU1079

PIU1080

PIU1087

PIU1090

PIU10101

PIU10113

PIU10114

PIU10115

PIU10122

COU1

D

PIU201

PIU202

PIU203

PIU204

PIU205

PIU206

PIU207

PIU208

PIU209

COU2

PIX101 PIX102 PIX10GND

PIX10GND2 COX

1

PI01

PI027

PI051

PI0101

PI0155

PICout01

PICV101 PICV201

PICV301 PICV401

PICV501 PICV601

PICV701 PICV801

PICV901 PICV1001

PICV1101 PICV1201

PICV1301

PICVref101 PICVref201

PIR2002 PIRpg02

PIRpu02

PIU107

PIU108

PIU109

PIU1016

PIU1026

PIU1028

PIU1039

PIU1047

PIU1063

PIU1068

PIU1069

PIU1079

PIU1090

PIU10101

PIU10113

PIU10122

PIU208

PICin01

PIU201

PIU203

PIU204

PI02

PI06

PI010

PI014

PI018

PI028

PI032

PI036

PI040

PI044

PI052

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0156

PIB101

PIB102

PIC2102

PIC4401 PIC5501

PIC6601

PICin02

PICldo02

PICout02

PICV102 PICV202

PICV302 PICV402

PICV502 PICV602

PICV702 PICV802

PICV902 PICV1002

PICV1102 PICV1202

PICV1302

PICVref102 PICVref202

PICx102 PICx202

PIR2

101

PIR5502 PIR6602

PIR7702

PIU1010

PIU1017

PIU1048

PIU1064

PIU1080

PIU1088

PIU10114

PIU205

PIU206

PIU209

PIX10GND

PIX10GND2

PIB103 PI

B104 PIRs01

PICldo01

PIU1087

PIU10115

PICx101

PIU1067

PIX101

PICx201

PIU1066

PIX102

PIR2001 PIR2

102

PIU207

PIRpg01

PIU202

PIU1065

PIU1089

PIC4402 PIC5502

PIC6602

PIR5501 PIR6601

PIR7701

PIC2101 PIRpu01 PI

Rs02

PIU1070

NLRST

Dodatek A Schematy modułoacutew urządzenia 55

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-15

Shee

t o

fFi

le

CU

sers

IM

UO

ldS

chD

ocD

raw

n B

y

8642 16141210 18 20 22 24

17 19 21 239 11 13 157531J5 H

eade

r 12x

2

5V

GN

D

5V

16V

100n

F

C1

16V

100n

F

C2

GN

DRS

TCSD

OU

TD

IO4

DIO

3SC

LKD

IND

IO1

DIO

2

PA2

PA3

PA4

PA5

PB1

PB2

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

1 23 4

J2

5V

GN

D

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIJ201

PIJ202

PIJ203

PIJ204

COJ2

PIJ501

PIJ502

PIJ503

PIJ504

PIJ505

PIJ506

PIJ507

PIJ508

PIJ509

PIJ5

010

PIJ5011

PIJ5012

PIJ5013

PIJ5014

PIJ5

015

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

COJ5

PI1041

PI1080

PI1059

PI1060

PI1061

PI1062

PIC101 PIC201

PIJ201

PIJ5

010

PIJ5011

PIJ5012

PI1066

PIJ506

NLCS

NLPA3

PI1067

PIJ505

NLDIN

NLPA4

PIJ507

NLDI

O1 PIJ509

NLDI

O2 PIJ501

NLDI

O3

PI1045

PIJ502

NLDI

O4

NLPB2

PI1058

PIJ504

NLDOUT

NLPA5

PI1042

PI1043

PI1078

PI1079

PIC102 PIC202

PIJ204

PIJ5013

PIJ5014

PIJ5

015

PIJ202

PIJ203

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

PI1063

PIJ503

NLPA2

NLSCLK

PI102

PIJ508

NLPB1

NLRS

T

PI1029

NLPB4

PI1030

NLPB5

PI103

NLPC0

PI1038

NLPC1

PI1039

NLPC2

PI1040

NLPC3

PI1019

NLPD0

PI1022

NLPD1

PI1021

NLPD2

PI1020

NLPD3

PI1026

NLPD4

PI1025

NLPD5

PI1024

NLPD6

PI1023

NLPD7

PI1014

NLPE0

PI1015

NLPE

1

PI1016

NLPE2

PI1017

NLPE3

PI1028

NLPE4

PI1027

NLPE

5

PI1057

NLPF

0

PI1068

NLPF

1

PI1056

NLPF2

PI1069

NLPF3

PI1055

NLPF

4

PI1054

NLPG2

PI1070

NLPG3

PI1071

NLPG4

PI1053

NLPG5

PI1052

NLPG6

PI1072

NLPG7

PI1010

NLPH0

PI109

NLPH1

PI1065

NLPH2

PI1064

NLPH3

PI1032

NLPJ0

PI1011

NLPJ1

PI1076

NLPK4

PI1049

NLPK5

PI1075

NLPK6

PI1050

NLPK7

PI1077

NLPL5

PI101

NLPL6

PI1044

NLPL7

PI1046

NLPM

4

PI1047

NLPM

5

PI1048

NLPM

6

PI1074

NLPM

7

PI1035

NLPN0

PI1034

NLPN1

PI106

NLPN2

PI107

NLPN3

PI1033

NLPN4

PI108

NLPN5

PI1031

NLPP0

PI1012

NLPP1

PI104

NLPP3

PI1036

NLPP4

PI105

NLPP5

PI1018

NLPQ2

PI1013

NLPQ3

PI1037

NLPQ4

PI1051

NLPQ5

PI1073

NLPQ6

56 Dodatek A Schematy modułoacutew urządzenia

Dodatek B

Wykresy pobranych trajektorii

Dodatek B Wykresy pobranych trajektorii 57

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B1 Trajektoria ucząca z minimalnym zastosowaniem filtru

58 Dodatek B Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

48 Spis tablic

Spis Dodatkoacutew

Dodatek A 51

Dodatek B 57

Spis tablic 49

50 Spis tablic

Dodatek A

Schematy modułoacutew urządzenia

Dodatek A Schematy modułoacutew urządzenia 51

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

A

nybu

s-B

ase

SchD

ocD

raw

n B

y

~RES

ET10

3

A0

~WEH

DIP

1_0

104

A1

DIP

1_1

105

A2

DIP

1_2

106

A3

DIP

1_3

107

A4

DIP

1_4

109

A5

DIP

1_5

110

A6

DIP

1_6

111

A7

DIP

1_7

112

A8

~LD

~SS

113

A9

SCLK

115

A10

DO

MIS

O11

6

A11

DIM

OSI

117

A12

ASM

_RX

119

A13

ASM

_TX

120

D7

DIP

2_7

121

D6

DIP

2_6

122

D5

DIP

2_5

123

D4

DIP

2_4

125

D3

DIP

2_3

126

D2

DIP

2_2

127

D1

DIP

2_1

129

MD

013

0D

0D

IP2_

013

1O

M0

132

OM

113

3

OM

213

5~C

S13

6~W

E~W

ELC

T13

7~I

RQ

PA

138

~OE

139

~LED

4AD

1514

1~L

ED4B

D14

142

~LED

3AD

1314

3

~LED

3BD

1214

5LE

D2A

D11

146

LED

2BD

1014

7LE

D1A

D9

148

LED

1BD

814

9

RX

ASM

_RX

151

TXA

SM_T

XO

M3

152

MI1

153

MI0

SY

NC

154

Hos

t Con

nect

or

B Any

bus_

CC

_B40

PK0

18

PK1

19

PK2

20

PK3

21

PC7

22

PC6

23

PC5

24

PC4

25

Dat

aBus

Pin

sU

1E

TM4C

1292

NC

PDTI

3R

PA6

40

PA7

41

PG0

49PG

150

PB3

92

PQ0

5

PQ1

6

PM3

75

PM2

76

PM1

77

PM0

78

PL0

81

PL1

82

PL2

83

PL3

84

PL4

85

PP2

103

Adr

esB

us P

ins

U1F

TM4C

1292

NC

PDTI

3R

D0

D1

D2

D3

D4

D5

D6

D7

D0

D1

D2

D3

D4

D5

D6

D7A

0A

1A

2A

3A

4A

5A

6A

7A

8A

9A

10A

11A

12A

13

A0

A1

A2

A3

A4

A5

A6

A7

A8

A9

A10

A11

A12

A13

PA0

33

PA1

34

PB0

95

U1A

TM4C

1292

NC

PDTI

3R

GN

D

R33

10k

R34

10k

R35

10k

Any

bus

Page

15

Any

bus

uC c

onne

ctio

n

1

Bar

osz

Kac

zor

1

4

PI0103

PI0104

PI0105

PI0106

PI0107

PI0109

PI0110

PI0111

PI0112

PI0113

PI0115

PI0116

PI0117

PI0119

PI0120

PI0121

PI0122

PI0123

PI0125

PI0126

PI0127

PI0129

PI0130

PI0131

PI0132

PI0133

PI0135

PI0136

PI0137

PI0138

PI0139

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

COB

PIR3301 PIR3302 CO

R33

PIR3401 PIR3402 COR34

PIR3501 PIR3502 CO

R35

PIU1033

PIU1034

PIU1095 COU1

A

PIU1018

PIU1019

PIU1020

PIU1021

PIU1022

PIU1023

PIU1024

PIU1025

COU1

E PIU105

PIU106

PIU1040

PIU1041

PIU1049

PIU1050

PIU1075

PIU1076

PIU1077

PIU1078

PIU1081

PIU1082

PIU1083

PIU1084

PIU1085

PIU1092

PIU10103

COU1

F

PI0104

PIU1040

NLA0

PI0105

PIU1041

NLA1

PI0106

PIU1050

NLA2

PI0107

PIU1049

NLA3

PI0109

PIU1075

NLA4

PI0110

PIU1076

NLA5

PI0111

PIU1077

NLA6

PI0112

PIU1078

NLA7

PI0113

PIU1081

NLA8

PI0115

PIU1082

NLA9

PI0116

PIU1083

NLA10

PI0117

PIU1084

NLA11

PI0119

PIU105

NLA12

PI0120

PIU106

NLA13

PI0131

PIU1018

NLD0

PI0129

PIU1019

NLD1

PI0127

PIU1020

NLD2

PI0126

PIU1021

NLD3 PI0125

PIU1022

NLD4

PI0123

PIU1023

NLD5 PI0122

PIU1024

NLD6

PI0121

PIU1025

NLD7

PIR3301 PIR3401

PIR3501

PI0103

PIU1033

PI0130

PI0132

PIR3302

PI0133

PIR3402

PI0135

PIR3502

PI0136

PIU1085

PI0137

PIU10103

PI0138

PIU1095

PI0139

PIU1092

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

PIU1034

52 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

C

onne

ctor

sSch

Doc

Dra

wn

By

PQ2

11

PQ3

27

PQ5

57

PQ6

58

PM7

71PM

672

PM5

73PM

474

PL5

86

PL7

93PL

694

PQ4

102

PP3

104

PP4

105

PP5

106

PN0

107

PN1

108

PN2

109

PN3

110

PN4

111

PN5

112

PP0

118

PP1

119

U1B

TM4C

1292

NC

PDTI

3R

PD0

1

PD1

2

PD2

3

PD3

4

PE3

12PE

213

PE1

14PE

015

PH0

29

PH1

30

PH2

31

PH3

32

PA2

35

PA3

36

PA4

37

PA5

38

PF0

42

PF1

43

PF2

44

PF3

45

PF4

46

PG2

51

PG3

52

PG4

53

PG5

54

PG6

55

PG7

56

PK7

59PK

660

PK5

61PK

462

PB2

91PB

196

PC3_

TDO

_SW

O97

PC2_

TDI

98PC

1_TM

S_SW

DIO

99PC

0_TC

K_S

WC

LK10

0

PJ0

116

PJ1

117

PB5

120

PB4

121

PE4

123

PE5

124

PD4

125

PD5

126

PD6

127

PD7

128

U1G

TM4C

1292

NC

PDTI

3R

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PH0

PH1

PH2

PH3

PJ0

PJ1

PK4

PK5

PK6

PK7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

PC0

PC1

PC2

PC3

3V3

GN

D

RST

92

101

74

83 5

6

J2 Pin

200

mm

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

Con

nect

or s

chem

a

1

Bar

tosz

Kac

zor

2

4

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIJ201

PIJ202

PIJ203

PIJ204

PIJ205

PIJ206

PIJ207

PIJ208

PIJ209

PIJ2010

COJ2

PIU1011

PIU1027

PIU1057

PIU1058

PIU1071

PIU1072

PIU1073

PIU1074

PIU1086

PIU1093

PIU1094

PIU10102

PIU10104

PIU10105

PIU10106

PIU10107

PIU10108

PIU10109

PIU10110

PIU10111

PIU10112

PIU10118

PIU10119

COU1B

PIU101

PIU102

PIU103

PIU104

PIU1012

PIU1013

PIU1014

PIU1015

PIU1029

PIU1030

PIU1031

PIU1032

PIU1035

PIU1036

PIU1037

PIU1038

PIU1042

PIU1043

PIU1044

PIU1045

PIU1046

PIU1051

PIU1052

PIU1053

PIU1054

PIU1055

PIU1056

PIU1059

PIU1060

PIU1061

PIU1062

PIU1091

PIU1096

PIU1097

PIU1098

PIU1099

PIU10100

PIU10116

PIU10117

PIU10120

PIU10121

PIU10123

PIU10124

PIU10125

PIU10126

PIU10127

PIU10128

COU1G

PI1041

PI1080

PIJ201

PI1059

PI1060

PI1061

PI1062

PI1042

PI1043

PI1078

PI1079

PIJ202

PIJ203

PIJ205

PIJ204

PI1063

PIU1035

NLPA2

PI1066

PIU1036

NLPA3

PI1067

PIU1037

NLPA4

PI1058

PIU1038

NLPA5

PI102

PIU1096

NLPB1

PI1045

PIU1091

NLPB2

PI1029

PIU10121

NLPB4

PI1030

PIU10120

NLPB5

PI103

PIJ209

PIU10100

NLPC0

PI1038

PIJ2010

PIU1099

NLPC

1

PI1039

PIJ207

PIU1098

NLPC2

PI1040

PIJ208

PIU1097

NLPC3

PI1019

PIU101

NLPD

0

PI1022

PIU102

NLPD1

PI1021

PIU103

NLPD

2

PI1020

PIU104

NLPD3

PI1026

PIU10125

NLPD

4

PI1025

PIU10126

NLPD5

PI1024

PIU10127

NLPD

6

PI1023

PIU10128

NLPD

7

PI1014

PIU1015

NLPE0

PI1015

PIU1014

NLPE1

PI1016

PIU1013

NLPE2

PI1017

PIU1012

NLPE3

PI1028

PIU10123

NLPE4

PI1027

PIU10124

NLPE5

PI1057

PIU1042

NLPF0

PI1068

PIU1043

NLPF1

PI1056

PIU1044

NLPF2

PI1069

PIU1045

NLPF3

PI1055

PIU1046

NLPF4

PI1054

PIU1051

NLPG2

PI1070

PIU1052

NLPG3

PI1071

PIU1053

NLPG4

PI1053

PIU1054

NLPG5

PI1052

PIU1055

NLPG6

PI1072

PIU1056

NLPG7

PI1010

PIU1029

NLPH0

PI109

PIU1030

NLPH1

PI1065

PIU1031

NLPH2

PI1064

PIU1032

NLPH3

PI1032

PIU10116

NLPJ0

PI1011

PIU10117

NLPJ1

PI1076

PIU1062

NLPK4

PI1049

PIU1061

NLPK5

PI1075

PIU1060

NLPK6

PI1050

PIU1059

NLPK7

PI1077

PIU1086

NLPL5

PI101

PIU1094

NLPL6

PI1044

PIU1093

NLPL7

PI1046

PIU1074

NLPM

4

PI1047

PIU1073

NLPM

5

PI1048

PIU1072

NLPM

6

PI1074

PIU1071

NLPM

7

PI1035

PIU10107

NLPN0

PI1034

PIU10108

NLPN1

PI106

PIU10109

NLPN2

PI107

PIU10110

NLPN3

PI1033

PIU10111

NLPN4

PI108

PIU10112

NLPN5

PI1031

PIU10118

NLPP0

PI1012

PIU10119

NLPP1

PI104

PIU10104

NLPP3

PI1036

PIU10105

NLPP4

PI105

PIU10106

NLPP5

PI1018

PIU1011

NLPQ

2

PI1013

PIU1027

NLPQ3

PI1037

PIU10102

NLPQ

4

PI1051

PIU1057

NLPQ5

PI1073

PIU1058

NLPQ

6

PIJ206

NLRST

Dodatek A Schematy modułoacutew urządzenia 53

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

N

etw

ork

SchD

ocD

raw

n B

y

B_1

PR

XP

3

B_1

CEN

SD

A4

B_1

NR

XN

5

B_2

PSD

P7

B_2

CEN

SC

L8

B_2

NS

DN

9

B_3

PTX

EN11

B_3

CEN

12

B_3

NT

XD

IS13

B_4

PTX

P15

B_4

CEN

~B

USP

16

B_4

NT

XN

17

NW

_LED

4B19

NW

_LED

4A20

NW

_LED

3B21

NW

_LED

3A22

NW

_LED

2B23

NW

_LED

2A24

NW

_LED

1B25

NW

_LED

1A26

A_1

PR

XP

29A

_1C

ENS

DA

30A

_1N

RX

N31

A_2

PSD

P33

A_2

CEN

SC

L34

A_2

NS

DN

35

A_3

PTX

EN37

A_3

CEN

38A

_3N

TX

DIS

39

A_4

PTX

P41

A_4

CEN

~B

USP

42A

_4N

TX

N43

C_T

X45

C_R

X46

C_T

XEN

47~C

_BU

SP48

GA

TE1

49G

ATE

250

Net

wor

k C

onne

ctor

A Any

bus_

CC

_B40

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T1 47

uHB_

1PB_

1CEN

B_1N

B_1P

B_1C

EN

B_1N

B_2P

B_2C

ENB_

2N

B_2P

B_2C

EN

B_2N

16V

100n

F

C1

16V

100n

F

C2

GN

DS

PE

2kV

1nF

C3

R1

75R

275

R5

1M2k

V1n

F

C4 PE

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T2 47

uH

A_1

P

A_1

CEN

A_1

N

A_2

P

A_2

CEN

A_2

N

16V

100n

F

C5

16V

100n

F

C6

GN

DS

PE

2kV

1nF

C7

R6

75R

775

A_1

PA

_1C

ENA

_1N

A_2

PA

_2C

ENA

_2N

DL1

R18

220

R17

220

R16

220

R15

220

R14

220

R13

220

R12

220

R11

220

DL2

DL3

DL4

GN

DS

3210 4

J4 Clic

k_M

ate

5x1

3210 4

J5 Clic

k_M

ate

5x1

Net

wor

k co

nnec

tor

1

3

4B

arto

sz k

aczo

r

PI03

PI04

PI05

PI07

PI08

PI09

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PI020

PI021

PI022

PI023

PI024

PI025

PI026

PI029

PI030

PI031

PI033

PI034

PI035

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

COA

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIC301 PIC302

COC3

PIC401 PIC402

COC4

PIC501 PIC502

COC5

PIC601 PIC602

COC6

PIC701 PIC702

COC7

PIDL101 PIDL102

PIDL103 PIDL104 CODL1

PIDL201 PIDL202

PIDL203 PIDL204 CODL2

PIDL301 PIDL302

PIDL303 PIDL304 CODL3

PIDL401 PIDL402

PIDL403 PIDL404 CODL4

PIJ400

PIJ401

PIJ402

PIJ403

PIJ404

COJ4 PIJ500

PIJ501

PIJ502

PIJ503

PIJ504

COJ5

PIR101 PIR102 COR

1 PIR201 PIR202 COR

2

PIR501 PIR502 COR

5

PIR601 PIR602 COR

6 PIR701 PIR702 COR

7

PIR1101 PIR1102 COR1

1

PIR1201 PIR1202 COR12

PIR1301 PIR1302 COR13

PIR1401 PIR1402 COR14

PIR1501 PIR1502 COR15

PIR1601 PIR1602 COR16

PIR1701 PIR1702 COR17 PIR1801 PIR1802

COR1

8

PIT101

PIT102

PIT103

PIT104

PIT105

PIT106

PIT107

PIT108

PIT109

PIT1010

PIT1011

PIT1012

COT1

PIT201

PIT202

PIT203

PIT204

PIT205

PIT206

PIT207

PIT208

PIT209

PIT2010

PIT2011

PIT2012

COT2

PI030

PIC501

PIT203

NLA01CEN

PI031

PIT202

NLA0

1N

PI029

PIT201

NLA0

1P

PI034

PIC601

PIT204

NLA02CEN

PI035

PIT206

NLA0

2N

PI033

PIT205

NLA0

2P

PI04

PIC101

PIT103

NLB01CEN

PI05

PIT102

NLB0

1N

PI03

PIT101

NLB0

1P

PI08

PIC201

PIT104

NLB02CEN

PI09

PIT106

NLB0

2N

PI07

PIT105

NLB0

2P

PIC102 PIC202

PIC502 PIC602

PIDL101 PIDL103

PIDL201 PIDL203

PIDL301 PIDL303

PIDL401 PIDL403

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PIR1102

PI020

PIR1202

PI021

PIR1302

PI022

PIR1402

PI023

PIR1502

PI024

PIR1602 PI025

PIR1702 PI026

PIR1802

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

PIC301 PIR101

PIR201

PIC401

PIJ404 PIJ504

PIR502

PIC701 PIR601

PIR701

PIDL102

PIR1701

PIDL104

PIR1801

PIDL202

PIR1501

PIDL204

PIR1601

PIDL302

PIR1301

PIDL304

PIR1401

PIDL402

PIR1101

PIDL404

PIR1201

PIJ400

PIT1012

PIJ401

PIT1011

PIJ402

PIT108

PIJ403

PIT107

PIJ500

PIT2012

PIJ501

PIT2011

PIJ502

PIT208

PIJ503

PIT207 PIR102

PIT1010

PIR202

PIT109

PIR602

PIT2010

PIR702

PIT209

PIC302

PIC402

PIC702

PIR501

54 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

P

ower

Supp

lyS

chD

ocD

raw

n B

y

5V GN

D

GN

D

3V3

GN

DS

GN

DS

3V3 R

pu10

k 16V

10nF

C21

GN

DS

Rs

470

TAC

TD60

B1

GN

DS

16V

10uF

Cin

R20

510k

Rpg

499k

R21

162k

16V

22uF

Cou

t

16V

4uF

Cld

o

3V3

GN

DS

GN

DS

16V

12pF

Cx2

16V

12pF

Cx1

GN

DS

3V3

1

GN

D2

GN

D6

GN

D10

GN

D14

GN

D18

3V3

27

GN

D28

GN

D32

GN

D36

GN

D40

GN

D44

3V3

51

GN

D52

Pow

er p

ins

3V3

101

GN

D10

2

GN

D10

8

GN

D11

4

GN

D11

8

GN

D12

4

GN

D12

8

GN

D13

4G

ND

140

GN

D14

4G

ND

150

3V3

155

GN

D15

6

C Any

bus_

CC

_B40

3V3

GN

DS

GN

DG

ND

S

WA

KE_

N64

HIB

_N65

XO

SC0

66

XO

SC1

67

RST

_N70

OSC

088

OSC

189

U1C

TM4C

1292

NC

PDTI

3R

VD

D7

VD

DA

8

VR

EFA

+9

GN

DA

10

VD

D16

GN

D17

VD

D26

VD

D28

VD

D39

VD

D47

GN

D48

VD

D63

VB

AT

68

VD

D69

VD

D79

GN

D80

VD

DC

87

VD

D90

VD

D10

1

VD

D11

3

GN

D11

4

VD

DC

115

VD

D12

2

U1D

TM4C

1292

NC

PDTI

3R

RST

GN

DX

18M

Hz

GN

D

EN1

PG2

VIN

3

VIN

4G

ND

5G

ND

6FB

7V

OU

T8

TPS8

2085

GN

D9

U2

TPS8

2085

GN

D

R66

500k

2kV

1nF

C55 PE

GN

DR

5550

0k2k

V1n

F

C44 PE

GN

D

R77

500k

2kV

1nF

C66 PE

GN

D

16V

100n

F

CV

116

V10

0nF

CV

216

V10

0nF

CV

316

V10

0nF

CV

4

3V3

16V

100n

F

CV

516

V10

0nF

CV

616

V10

0nF

CV

716

V10

0nF

CV

816

V10

0nF

CV

916

V10

0nF

CV

1016

V10

0nF

CV

1116

V10

0nF

CV

1216

V10

0nF

CV

13

GN

D

16V

1uF

CV

ref1

16V

10nF

CV

ref2

GN

DG

ND Po

wer

supp

ly

1

4

4 B

arto

sz K

aczo

r

PI01

PI02

PI06

PI010

PI014

PI018

PI027

PI028

PI032

PI036

PI040

PI044

PI051

PI052

PI0101

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0155

PI0156

COC

PIB101

PIB102

PIB103 PI

B104 COB1

PIC2101 PIC2102

COC2

1

PIC4401 PIC4402

COC44

PIC5501 PIC5502

COC55

PIC6601 PIC6602

COC66

PICin01 PICin02

COCin

PICldo01 PICldo02

COCl

do

PICout01 PICout02

COCo

ut

PICV101 PICV102

COCV1

PICV201 PICV202

COCV

2 PICV301 PICV302

COCV

3 PICV401 PICV402

COCV

4 PICV501 PICV502

COCV5

PICV601 PICV602

COCV

6 PICV701 PICV702

COCV

7 PICV801 PICV802

COCV

8 PICV901 PICV902

COCV

9 PICV1001 PICV1002

COCV10

PICV1101 PICV1102

COCV11

PICV1201 PICV1202

COCV12

PICV1301 PICV1302

COCV13

PICVref101 PICVref102

COCV

ref1

PICVref201 PICVref202

COCVref2

PICx101 PICx102

COCx1

PICx201 PICx202

COCx2

PIR2001 PIR2002 COR20

PIR2

101

PIR2

102 COR

21

PIR5501 PIR5502 COR55

PIR6601 PIR6602 COR

66

PIR7701 PIR7702 CO

R77

PIRpg01 PIRpg02 CO

Rpg

PIRpu01 PIRpu02 CORpu

PIRs01

PIRs

02

CORs

PIU1064

PIU1065

PIU1066

PIU1067

PIU1070

PIU1088

PIU1089

COU1

C

PIU107

PIU108

PIU109

PIU1010

PIU1016

PIU1017

PIU1026

PIU1028

PIU1039

PIU1047

PIU1048

PIU1063

PIU1068

PIU1069

PIU1079

PIU1080

PIU1087

PIU1090

PIU10101

PIU10113

PIU10114

PIU10115

PIU10122

COU1

D

PIU201

PIU202

PIU203

PIU204

PIU205

PIU206

PIU207

PIU208

PIU209

COU2

PIX101 PIX102 PIX10GND

PIX10GND2 COX

1

PI01

PI027

PI051

PI0101

PI0155

PICout01

PICV101 PICV201

PICV301 PICV401

PICV501 PICV601

PICV701 PICV801

PICV901 PICV1001

PICV1101 PICV1201

PICV1301

PICVref101 PICVref201

PIR2002 PIRpg02

PIRpu02

PIU107

PIU108

PIU109

PIU1016

PIU1026

PIU1028

PIU1039

PIU1047

PIU1063

PIU1068

PIU1069

PIU1079

PIU1090

PIU10101

PIU10113

PIU10122

PIU208

PICin01

PIU201

PIU203

PIU204

PI02

PI06

PI010

PI014

PI018

PI028

PI032

PI036

PI040

PI044

PI052

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0156

PIB101

PIB102

PIC2102

PIC4401 PIC5501

PIC6601

PICin02

PICldo02

PICout02

PICV102 PICV202

PICV302 PICV402

PICV502 PICV602

PICV702 PICV802

PICV902 PICV1002

PICV1102 PICV1202

PICV1302

PICVref102 PICVref202

PICx102 PICx202

PIR2

101

PIR5502 PIR6602

PIR7702

PIU1010

PIU1017

PIU1048

PIU1064

PIU1080

PIU1088

PIU10114

PIU205

PIU206

PIU209

PIX10GND

PIX10GND2

PIB103 PI

B104 PIRs01

PICldo01

PIU1087

PIU10115

PICx101

PIU1067

PIX101

PICx201

PIU1066

PIX102

PIR2001 PIR2

102

PIU207

PIRpg01

PIU202

PIU1065

PIU1089

PIC4402 PIC5502

PIC6602

PIR5501 PIR6601

PIR7701

PIC2101 PIRpu01 PI

Rs02

PIU1070

NLRST

Dodatek A Schematy modułoacutew urządzenia 55

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-15

Shee

t o

fFi

le

CU

sers

IM

UO

ldS

chD

ocD

raw

n B

y

8642 16141210 18 20 22 24

17 19 21 239 11 13 157531J5 H

eade

r 12x

2

5V

GN

D

5V

16V

100n

F

C1

16V

100n

F

C2

GN

DRS

TCSD

OU

TD

IO4

DIO

3SC

LKD

IND

IO1

DIO

2

PA2

PA3

PA4

PA5

PB1

PB2

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

1 23 4

J2

5V

GN

D

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIJ201

PIJ202

PIJ203

PIJ204

COJ2

PIJ501

PIJ502

PIJ503

PIJ504

PIJ505

PIJ506

PIJ507

PIJ508

PIJ509

PIJ5

010

PIJ5011

PIJ5012

PIJ5013

PIJ5014

PIJ5

015

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

COJ5

PI1041

PI1080

PI1059

PI1060

PI1061

PI1062

PIC101 PIC201

PIJ201

PIJ5

010

PIJ5011

PIJ5012

PI1066

PIJ506

NLCS

NLPA3

PI1067

PIJ505

NLDIN

NLPA4

PIJ507

NLDI

O1 PIJ509

NLDI

O2 PIJ501

NLDI

O3

PI1045

PIJ502

NLDI

O4

NLPB2

PI1058

PIJ504

NLDOUT

NLPA5

PI1042

PI1043

PI1078

PI1079

PIC102 PIC202

PIJ204

PIJ5013

PIJ5014

PIJ5

015

PIJ202

PIJ203

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

PI1063

PIJ503

NLPA2

NLSCLK

PI102

PIJ508

NLPB1

NLRS

T

PI1029

NLPB4

PI1030

NLPB5

PI103

NLPC0

PI1038

NLPC1

PI1039

NLPC2

PI1040

NLPC3

PI1019

NLPD0

PI1022

NLPD1

PI1021

NLPD2

PI1020

NLPD3

PI1026

NLPD4

PI1025

NLPD5

PI1024

NLPD6

PI1023

NLPD7

PI1014

NLPE0

PI1015

NLPE

1

PI1016

NLPE2

PI1017

NLPE3

PI1028

NLPE4

PI1027

NLPE

5

PI1057

NLPF

0

PI1068

NLPF

1

PI1056

NLPF2

PI1069

NLPF3

PI1055

NLPF

4

PI1054

NLPG2

PI1070

NLPG3

PI1071

NLPG4

PI1053

NLPG5

PI1052

NLPG6

PI1072

NLPG7

PI1010

NLPH0

PI109

NLPH1

PI1065

NLPH2

PI1064

NLPH3

PI1032

NLPJ0

PI1011

NLPJ1

PI1076

NLPK4

PI1049

NLPK5

PI1075

NLPK6

PI1050

NLPK7

PI1077

NLPL5

PI101

NLPL6

PI1044

NLPL7

PI1046

NLPM

4

PI1047

NLPM

5

PI1048

NLPM

6

PI1074

NLPM

7

PI1035

NLPN0

PI1034

NLPN1

PI106

NLPN2

PI107

NLPN3

PI1033

NLPN4

PI108

NLPN5

PI1031

NLPP0

PI1012

NLPP1

PI104

NLPP3

PI1036

NLPP4

PI105

NLPP5

PI1018

NLPQ2

PI1013

NLPQ3

PI1037

NLPQ4

PI1051

NLPQ5

PI1073

NLPQ6

56 Dodatek A Schematy modułoacutew urządzenia

Dodatek B

Wykresy pobranych trajektorii

Dodatek B Wykresy pobranych trajektorii 57

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B1 Trajektoria ucząca z minimalnym zastosowaniem filtru

58 Dodatek B Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

Spis Dodatkoacutew

Dodatek A 51

Dodatek B 57

Spis tablic 49

50 Spis tablic

Dodatek A

Schematy modułoacutew urządzenia

Dodatek A Schematy modułoacutew urządzenia 51

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

A

nybu

s-B

ase

SchD

ocD

raw

n B

y

~RES

ET10

3

A0

~WEH

DIP

1_0

104

A1

DIP

1_1

105

A2

DIP

1_2

106

A3

DIP

1_3

107

A4

DIP

1_4

109

A5

DIP

1_5

110

A6

DIP

1_6

111

A7

DIP

1_7

112

A8

~LD

~SS

113

A9

SCLK

115

A10

DO

MIS

O11

6

A11

DIM

OSI

117

A12

ASM

_RX

119

A13

ASM

_TX

120

D7

DIP

2_7

121

D6

DIP

2_6

122

D5

DIP

2_5

123

D4

DIP

2_4

125

D3

DIP

2_3

126

D2

DIP

2_2

127

D1

DIP

2_1

129

MD

013

0D

0D

IP2_

013

1O

M0

132

OM

113

3

OM

213

5~C

S13

6~W

E~W

ELC

T13

7~I

RQ

PA

138

~OE

139

~LED

4AD

1514

1~L

ED4B

D14

142

~LED

3AD

1314

3

~LED

3BD

1214

5LE

D2A

D11

146

LED

2BD

1014

7LE

D1A

D9

148

LED

1BD

814

9

RX

ASM

_RX

151

TXA

SM_T

XO

M3

152

MI1

153

MI0

SY

NC

154

Hos

t Con

nect

or

B Any

bus_

CC

_B40

PK0

18

PK1

19

PK2

20

PK3

21

PC7

22

PC6

23

PC5

24

PC4

25

Dat

aBus

Pin

sU

1E

TM4C

1292

NC

PDTI

3R

PA6

40

PA7

41

PG0

49PG

150

PB3

92

PQ0

5

PQ1

6

PM3

75

PM2

76

PM1

77

PM0

78

PL0

81

PL1

82

PL2

83

PL3

84

PL4

85

PP2

103

Adr

esB

us P

ins

U1F

TM4C

1292

NC

PDTI

3R

D0

D1

D2

D3

D4

D5

D6

D7

D0

D1

D2

D3

D4

D5

D6

D7A

0A

1A

2A

3A

4A

5A

6A

7A

8A

9A

10A

11A

12A

13

A0

A1

A2

A3

A4

A5

A6

A7

A8

A9

A10

A11

A12

A13

PA0

33

PA1

34

PB0

95

U1A

TM4C

1292

NC

PDTI

3R

GN

D

R33

10k

R34

10k

R35

10k

Any

bus

Page

15

Any

bus

uC c

onne

ctio

n

1

Bar

osz

Kac

zor

1

4

PI0103

PI0104

PI0105

PI0106

PI0107

PI0109

PI0110

PI0111

PI0112

PI0113

PI0115

PI0116

PI0117

PI0119

PI0120

PI0121

PI0122

PI0123

PI0125

PI0126

PI0127

PI0129

PI0130

PI0131

PI0132

PI0133

PI0135

PI0136

PI0137

PI0138

PI0139

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

COB

PIR3301 PIR3302 CO

R33

PIR3401 PIR3402 COR34

PIR3501 PIR3502 CO

R35

PIU1033

PIU1034

PIU1095 COU1

A

PIU1018

PIU1019

PIU1020

PIU1021

PIU1022

PIU1023

PIU1024

PIU1025

COU1

E PIU105

PIU106

PIU1040

PIU1041

PIU1049

PIU1050

PIU1075

PIU1076

PIU1077

PIU1078

PIU1081

PIU1082

PIU1083

PIU1084

PIU1085

PIU1092

PIU10103

COU1

F

PI0104

PIU1040

NLA0

PI0105

PIU1041

NLA1

PI0106

PIU1050

NLA2

PI0107

PIU1049

NLA3

PI0109

PIU1075

NLA4

PI0110

PIU1076

NLA5

PI0111

PIU1077

NLA6

PI0112

PIU1078

NLA7

PI0113

PIU1081

NLA8

PI0115

PIU1082

NLA9

PI0116

PIU1083

NLA10

PI0117

PIU1084

NLA11

PI0119

PIU105

NLA12

PI0120

PIU106

NLA13

PI0131

PIU1018

NLD0

PI0129

PIU1019

NLD1

PI0127

PIU1020

NLD2

PI0126

PIU1021

NLD3 PI0125

PIU1022

NLD4

PI0123

PIU1023

NLD5 PI0122

PIU1024

NLD6

PI0121

PIU1025

NLD7

PIR3301 PIR3401

PIR3501

PI0103

PIU1033

PI0130

PI0132

PIR3302

PI0133

PIR3402

PI0135

PIR3502

PI0136

PIU1085

PI0137

PIU10103

PI0138

PIU1095

PI0139

PIU1092

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

PIU1034

52 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

C

onne

ctor

sSch

Doc

Dra

wn

By

PQ2

11

PQ3

27

PQ5

57

PQ6

58

PM7

71PM

672

PM5

73PM

474

PL5

86

PL7

93PL

694

PQ4

102

PP3

104

PP4

105

PP5

106

PN0

107

PN1

108

PN2

109

PN3

110

PN4

111

PN5

112

PP0

118

PP1

119

U1B

TM4C

1292

NC

PDTI

3R

PD0

1

PD1

2

PD2

3

PD3

4

PE3

12PE

213

PE1

14PE

015

PH0

29

PH1

30

PH2

31

PH3

32

PA2

35

PA3

36

PA4

37

PA5

38

PF0

42

PF1

43

PF2

44

PF3

45

PF4

46

PG2

51

PG3

52

PG4

53

PG5

54

PG6

55

PG7

56

PK7

59PK

660

PK5

61PK

462

PB2

91PB

196

PC3_

TDO

_SW

O97

PC2_

TDI

98PC

1_TM

S_SW

DIO

99PC

0_TC

K_S

WC

LK10

0

PJ0

116

PJ1

117

PB5

120

PB4

121

PE4

123

PE5

124

PD4

125

PD5

126

PD6

127

PD7

128

U1G

TM4C

1292

NC

PDTI

3R

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PH0

PH1

PH2

PH3

PJ0

PJ1

PK4

PK5

PK6

PK7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

PC0

PC1

PC2

PC3

3V3

GN

D

RST

92

101

74

83 5

6

J2 Pin

200

mm

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

Con

nect

or s

chem

a

1

Bar

tosz

Kac

zor

2

4

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIJ201

PIJ202

PIJ203

PIJ204

PIJ205

PIJ206

PIJ207

PIJ208

PIJ209

PIJ2010

COJ2

PIU1011

PIU1027

PIU1057

PIU1058

PIU1071

PIU1072

PIU1073

PIU1074

PIU1086

PIU1093

PIU1094

PIU10102

PIU10104

PIU10105

PIU10106

PIU10107

PIU10108

PIU10109

PIU10110

PIU10111

PIU10112

PIU10118

PIU10119

COU1B

PIU101

PIU102

PIU103

PIU104

PIU1012

PIU1013

PIU1014

PIU1015

PIU1029

PIU1030

PIU1031

PIU1032

PIU1035

PIU1036

PIU1037

PIU1038

PIU1042

PIU1043

PIU1044

PIU1045

PIU1046

PIU1051

PIU1052

PIU1053

PIU1054

PIU1055

PIU1056

PIU1059

PIU1060

PIU1061

PIU1062

PIU1091

PIU1096

PIU1097

PIU1098

PIU1099

PIU10100

PIU10116

PIU10117

PIU10120

PIU10121

PIU10123

PIU10124

PIU10125

PIU10126

PIU10127

PIU10128

COU1G

PI1041

PI1080

PIJ201

PI1059

PI1060

PI1061

PI1062

PI1042

PI1043

PI1078

PI1079

PIJ202

PIJ203

PIJ205

PIJ204

PI1063

PIU1035

NLPA2

PI1066

PIU1036

NLPA3

PI1067

PIU1037

NLPA4

PI1058

PIU1038

NLPA5

PI102

PIU1096

NLPB1

PI1045

PIU1091

NLPB2

PI1029

PIU10121

NLPB4

PI1030

PIU10120

NLPB5

PI103

PIJ209

PIU10100

NLPC0

PI1038

PIJ2010

PIU1099

NLPC

1

PI1039

PIJ207

PIU1098

NLPC2

PI1040

PIJ208

PIU1097

NLPC3

PI1019

PIU101

NLPD

0

PI1022

PIU102

NLPD1

PI1021

PIU103

NLPD

2

PI1020

PIU104

NLPD3

PI1026

PIU10125

NLPD

4

PI1025

PIU10126

NLPD5

PI1024

PIU10127

NLPD

6

PI1023

PIU10128

NLPD

7

PI1014

PIU1015

NLPE0

PI1015

PIU1014

NLPE1

PI1016

PIU1013

NLPE2

PI1017

PIU1012

NLPE3

PI1028

PIU10123

NLPE4

PI1027

PIU10124

NLPE5

PI1057

PIU1042

NLPF0

PI1068

PIU1043

NLPF1

PI1056

PIU1044

NLPF2

PI1069

PIU1045

NLPF3

PI1055

PIU1046

NLPF4

PI1054

PIU1051

NLPG2

PI1070

PIU1052

NLPG3

PI1071

PIU1053

NLPG4

PI1053

PIU1054

NLPG5

PI1052

PIU1055

NLPG6

PI1072

PIU1056

NLPG7

PI1010

PIU1029

NLPH0

PI109

PIU1030

NLPH1

PI1065

PIU1031

NLPH2

PI1064

PIU1032

NLPH3

PI1032

PIU10116

NLPJ0

PI1011

PIU10117

NLPJ1

PI1076

PIU1062

NLPK4

PI1049

PIU1061

NLPK5

PI1075

PIU1060

NLPK6

PI1050

PIU1059

NLPK7

PI1077

PIU1086

NLPL5

PI101

PIU1094

NLPL6

PI1044

PIU1093

NLPL7

PI1046

PIU1074

NLPM

4

PI1047

PIU1073

NLPM

5

PI1048

PIU1072

NLPM

6

PI1074

PIU1071

NLPM

7

PI1035

PIU10107

NLPN0

PI1034

PIU10108

NLPN1

PI106

PIU10109

NLPN2

PI107

PIU10110

NLPN3

PI1033

PIU10111

NLPN4

PI108

PIU10112

NLPN5

PI1031

PIU10118

NLPP0

PI1012

PIU10119

NLPP1

PI104

PIU10104

NLPP3

PI1036

PIU10105

NLPP4

PI105

PIU10106

NLPP5

PI1018

PIU1011

NLPQ

2

PI1013

PIU1027

NLPQ3

PI1037

PIU10102

NLPQ

4

PI1051

PIU1057

NLPQ5

PI1073

PIU1058

NLPQ

6

PIJ206

NLRST

Dodatek A Schematy modułoacutew urządzenia 53

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

N

etw

ork

SchD

ocD

raw

n B

y

B_1

PR

XP

3

B_1

CEN

SD

A4

B_1

NR

XN

5

B_2

PSD

P7

B_2

CEN

SC

L8

B_2

NS

DN

9

B_3

PTX

EN11

B_3

CEN

12

B_3

NT

XD

IS13

B_4

PTX

P15

B_4

CEN

~B

USP

16

B_4

NT

XN

17

NW

_LED

4B19

NW

_LED

4A20

NW

_LED

3B21

NW

_LED

3A22

NW

_LED

2B23

NW

_LED

2A24

NW

_LED

1B25

NW

_LED

1A26

A_1

PR

XP

29A

_1C

ENS

DA

30A

_1N

RX

N31

A_2

PSD

P33

A_2

CEN

SC

L34

A_2

NS

DN

35

A_3

PTX

EN37

A_3

CEN

38A

_3N

TX

DIS

39

A_4

PTX

P41

A_4

CEN

~B

USP

42A

_4N

TX

N43

C_T

X45

C_R

X46

C_T

XEN

47~C

_BU

SP48

GA

TE1

49G

ATE

250

Net

wor

k C

onne

ctor

A Any

bus_

CC

_B40

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T1 47

uHB_

1PB_

1CEN

B_1N

B_1P

B_1C

EN

B_1N

B_2P

B_2C

ENB_

2N

B_2P

B_2C

EN

B_2N

16V

100n

F

C1

16V

100n

F

C2

GN

DS

PE

2kV

1nF

C3

R1

75R

275

R5

1M2k

V1n

F

C4 PE

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T2 47

uH

A_1

P

A_1

CEN

A_1

N

A_2

P

A_2

CEN

A_2

N

16V

100n

F

C5

16V

100n

F

C6

GN

DS

PE

2kV

1nF

C7

R6

75R

775

A_1

PA

_1C

ENA

_1N

A_2

PA

_2C

ENA

_2N

DL1

R18

220

R17

220

R16

220

R15

220

R14

220

R13

220

R12

220

R11

220

DL2

DL3

DL4

GN

DS

3210 4

J4 Clic

k_M

ate

5x1

3210 4

J5 Clic

k_M

ate

5x1

Net

wor

k co

nnec

tor

1

3

4B

arto

sz k

aczo

r

PI03

PI04

PI05

PI07

PI08

PI09

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PI020

PI021

PI022

PI023

PI024

PI025

PI026

PI029

PI030

PI031

PI033

PI034

PI035

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

COA

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIC301 PIC302

COC3

PIC401 PIC402

COC4

PIC501 PIC502

COC5

PIC601 PIC602

COC6

PIC701 PIC702

COC7

PIDL101 PIDL102

PIDL103 PIDL104 CODL1

PIDL201 PIDL202

PIDL203 PIDL204 CODL2

PIDL301 PIDL302

PIDL303 PIDL304 CODL3

PIDL401 PIDL402

PIDL403 PIDL404 CODL4

PIJ400

PIJ401

PIJ402

PIJ403

PIJ404

COJ4 PIJ500

PIJ501

PIJ502

PIJ503

PIJ504

COJ5

PIR101 PIR102 COR

1 PIR201 PIR202 COR

2

PIR501 PIR502 COR

5

PIR601 PIR602 COR

6 PIR701 PIR702 COR

7

PIR1101 PIR1102 COR1

1

PIR1201 PIR1202 COR12

PIR1301 PIR1302 COR13

PIR1401 PIR1402 COR14

PIR1501 PIR1502 COR15

PIR1601 PIR1602 COR16

PIR1701 PIR1702 COR17 PIR1801 PIR1802

COR1

8

PIT101

PIT102

PIT103

PIT104

PIT105

PIT106

PIT107

PIT108

PIT109

PIT1010

PIT1011

PIT1012

COT1

PIT201

PIT202

PIT203

PIT204

PIT205

PIT206

PIT207

PIT208

PIT209

PIT2010

PIT2011

PIT2012

COT2

PI030

PIC501

PIT203

NLA01CEN

PI031

PIT202

NLA0

1N

PI029

PIT201

NLA0

1P

PI034

PIC601

PIT204

NLA02CEN

PI035

PIT206

NLA0

2N

PI033

PIT205

NLA0

2P

PI04

PIC101

PIT103

NLB01CEN

PI05

PIT102

NLB0

1N

PI03

PIT101

NLB0

1P

PI08

PIC201

PIT104

NLB02CEN

PI09

PIT106

NLB0

2N

PI07

PIT105

NLB0

2P

PIC102 PIC202

PIC502 PIC602

PIDL101 PIDL103

PIDL201 PIDL203

PIDL301 PIDL303

PIDL401 PIDL403

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PIR1102

PI020

PIR1202

PI021

PIR1302

PI022

PIR1402

PI023

PIR1502

PI024

PIR1602 PI025

PIR1702 PI026

PIR1802

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

PIC301 PIR101

PIR201

PIC401

PIJ404 PIJ504

PIR502

PIC701 PIR601

PIR701

PIDL102

PIR1701

PIDL104

PIR1801

PIDL202

PIR1501

PIDL204

PIR1601

PIDL302

PIR1301

PIDL304

PIR1401

PIDL402

PIR1101

PIDL404

PIR1201

PIJ400

PIT1012

PIJ401

PIT1011

PIJ402

PIT108

PIJ403

PIT107

PIJ500

PIT2012

PIJ501

PIT2011

PIJ502

PIT208

PIJ503

PIT207 PIR102

PIT1010

PIR202

PIT109

PIR602

PIT2010

PIR702

PIT209

PIC302

PIC402

PIC702

PIR501

54 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

P

ower

Supp

lyS

chD

ocD

raw

n B

y

5V GN

D

GN

D

3V3

GN

DS

GN

DS

3V3 R

pu10

k 16V

10nF

C21

GN

DS

Rs

470

TAC

TD60

B1

GN

DS

16V

10uF

Cin

R20

510k

Rpg

499k

R21

162k

16V

22uF

Cou

t

16V

4uF

Cld

o

3V3

GN

DS

GN

DS

16V

12pF

Cx2

16V

12pF

Cx1

GN

DS

3V3

1

GN

D2

GN

D6

GN

D10

GN

D14

GN

D18

3V3

27

GN

D28

GN

D32

GN

D36

GN

D40

GN

D44

3V3

51

GN

D52

Pow

er p

ins

3V3

101

GN

D10

2

GN

D10

8

GN

D11

4

GN

D11

8

GN

D12

4

GN

D12

8

GN

D13

4G

ND

140

GN

D14

4G

ND

150

3V3

155

GN

D15

6

C Any

bus_

CC

_B40

3V3

GN

DS

GN

DG

ND

S

WA

KE_

N64

HIB

_N65

XO

SC0

66

XO

SC1

67

RST

_N70

OSC

088

OSC

189

U1C

TM4C

1292

NC

PDTI

3R

VD

D7

VD

DA

8

VR

EFA

+9

GN

DA

10

VD

D16

GN

D17

VD

D26

VD

D28

VD

D39

VD

D47

GN

D48

VD

D63

VB

AT

68

VD

D69

VD

D79

GN

D80

VD

DC

87

VD

D90

VD

D10

1

VD

D11

3

GN

D11

4

VD

DC

115

VD

D12

2

U1D

TM4C

1292

NC

PDTI

3R

RST

GN

DX

18M

Hz

GN

D

EN1

PG2

VIN

3

VIN

4G

ND

5G

ND

6FB

7V

OU

T8

TPS8

2085

GN

D9

U2

TPS8

2085

GN

D

R66

500k

2kV

1nF

C55 PE

GN

DR

5550

0k2k

V1n

F

C44 PE

GN

D

R77

500k

2kV

1nF

C66 PE

GN

D

16V

100n

F

CV

116

V10

0nF

CV

216

V10

0nF

CV

316

V10

0nF

CV

4

3V3

16V

100n

F

CV

516

V10

0nF

CV

616

V10

0nF

CV

716

V10

0nF

CV

816

V10

0nF

CV

916

V10

0nF

CV

1016

V10

0nF

CV

1116

V10

0nF

CV

1216

V10

0nF

CV

13

GN

D

16V

1uF

CV

ref1

16V

10nF

CV

ref2

GN

DG

ND Po

wer

supp

ly

1

4

4 B

arto

sz K

aczo

r

PI01

PI02

PI06

PI010

PI014

PI018

PI027

PI028

PI032

PI036

PI040

PI044

PI051

PI052

PI0101

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0155

PI0156

COC

PIB101

PIB102

PIB103 PI

B104 COB1

PIC2101 PIC2102

COC2

1

PIC4401 PIC4402

COC44

PIC5501 PIC5502

COC55

PIC6601 PIC6602

COC66

PICin01 PICin02

COCin

PICldo01 PICldo02

COCl

do

PICout01 PICout02

COCo

ut

PICV101 PICV102

COCV1

PICV201 PICV202

COCV

2 PICV301 PICV302

COCV

3 PICV401 PICV402

COCV

4 PICV501 PICV502

COCV5

PICV601 PICV602

COCV

6 PICV701 PICV702

COCV

7 PICV801 PICV802

COCV

8 PICV901 PICV902

COCV

9 PICV1001 PICV1002

COCV10

PICV1101 PICV1102

COCV11

PICV1201 PICV1202

COCV12

PICV1301 PICV1302

COCV13

PICVref101 PICVref102

COCV

ref1

PICVref201 PICVref202

COCVref2

PICx101 PICx102

COCx1

PICx201 PICx202

COCx2

PIR2001 PIR2002 COR20

PIR2

101

PIR2

102 COR

21

PIR5501 PIR5502 COR55

PIR6601 PIR6602 COR

66

PIR7701 PIR7702 CO

R77

PIRpg01 PIRpg02 CO

Rpg

PIRpu01 PIRpu02 CORpu

PIRs01

PIRs

02

CORs

PIU1064

PIU1065

PIU1066

PIU1067

PIU1070

PIU1088

PIU1089

COU1

C

PIU107

PIU108

PIU109

PIU1010

PIU1016

PIU1017

PIU1026

PIU1028

PIU1039

PIU1047

PIU1048

PIU1063

PIU1068

PIU1069

PIU1079

PIU1080

PIU1087

PIU1090

PIU10101

PIU10113

PIU10114

PIU10115

PIU10122

COU1

D

PIU201

PIU202

PIU203

PIU204

PIU205

PIU206

PIU207

PIU208

PIU209

COU2

PIX101 PIX102 PIX10GND

PIX10GND2 COX

1

PI01

PI027

PI051

PI0101

PI0155

PICout01

PICV101 PICV201

PICV301 PICV401

PICV501 PICV601

PICV701 PICV801

PICV901 PICV1001

PICV1101 PICV1201

PICV1301

PICVref101 PICVref201

PIR2002 PIRpg02

PIRpu02

PIU107

PIU108

PIU109

PIU1016

PIU1026

PIU1028

PIU1039

PIU1047

PIU1063

PIU1068

PIU1069

PIU1079

PIU1090

PIU10101

PIU10113

PIU10122

PIU208

PICin01

PIU201

PIU203

PIU204

PI02

PI06

PI010

PI014

PI018

PI028

PI032

PI036

PI040

PI044

PI052

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0156

PIB101

PIB102

PIC2102

PIC4401 PIC5501

PIC6601

PICin02

PICldo02

PICout02

PICV102 PICV202

PICV302 PICV402

PICV502 PICV602

PICV702 PICV802

PICV902 PICV1002

PICV1102 PICV1202

PICV1302

PICVref102 PICVref202

PICx102 PICx202

PIR2

101

PIR5502 PIR6602

PIR7702

PIU1010

PIU1017

PIU1048

PIU1064

PIU1080

PIU1088

PIU10114

PIU205

PIU206

PIU209

PIX10GND

PIX10GND2

PIB103 PI

B104 PIRs01

PICldo01

PIU1087

PIU10115

PICx101

PIU1067

PIX101

PICx201

PIU1066

PIX102

PIR2001 PIR2

102

PIU207

PIRpg01

PIU202

PIU1065

PIU1089

PIC4402 PIC5502

PIC6602

PIR5501 PIR6601

PIR7701

PIC2101 PIRpu01 PI

Rs02

PIU1070

NLRST

Dodatek A Schematy modułoacutew urządzenia 55

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-15

Shee

t o

fFi

le

CU

sers

IM

UO

ldS

chD

ocD

raw

n B

y

8642 16141210 18 20 22 24

17 19 21 239 11 13 157531J5 H

eade

r 12x

2

5V

GN

D

5V

16V

100n

F

C1

16V

100n

F

C2

GN

DRS

TCSD

OU

TD

IO4

DIO

3SC

LKD

IND

IO1

DIO

2

PA2

PA3

PA4

PA5

PB1

PB2

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

1 23 4

J2

5V

GN

D

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIJ201

PIJ202

PIJ203

PIJ204

COJ2

PIJ501

PIJ502

PIJ503

PIJ504

PIJ505

PIJ506

PIJ507

PIJ508

PIJ509

PIJ5

010

PIJ5011

PIJ5012

PIJ5013

PIJ5014

PIJ5

015

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

COJ5

PI1041

PI1080

PI1059

PI1060

PI1061

PI1062

PIC101 PIC201

PIJ201

PIJ5

010

PIJ5011

PIJ5012

PI1066

PIJ506

NLCS

NLPA3

PI1067

PIJ505

NLDIN

NLPA4

PIJ507

NLDI

O1 PIJ509

NLDI

O2 PIJ501

NLDI

O3

PI1045

PIJ502

NLDI

O4

NLPB2

PI1058

PIJ504

NLDOUT

NLPA5

PI1042

PI1043

PI1078

PI1079

PIC102 PIC202

PIJ204

PIJ5013

PIJ5014

PIJ5

015

PIJ202

PIJ203

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

PI1063

PIJ503

NLPA2

NLSCLK

PI102

PIJ508

NLPB1

NLRS

T

PI1029

NLPB4

PI1030

NLPB5

PI103

NLPC0

PI1038

NLPC1

PI1039

NLPC2

PI1040

NLPC3

PI1019

NLPD0

PI1022

NLPD1

PI1021

NLPD2

PI1020

NLPD3

PI1026

NLPD4

PI1025

NLPD5

PI1024

NLPD6

PI1023

NLPD7

PI1014

NLPE0

PI1015

NLPE

1

PI1016

NLPE2

PI1017

NLPE3

PI1028

NLPE4

PI1027

NLPE

5

PI1057

NLPF

0

PI1068

NLPF

1

PI1056

NLPF2

PI1069

NLPF3

PI1055

NLPF

4

PI1054

NLPG2

PI1070

NLPG3

PI1071

NLPG4

PI1053

NLPG5

PI1052

NLPG6

PI1072

NLPG7

PI1010

NLPH0

PI109

NLPH1

PI1065

NLPH2

PI1064

NLPH3

PI1032

NLPJ0

PI1011

NLPJ1

PI1076

NLPK4

PI1049

NLPK5

PI1075

NLPK6

PI1050

NLPK7

PI1077

NLPL5

PI101

NLPL6

PI1044

NLPL7

PI1046

NLPM

4

PI1047

NLPM

5

PI1048

NLPM

6

PI1074

NLPM

7

PI1035

NLPN0

PI1034

NLPN1

PI106

NLPN2

PI107

NLPN3

PI1033

NLPN4

PI108

NLPN5

PI1031

NLPP0

PI1012

NLPP1

PI104

NLPP3

PI1036

NLPP4

PI105

NLPP5

PI1018

NLPQ2

PI1013

NLPQ3

PI1037

NLPQ4

PI1051

NLPQ5

PI1073

NLPQ6

56 Dodatek A Schematy modułoacutew urządzenia

Dodatek B

Wykresy pobranych trajektorii

Dodatek B Wykresy pobranych trajektorii 57

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B1 Trajektoria ucząca z minimalnym zastosowaniem filtru

58 Dodatek B Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

50 Spis tablic

Dodatek A

Schematy modułoacutew urządzenia

Dodatek A Schematy modułoacutew urządzenia 51

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

A

nybu

s-B

ase

SchD

ocD

raw

n B

y

~RES

ET10

3

A0

~WEH

DIP

1_0

104

A1

DIP

1_1

105

A2

DIP

1_2

106

A3

DIP

1_3

107

A4

DIP

1_4

109

A5

DIP

1_5

110

A6

DIP

1_6

111

A7

DIP

1_7

112

A8

~LD

~SS

113

A9

SCLK

115

A10

DO

MIS

O11

6

A11

DIM

OSI

117

A12

ASM

_RX

119

A13

ASM

_TX

120

D7

DIP

2_7

121

D6

DIP

2_6

122

D5

DIP

2_5

123

D4

DIP

2_4

125

D3

DIP

2_3

126

D2

DIP

2_2

127

D1

DIP

2_1

129

MD

013

0D

0D

IP2_

013

1O

M0

132

OM

113

3

OM

213

5~C

S13

6~W

E~W

ELC

T13

7~I

RQ

PA

138

~OE

139

~LED

4AD

1514

1~L

ED4B

D14

142

~LED

3AD

1314

3

~LED

3BD

1214

5LE

D2A

D11

146

LED

2BD

1014

7LE

D1A

D9

148

LED

1BD

814

9

RX

ASM

_RX

151

TXA

SM_T

XO

M3

152

MI1

153

MI0

SY

NC

154

Hos

t Con

nect

or

B Any

bus_

CC

_B40

PK0

18

PK1

19

PK2

20

PK3

21

PC7

22

PC6

23

PC5

24

PC4

25

Dat

aBus

Pin

sU

1E

TM4C

1292

NC

PDTI

3R

PA6

40

PA7

41

PG0

49PG

150

PB3

92

PQ0

5

PQ1

6

PM3

75

PM2

76

PM1

77

PM0

78

PL0

81

PL1

82

PL2

83

PL3

84

PL4

85

PP2

103

Adr

esB

us P

ins

U1F

TM4C

1292

NC

PDTI

3R

D0

D1

D2

D3

D4

D5

D6

D7

D0

D1

D2

D3

D4

D5

D6

D7A

0A

1A

2A

3A

4A

5A

6A

7A

8A

9A

10A

11A

12A

13

A0

A1

A2

A3

A4

A5

A6

A7

A8

A9

A10

A11

A12

A13

PA0

33

PA1

34

PB0

95

U1A

TM4C

1292

NC

PDTI

3R

GN

D

R33

10k

R34

10k

R35

10k

Any

bus

Page

15

Any

bus

uC c

onne

ctio

n

1

Bar

osz

Kac

zor

1

4

PI0103

PI0104

PI0105

PI0106

PI0107

PI0109

PI0110

PI0111

PI0112

PI0113

PI0115

PI0116

PI0117

PI0119

PI0120

PI0121

PI0122

PI0123

PI0125

PI0126

PI0127

PI0129

PI0130

PI0131

PI0132

PI0133

PI0135

PI0136

PI0137

PI0138

PI0139

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

COB

PIR3301 PIR3302 CO

R33

PIR3401 PIR3402 COR34

PIR3501 PIR3502 CO

R35

PIU1033

PIU1034

PIU1095 COU1

A

PIU1018

PIU1019

PIU1020

PIU1021

PIU1022

PIU1023

PIU1024

PIU1025

COU1

E PIU105

PIU106

PIU1040

PIU1041

PIU1049

PIU1050

PIU1075

PIU1076

PIU1077

PIU1078

PIU1081

PIU1082

PIU1083

PIU1084

PIU1085

PIU1092

PIU10103

COU1

F

PI0104

PIU1040

NLA0

PI0105

PIU1041

NLA1

PI0106

PIU1050

NLA2

PI0107

PIU1049

NLA3

PI0109

PIU1075

NLA4

PI0110

PIU1076

NLA5

PI0111

PIU1077

NLA6

PI0112

PIU1078

NLA7

PI0113

PIU1081

NLA8

PI0115

PIU1082

NLA9

PI0116

PIU1083

NLA10

PI0117

PIU1084

NLA11

PI0119

PIU105

NLA12

PI0120

PIU106

NLA13

PI0131

PIU1018

NLD0

PI0129

PIU1019

NLD1

PI0127

PIU1020

NLD2

PI0126

PIU1021

NLD3 PI0125

PIU1022

NLD4

PI0123

PIU1023

NLD5 PI0122

PIU1024

NLD6

PI0121

PIU1025

NLD7

PIR3301 PIR3401

PIR3501

PI0103

PIU1033

PI0130

PI0132

PIR3302

PI0133

PIR3402

PI0135

PIR3502

PI0136

PIU1085

PI0137

PIU10103

PI0138

PIU1095

PI0139

PIU1092

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

PIU1034

52 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

C

onne

ctor

sSch

Doc

Dra

wn

By

PQ2

11

PQ3

27

PQ5

57

PQ6

58

PM7

71PM

672

PM5

73PM

474

PL5

86

PL7

93PL

694

PQ4

102

PP3

104

PP4

105

PP5

106

PN0

107

PN1

108

PN2

109

PN3

110

PN4

111

PN5

112

PP0

118

PP1

119

U1B

TM4C

1292

NC

PDTI

3R

PD0

1

PD1

2

PD2

3

PD3

4

PE3

12PE

213

PE1

14PE

015

PH0

29

PH1

30

PH2

31

PH3

32

PA2

35

PA3

36

PA4

37

PA5

38

PF0

42

PF1

43

PF2

44

PF3

45

PF4

46

PG2

51

PG3

52

PG4

53

PG5

54

PG6

55

PG7

56

PK7

59PK

660

PK5

61PK

462

PB2

91PB

196

PC3_

TDO

_SW

O97

PC2_

TDI

98PC

1_TM

S_SW

DIO

99PC

0_TC

K_S

WC

LK10

0

PJ0

116

PJ1

117

PB5

120

PB4

121

PE4

123

PE5

124

PD4

125

PD5

126

PD6

127

PD7

128

U1G

TM4C

1292

NC

PDTI

3R

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PH0

PH1

PH2

PH3

PJ0

PJ1

PK4

PK5

PK6

PK7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

PC0

PC1

PC2

PC3

3V3

GN

D

RST

92

101

74

83 5

6

J2 Pin

200

mm

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

Con

nect

or s

chem

a

1

Bar

tosz

Kac

zor

2

4

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIJ201

PIJ202

PIJ203

PIJ204

PIJ205

PIJ206

PIJ207

PIJ208

PIJ209

PIJ2010

COJ2

PIU1011

PIU1027

PIU1057

PIU1058

PIU1071

PIU1072

PIU1073

PIU1074

PIU1086

PIU1093

PIU1094

PIU10102

PIU10104

PIU10105

PIU10106

PIU10107

PIU10108

PIU10109

PIU10110

PIU10111

PIU10112

PIU10118

PIU10119

COU1B

PIU101

PIU102

PIU103

PIU104

PIU1012

PIU1013

PIU1014

PIU1015

PIU1029

PIU1030

PIU1031

PIU1032

PIU1035

PIU1036

PIU1037

PIU1038

PIU1042

PIU1043

PIU1044

PIU1045

PIU1046

PIU1051

PIU1052

PIU1053

PIU1054

PIU1055

PIU1056

PIU1059

PIU1060

PIU1061

PIU1062

PIU1091

PIU1096

PIU1097

PIU1098

PIU1099

PIU10100

PIU10116

PIU10117

PIU10120

PIU10121

PIU10123

PIU10124

PIU10125

PIU10126

PIU10127

PIU10128

COU1G

PI1041

PI1080

PIJ201

PI1059

PI1060

PI1061

PI1062

PI1042

PI1043

PI1078

PI1079

PIJ202

PIJ203

PIJ205

PIJ204

PI1063

PIU1035

NLPA2

PI1066

PIU1036

NLPA3

PI1067

PIU1037

NLPA4

PI1058

PIU1038

NLPA5

PI102

PIU1096

NLPB1

PI1045

PIU1091

NLPB2

PI1029

PIU10121

NLPB4

PI1030

PIU10120

NLPB5

PI103

PIJ209

PIU10100

NLPC0

PI1038

PIJ2010

PIU1099

NLPC

1

PI1039

PIJ207

PIU1098

NLPC2

PI1040

PIJ208

PIU1097

NLPC3

PI1019

PIU101

NLPD

0

PI1022

PIU102

NLPD1

PI1021

PIU103

NLPD

2

PI1020

PIU104

NLPD3

PI1026

PIU10125

NLPD

4

PI1025

PIU10126

NLPD5

PI1024

PIU10127

NLPD

6

PI1023

PIU10128

NLPD

7

PI1014

PIU1015

NLPE0

PI1015

PIU1014

NLPE1

PI1016

PIU1013

NLPE2

PI1017

PIU1012

NLPE3

PI1028

PIU10123

NLPE4

PI1027

PIU10124

NLPE5

PI1057

PIU1042

NLPF0

PI1068

PIU1043

NLPF1

PI1056

PIU1044

NLPF2

PI1069

PIU1045

NLPF3

PI1055

PIU1046

NLPF4

PI1054

PIU1051

NLPG2

PI1070

PIU1052

NLPG3

PI1071

PIU1053

NLPG4

PI1053

PIU1054

NLPG5

PI1052

PIU1055

NLPG6

PI1072

PIU1056

NLPG7

PI1010

PIU1029

NLPH0

PI109

PIU1030

NLPH1

PI1065

PIU1031

NLPH2

PI1064

PIU1032

NLPH3

PI1032

PIU10116

NLPJ0

PI1011

PIU10117

NLPJ1

PI1076

PIU1062

NLPK4

PI1049

PIU1061

NLPK5

PI1075

PIU1060

NLPK6

PI1050

PIU1059

NLPK7

PI1077

PIU1086

NLPL5

PI101

PIU1094

NLPL6

PI1044

PIU1093

NLPL7

PI1046

PIU1074

NLPM

4

PI1047

PIU1073

NLPM

5

PI1048

PIU1072

NLPM

6

PI1074

PIU1071

NLPM

7

PI1035

PIU10107

NLPN0

PI1034

PIU10108

NLPN1

PI106

PIU10109

NLPN2

PI107

PIU10110

NLPN3

PI1033

PIU10111

NLPN4

PI108

PIU10112

NLPN5

PI1031

PIU10118

NLPP0

PI1012

PIU10119

NLPP1

PI104

PIU10104

NLPP3

PI1036

PIU10105

NLPP4

PI105

PIU10106

NLPP5

PI1018

PIU1011

NLPQ

2

PI1013

PIU1027

NLPQ3

PI1037

PIU10102

NLPQ

4

PI1051

PIU1057

NLPQ5

PI1073

PIU1058

NLPQ

6

PIJ206

NLRST

Dodatek A Schematy modułoacutew urządzenia 53

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

N

etw

ork

SchD

ocD

raw

n B

y

B_1

PR

XP

3

B_1

CEN

SD

A4

B_1

NR

XN

5

B_2

PSD

P7

B_2

CEN

SC

L8

B_2

NS

DN

9

B_3

PTX

EN11

B_3

CEN

12

B_3

NT

XD

IS13

B_4

PTX

P15

B_4

CEN

~B

USP

16

B_4

NT

XN

17

NW

_LED

4B19

NW

_LED

4A20

NW

_LED

3B21

NW

_LED

3A22

NW

_LED

2B23

NW

_LED

2A24

NW

_LED

1B25

NW

_LED

1A26

A_1

PR

XP

29A

_1C

ENS

DA

30A

_1N

RX

N31

A_2

PSD

P33

A_2

CEN

SC

L34

A_2

NS

DN

35

A_3

PTX

EN37

A_3

CEN

38A

_3N

TX

DIS

39

A_4

PTX

P41

A_4

CEN

~B

USP

42A

_4N

TX

N43

C_T

X45

C_R

X46

C_T

XEN

47~C

_BU

SP48

GA

TE1

49G

ATE

250

Net

wor

k C

onne

ctor

A Any

bus_

CC

_B40

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T1 47

uHB_

1PB_

1CEN

B_1N

B_1P

B_1C

EN

B_1N

B_2P

B_2C

ENB_

2N

B_2P

B_2C

EN

B_2N

16V

100n

F

C1

16V

100n

F

C2

GN

DS

PE

2kV

1nF

C3

R1

75R

275

R5

1M2k

V1n

F

C4 PE

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T2 47

uH

A_1

P

A_1

CEN

A_1

N

A_2

P

A_2

CEN

A_2

N

16V

100n

F

C5

16V

100n

F

C6

GN

DS

PE

2kV

1nF

C7

R6

75R

775

A_1

PA

_1C

ENA

_1N

A_2

PA

_2C

ENA

_2N

DL1

R18

220

R17

220

R16

220

R15

220

R14

220

R13

220

R12

220

R11

220

DL2

DL3

DL4

GN

DS

3210 4

J4 Clic

k_M

ate

5x1

3210 4

J5 Clic

k_M

ate

5x1

Net

wor

k co

nnec

tor

1

3

4B

arto

sz k

aczo

r

PI03

PI04

PI05

PI07

PI08

PI09

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PI020

PI021

PI022

PI023

PI024

PI025

PI026

PI029

PI030

PI031

PI033

PI034

PI035

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

COA

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIC301 PIC302

COC3

PIC401 PIC402

COC4

PIC501 PIC502

COC5

PIC601 PIC602

COC6

PIC701 PIC702

COC7

PIDL101 PIDL102

PIDL103 PIDL104 CODL1

PIDL201 PIDL202

PIDL203 PIDL204 CODL2

PIDL301 PIDL302

PIDL303 PIDL304 CODL3

PIDL401 PIDL402

PIDL403 PIDL404 CODL4

PIJ400

PIJ401

PIJ402

PIJ403

PIJ404

COJ4 PIJ500

PIJ501

PIJ502

PIJ503

PIJ504

COJ5

PIR101 PIR102 COR

1 PIR201 PIR202 COR

2

PIR501 PIR502 COR

5

PIR601 PIR602 COR

6 PIR701 PIR702 COR

7

PIR1101 PIR1102 COR1

1

PIR1201 PIR1202 COR12

PIR1301 PIR1302 COR13

PIR1401 PIR1402 COR14

PIR1501 PIR1502 COR15

PIR1601 PIR1602 COR16

PIR1701 PIR1702 COR17 PIR1801 PIR1802

COR1

8

PIT101

PIT102

PIT103

PIT104

PIT105

PIT106

PIT107

PIT108

PIT109

PIT1010

PIT1011

PIT1012

COT1

PIT201

PIT202

PIT203

PIT204

PIT205

PIT206

PIT207

PIT208

PIT209

PIT2010

PIT2011

PIT2012

COT2

PI030

PIC501

PIT203

NLA01CEN

PI031

PIT202

NLA0

1N

PI029

PIT201

NLA0

1P

PI034

PIC601

PIT204

NLA02CEN

PI035

PIT206

NLA0

2N

PI033

PIT205

NLA0

2P

PI04

PIC101

PIT103

NLB01CEN

PI05

PIT102

NLB0

1N

PI03

PIT101

NLB0

1P

PI08

PIC201

PIT104

NLB02CEN

PI09

PIT106

NLB0

2N

PI07

PIT105

NLB0

2P

PIC102 PIC202

PIC502 PIC602

PIDL101 PIDL103

PIDL201 PIDL203

PIDL301 PIDL303

PIDL401 PIDL403

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PIR1102

PI020

PIR1202

PI021

PIR1302

PI022

PIR1402

PI023

PIR1502

PI024

PIR1602 PI025

PIR1702 PI026

PIR1802

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

PIC301 PIR101

PIR201

PIC401

PIJ404 PIJ504

PIR502

PIC701 PIR601

PIR701

PIDL102

PIR1701

PIDL104

PIR1801

PIDL202

PIR1501

PIDL204

PIR1601

PIDL302

PIR1301

PIDL304

PIR1401

PIDL402

PIR1101

PIDL404

PIR1201

PIJ400

PIT1012

PIJ401

PIT1011

PIJ402

PIT108

PIJ403

PIT107

PIJ500

PIT2012

PIJ501

PIT2011

PIJ502

PIT208

PIJ503

PIT207 PIR102

PIT1010

PIR202

PIT109

PIR602

PIT2010

PIR702

PIT209

PIC302

PIC402

PIC702

PIR501

54 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

P

ower

Supp

lyS

chD

ocD

raw

n B

y

5V GN

D

GN

D

3V3

GN

DS

GN

DS

3V3 R

pu10

k 16V

10nF

C21

GN

DS

Rs

470

TAC

TD60

B1

GN

DS

16V

10uF

Cin

R20

510k

Rpg

499k

R21

162k

16V

22uF

Cou

t

16V

4uF

Cld

o

3V3

GN

DS

GN

DS

16V

12pF

Cx2

16V

12pF

Cx1

GN

DS

3V3

1

GN

D2

GN

D6

GN

D10

GN

D14

GN

D18

3V3

27

GN

D28

GN

D32

GN

D36

GN

D40

GN

D44

3V3

51

GN

D52

Pow

er p

ins

3V3

101

GN

D10

2

GN

D10

8

GN

D11

4

GN

D11

8

GN

D12

4

GN

D12

8

GN

D13

4G

ND

140

GN

D14

4G

ND

150

3V3

155

GN

D15

6

C Any

bus_

CC

_B40

3V3

GN

DS

GN

DG

ND

S

WA

KE_

N64

HIB

_N65

XO

SC0

66

XO

SC1

67

RST

_N70

OSC

088

OSC

189

U1C

TM4C

1292

NC

PDTI

3R

VD

D7

VD

DA

8

VR

EFA

+9

GN

DA

10

VD

D16

GN

D17

VD

D26

VD

D28

VD

D39

VD

D47

GN

D48

VD

D63

VB

AT

68

VD

D69

VD

D79

GN

D80

VD

DC

87

VD

D90

VD

D10

1

VD

D11

3

GN

D11

4

VD

DC

115

VD

D12

2

U1D

TM4C

1292

NC

PDTI

3R

RST

GN

DX

18M

Hz

GN

D

EN1

PG2

VIN

3

VIN

4G

ND

5G

ND

6FB

7V

OU

T8

TPS8

2085

GN

D9

U2

TPS8

2085

GN

D

R66

500k

2kV

1nF

C55 PE

GN

DR

5550

0k2k

V1n

F

C44 PE

GN

D

R77

500k

2kV

1nF

C66 PE

GN

D

16V

100n

F

CV

116

V10

0nF

CV

216

V10

0nF

CV

316

V10

0nF

CV

4

3V3

16V

100n

F

CV

516

V10

0nF

CV

616

V10

0nF

CV

716

V10

0nF

CV

816

V10

0nF

CV

916

V10

0nF

CV

1016

V10

0nF

CV

1116

V10

0nF

CV

1216

V10

0nF

CV

13

GN

D

16V

1uF

CV

ref1

16V

10nF

CV

ref2

GN

DG

ND Po

wer

supp

ly

1

4

4 B

arto

sz K

aczo

r

PI01

PI02

PI06

PI010

PI014

PI018

PI027

PI028

PI032

PI036

PI040

PI044

PI051

PI052

PI0101

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0155

PI0156

COC

PIB101

PIB102

PIB103 PI

B104 COB1

PIC2101 PIC2102

COC2

1

PIC4401 PIC4402

COC44

PIC5501 PIC5502

COC55

PIC6601 PIC6602

COC66

PICin01 PICin02

COCin

PICldo01 PICldo02

COCl

do

PICout01 PICout02

COCo

ut

PICV101 PICV102

COCV1

PICV201 PICV202

COCV

2 PICV301 PICV302

COCV

3 PICV401 PICV402

COCV

4 PICV501 PICV502

COCV5

PICV601 PICV602

COCV

6 PICV701 PICV702

COCV

7 PICV801 PICV802

COCV

8 PICV901 PICV902

COCV

9 PICV1001 PICV1002

COCV10

PICV1101 PICV1102

COCV11

PICV1201 PICV1202

COCV12

PICV1301 PICV1302

COCV13

PICVref101 PICVref102

COCV

ref1

PICVref201 PICVref202

COCVref2

PICx101 PICx102

COCx1

PICx201 PICx202

COCx2

PIR2001 PIR2002 COR20

PIR2

101

PIR2

102 COR

21

PIR5501 PIR5502 COR55

PIR6601 PIR6602 COR

66

PIR7701 PIR7702 CO

R77

PIRpg01 PIRpg02 CO

Rpg

PIRpu01 PIRpu02 CORpu

PIRs01

PIRs

02

CORs

PIU1064

PIU1065

PIU1066

PIU1067

PIU1070

PIU1088

PIU1089

COU1

C

PIU107

PIU108

PIU109

PIU1010

PIU1016

PIU1017

PIU1026

PIU1028

PIU1039

PIU1047

PIU1048

PIU1063

PIU1068

PIU1069

PIU1079

PIU1080

PIU1087

PIU1090

PIU10101

PIU10113

PIU10114

PIU10115

PIU10122

COU1

D

PIU201

PIU202

PIU203

PIU204

PIU205

PIU206

PIU207

PIU208

PIU209

COU2

PIX101 PIX102 PIX10GND

PIX10GND2 COX

1

PI01

PI027

PI051

PI0101

PI0155

PICout01

PICV101 PICV201

PICV301 PICV401

PICV501 PICV601

PICV701 PICV801

PICV901 PICV1001

PICV1101 PICV1201

PICV1301

PICVref101 PICVref201

PIR2002 PIRpg02

PIRpu02

PIU107

PIU108

PIU109

PIU1016

PIU1026

PIU1028

PIU1039

PIU1047

PIU1063

PIU1068

PIU1069

PIU1079

PIU1090

PIU10101

PIU10113

PIU10122

PIU208

PICin01

PIU201

PIU203

PIU204

PI02

PI06

PI010

PI014

PI018

PI028

PI032

PI036

PI040

PI044

PI052

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0156

PIB101

PIB102

PIC2102

PIC4401 PIC5501

PIC6601

PICin02

PICldo02

PICout02

PICV102 PICV202

PICV302 PICV402

PICV502 PICV602

PICV702 PICV802

PICV902 PICV1002

PICV1102 PICV1202

PICV1302

PICVref102 PICVref202

PICx102 PICx202

PIR2

101

PIR5502 PIR6602

PIR7702

PIU1010

PIU1017

PIU1048

PIU1064

PIU1080

PIU1088

PIU10114

PIU205

PIU206

PIU209

PIX10GND

PIX10GND2

PIB103 PI

B104 PIRs01

PICldo01

PIU1087

PIU10115

PICx101

PIU1067

PIX101

PICx201

PIU1066

PIX102

PIR2001 PIR2

102

PIU207

PIRpg01

PIU202

PIU1065

PIU1089

PIC4402 PIC5502

PIC6602

PIR5501 PIR6601

PIR7701

PIC2101 PIRpu01 PI

Rs02

PIU1070

NLRST

Dodatek A Schematy modułoacutew urządzenia 55

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-15

Shee

t o

fFi

le

CU

sers

IM

UO

ldS

chD

ocD

raw

n B

y

8642 16141210 18 20 22 24

17 19 21 239 11 13 157531J5 H

eade

r 12x

2

5V

GN

D

5V

16V

100n

F

C1

16V

100n

F

C2

GN

DRS

TCSD

OU

TD

IO4

DIO

3SC

LKD

IND

IO1

DIO

2

PA2

PA3

PA4

PA5

PB1

PB2

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

1 23 4

J2

5V

GN

D

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIJ201

PIJ202

PIJ203

PIJ204

COJ2

PIJ501

PIJ502

PIJ503

PIJ504

PIJ505

PIJ506

PIJ507

PIJ508

PIJ509

PIJ5

010

PIJ5011

PIJ5012

PIJ5013

PIJ5014

PIJ5

015

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

COJ5

PI1041

PI1080

PI1059

PI1060

PI1061

PI1062

PIC101 PIC201

PIJ201

PIJ5

010

PIJ5011

PIJ5012

PI1066

PIJ506

NLCS

NLPA3

PI1067

PIJ505

NLDIN

NLPA4

PIJ507

NLDI

O1 PIJ509

NLDI

O2 PIJ501

NLDI

O3

PI1045

PIJ502

NLDI

O4

NLPB2

PI1058

PIJ504

NLDOUT

NLPA5

PI1042

PI1043

PI1078

PI1079

PIC102 PIC202

PIJ204

PIJ5013

PIJ5014

PIJ5

015

PIJ202

PIJ203

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

PI1063

PIJ503

NLPA2

NLSCLK

PI102

PIJ508

NLPB1

NLRS

T

PI1029

NLPB4

PI1030

NLPB5

PI103

NLPC0

PI1038

NLPC1

PI1039

NLPC2

PI1040

NLPC3

PI1019

NLPD0

PI1022

NLPD1

PI1021

NLPD2

PI1020

NLPD3

PI1026

NLPD4

PI1025

NLPD5

PI1024

NLPD6

PI1023

NLPD7

PI1014

NLPE0

PI1015

NLPE

1

PI1016

NLPE2

PI1017

NLPE3

PI1028

NLPE4

PI1027

NLPE

5

PI1057

NLPF

0

PI1068

NLPF

1

PI1056

NLPF2

PI1069

NLPF3

PI1055

NLPF

4

PI1054

NLPG2

PI1070

NLPG3

PI1071

NLPG4

PI1053

NLPG5

PI1052

NLPG6

PI1072

NLPG7

PI1010

NLPH0

PI109

NLPH1

PI1065

NLPH2

PI1064

NLPH3

PI1032

NLPJ0

PI1011

NLPJ1

PI1076

NLPK4

PI1049

NLPK5

PI1075

NLPK6

PI1050

NLPK7

PI1077

NLPL5

PI101

NLPL6

PI1044

NLPL7

PI1046

NLPM

4

PI1047

NLPM

5

PI1048

NLPM

6

PI1074

NLPM

7

PI1035

NLPN0

PI1034

NLPN1

PI106

NLPN2

PI107

NLPN3

PI1033

NLPN4

PI108

NLPN5

PI1031

NLPP0

PI1012

NLPP1

PI104

NLPP3

PI1036

NLPP4

PI105

NLPP5

PI1018

NLPQ2

PI1013

NLPQ3

PI1037

NLPQ4

PI1051

NLPQ5

PI1073

NLPQ6

56 Dodatek A Schematy modułoacutew urządzenia

Dodatek B

Wykresy pobranych trajektorii

Dodatek B Wykresy pobranych trajektorii 57

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B1 Trajektoria ucząca z minimalnym zastosowaniem filtru

58 Dodatek B Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

Dodatek A

Schematy modułoacutew urządzenia

Dodatek A Schematy modułoacutew urządzenia 51

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

A

nybu

s-B

ase

SchD

ocD

raw

n B

y

~RES

ET10

3

A0

~WEH

DIP

1_0

104

A1

DIP

1_1

105

A2

DIP

1_2

106

A3

DIP

1_3

107

A4

DIP

1_4

109

A5

DIP

1_5

110

A6

DIP

1_6

111

A7

DIP

1_7

112

A8

~LD

~SS

113

A9

SCLK

115

A10

DO

MIS

O11

6

A11

DIM

OSI

117

A12

ASM

_RX

119

A13

ASM

_TX

120

D7

DIP

2_7

121

D6

DIP

2_6

122

D5

DIP

2_5

123

D4

DIP

2_4

125

D3

DIP

2_3

126

D2

DIP

2_2

127

D1

DIP

2_1

129

MD

013

0D

0D

IP2_

013

1O

M0

132

OM

113

3

OM

213

5~C

S13

6~W

E~W

ELC

T13

7~I

RQ

PA

138

~OE

139

~LED

4AD

1514

1~L

ED4B

D14

142

~LED

3AD

1314

3

~LED

3BD

1214

5LE

D2A

D11

146

LED

2BD

1014

7LE

D1A

D9

148

LED

1BD

814

9

RX

ASM

_RX

151

TXA

SM_T

XO

M3

152

MI1

153

MI0

SY

NC

154

Hos

t Con

nect

or

B Any

bus_

CC

_B40

PK0

18

PK1

19

PK2

20

PK3

21

PC7

22

PC6

23

PC5

24

PC4

25

Dat

aBus

Pin

sU

1E

TM4C

1292

NC

PDTI

3R

PA6

40

PA7

41

PG0

49PG

150

PB3

92

PQ0

5

PQ1

6

PM3

75

PM2

76

PM1

77

PM0

78

PL0

81

PL1

82

PL2

83

PL3

84

PL4

85

PP2

103

Adr

esB

us P

ins

U1F

TM4C

1292

NC

PDTI

3R

D0

D1

D2

D3

D4

D5

D6

D7

D0

D1

D2

D3

D4

D5

D6

D7A

0A

1A

2A

3A

4A

5A

6A

7A

8A

9A

10A

11A

12A

13

A0

A1

A2

A3

A4

A5

A6

A7

A8

A9

A10

A11

A12

A13

PA0

33

PA1

34

PB0

95

U1A

TM4C

1292

NC

PDTI

3R

GN

D

R33

10k

R34

10k

R35

10k

Any

bus

Page

15

Any

bus

uC c

onne

ctio

n

1

Bar

osz

Kac

zor

1

4

PI0103

PI0104

PI0105

PI0106

PI0107

PI0109

PI0110

PI0111

PI0112

PI0113

PI0115

PI0116

PI0117

PI0119

PI0120

PI0121

PI0122

PI0123

PI0125

PI0126

PI0127

PI0129

PI0130

PI0131

PI0132

PI0133

PI0135

PI0136

PI0137

PI0138

PI0139

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

COB

PIR3301 PIR3302 CO

R33

PIR3401 PIR3402 COR34

PIR3501 PIR3502 CO

R35

PIU1033

PIU1034

PIU1095 COU1

A

PIU1018

PIU1019

PIU1020

PIU1021

PIU1022

PIU1023

PIU1024

PIU1025

COU1

E PIU105

PIU106

PIU1040

PIU1041

PIU1049

PIU1050

PIU1075

PIU1076

PIU1077

PIU1078

PIU1081

PIU1082

PIU1083

PIU1084

PIU1085

PIU1092

PIU10103

COU1

F

PI0104

PIU1040

NLA0

PI0105

PIU1041

NLA1

PI0106

PIU1050

NLA2

PI0107

PIU1049

NLA3

PI0109

PIU1075

NLA4

PI0110

PIU1076

NLA5

PI0111

PIU1077

NLA6

PI0112

PIU1078

NLA7

PI0113

PIU1081

NLA8

PI0115

PIU1082

NLA9

PI0116

PIU1083

NLA10

PI0117

PIU1084

NLA11

PI0119

PIU105

NLA12

PI0120

PIU106

NLA13

PI0131

PIU1018

NLD0

PI0129

PIU1019

NLD1

PI0127

PIU1020

NLD2

PI0126

PIU1021

NLD3 PI0125

PIU1022

NLD4

PI0123

PIU1023

NLD5 PI0122

PIU1024

NLD6

PI0121

PIU1025

NLD7

PIR3301 PIR3401

PIR3501

PI0103

PIU1033

PI0130

PI0132

PIR3302

PI0133

PIR3402

PI0135

PIR3502

PI0136

PIU1085

PI0137

PIU10103

PI0138

PIU1095

PI0139

PIU1092

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

PIU1034

52 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

C

onne

ctor

sSch

Doc

Dra

wn

By

PQ2

11

PQ3

27

PQ5

57

PQ6

58

PM7

71PM

672

PM5

73PM

474

PL5

86

PL7

93PL

694

PQ4

102

PP3

104

PP4

105

PP5

106

PN0

107

PN1

108

PN2

109

PN3

110

PN4

111

PN5

112

PP0

118

PP1

119

U1B

TM4C

1292

NC

PDTI

3R

PD0

1

PD1

2

PD2

3

PD3

4

PE3

12PE

213

PE1

14PE

015

PH0

29

PH1

30

PH2

31

PH3

32

PA2

35

PA3

36

PA4

37

PA5

38

PF0

42

PF1

43

PF2

44

PF3

45

PF4

46

PG2

51

PG3

52

PG4

53

PG5

54

PG6

55

PG7

56

PK7

59PK

660

PK5

61PK

462

PB2

91PB

196

PC3_

TDO

_SW

O97

PC2_

TDI

98PC

1_TM

S_SW

DIO

99PC

0_TC

K_S

WC

LK10

0

PJ0

116

PJ1

117

PB5

120

PB4

121

PE4

123

PE5

124

PD4

125

PD5

126

PD6

127

PD7

128

U1G

TM4C

1292

NC

PDTI

3R

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PH0

PH1

PH2

PH3

PJ0

PJ1

PK4

PK5

PK6

PK7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

PC0

PC1

PC2

PC3

3V3

GN

D

RST

92

101

74

83 5

6

J2 Pin

200

mm

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

Con

nect

or s

chem

a

1

Bar

tosz

Kac

zor

2

4

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIJ201

PIJ202

PIJ203

PIJ204

PIJ205

PIJ206

PIJ207

PIJ208

PIJ209

PIJ2010

COJ2

PIU1011

PIU1027

PIU1057

PIU1058

PIU1071

PIU1072

PIU1073

PIU1074

PIU1086

PIU1093

PIU1094

PIU10102

PIU10104

PIU10105

PIU10106

PIU10107

PIU10108

PIU10109

PIU10110

PIU10111

PIU10112

PIU10118

PIU10119

COU1B

PIU101

PIU102

PIU103

PIU104

PIU1012

PIU1013

PIU1014

PIU1015

PIU1029

PIU1030

PIU1031

PIU1032

PIU1035

PIU1036

PIU1037

PIU1038

PIU1042

PIU1043

PIU1044

PIU1045

PIU1046

PIU1051

PIU1052

PIU1053

PIU1054

PIU1055

PIU1056

PIU1059

PIU1060

PIU1061

PIU1062

PIU1091

PIU1096

PIU1097

PIU1098

PIU1099

PIU10100

PIU10116

PIU10117

PIU10120

PIU10121

PIU10123

PIU10124

PIU10125

PIU10126

PIU10127

PIU10128

COU1G

PI1041

PI1080

PIJ201

PI1059

PI1060

PI1061

PI1062

PI1042

PI1043

PI1078

PI1079

PIJ202

PIJ203

PIJ205

PIJ204

PI1063

PIU1035

NLPA2

PI1066

PIU1036

NLPA3

PI1067

PIU1037

NLPA4

PI1058

PIU1038

NLPA5

PI102

PIU1096

NLPB1

PI1045

PIU1091

NLPB2

PI1029

PIU10121

NLPB4

PI1030

PIU10120

NLPB5

PI103

PIJ209

PIU10100

NLPC0

PI1038

PIJ2010

PIU1099

NLPC

1

PI1039

PIJ207

PIU1098

NLPC2

PI1040

PIJ208

PIU1097

NLPC3

PI1019

PIU101

NLPD

0

PI1022

PIU102

NLPD1

PI1021

PIU103

NLPD

2

PI1020

PIU104

NLPD3

PI1026

PIU10125

NLPD

4

PI1025

PIU10126

NLPD5

PI1024

PIU10127

NLPD

6

PI1023

PIU10128

NLPD

7

PI1014

PIU1015

NLPE0

PI1015

PIU1014

NLPE1

PI1016

PIU1013

NLPE2

PI1017

PIU1012

NLPE3

PI1028

PIU10123

NLPE4

PI1027

PIU10124

NLPE5

PI1057

PIU1042

NLPF0

PI1068

PIU1043

NLPF1

PI1056

PIU1044

NLPF2

PI1069

PIU1045

NLPF3

PI1055

PIU1046

NLPF4

PI1054

PIU1051

NLPG2

PI1070

PIU1052

NLPG3

PI1071

PIU1053

NLPG4

PI1053

PIU1054

NLPG5

PI1052

PIU1055

NLPG6

PI1072

PIU1056

NLPG7

PI1010

PIU1029

NLPH0

PI109

PIU1030

NLPH1

PI1065

PIU1031

NLPH2

PI1064

PIU1032

NLPH3

PI1032

PIU10116

NLPJ0

PI1011

PIU10117

NLPJ1

PI1076

PIU1062

NLPK4

PI1049

PIU1061

NLPK5

PI1075

PIU1060

NLPK6

PI1050

PIU1059

NLPK7

PI1077

PIU1086

NLPL5

PI101

PIU1094

NLPL6

PI1044

PIU1093

NLPL7

PI1046

PIU1074

NLPM

4

PI1047

PIU1073

NLPM

5

PI1048

PIU1072

NLPM

6

PI1074

PIU1071

NLPM

7

PI1035

PIU10107

NLPN0

PI1034

PIU10108

NLPN1

PI106

PIU10109

NLPN2

PI107

PIU10110

NLPN3

PI1033

PIU10111

NLPN4

PI108

PIU10112

NLPN5

PI1031

PIU10118

NLPP0

PI1012

PIU10119

NLPP1

PI104

PIU10104

NLPP3

PI1036

PIU10105

NLPP4

PI105

PIU10106

NLPP5

PI1018

PIU1011

NLPQ

2

PI1013

PIU1027

NLPQ3

PI1037

PIU10102

NLPQ

4

PI1051

PIU1057

NLPQ5

PI1073

PIU1058

NLPQ

6

PIJ206

NLRST

Dodatek A Schematy modułoacutew urządzenia 53

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

N

etw

ork

SchD

ocD

raw

n B

y

B_1

PR

XP

3

B_1

CEN

SD

A4

B_1

NR

XN

5

B_2

PSD

P7

B_2

CEN

SC

L8

B_2

NS

DN

9

B_3

PTX

EN11

B_3

CEN

12

B_3

NT

XD

IS13

B_4

PTX

P15

B_4

CEN

~B

USP

16

B_4

NT

XN

17

NW

_LED

4B19

NW

_LED

4A20

NW

_LED

3B21

NW

_LED

3A22

NW

_LED

2B23

NW

_LED

2A24

NW

_LED

1B25

NW

_LED

1A26

A_1

PR

XP

29A

_1C

ENS

DA

30A

_1N

RX

N31

A_2

PSD

P33

A_2

CEN

SC

L34

A_2

NS

DN

35

A_3

PTX

EN37

A_3

CEN

38A

_3N

TX

DIS

39

A_4

PTX

P41

A_4

CEN

~B

USP

42A

_4N

TX

N43

C_T

X45

C_R

X46

C_T

XEN

47~C

_BU

SP48

GA

TE1

49G

ATE

250

Net

wor

k C

onne

ctor

A Any

bus_

CC

_B40

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T1 47

uHB_

1PB_

1CEN

B_1N

B_1P

B_1C

EN

B_1N

B_2P

B_2C

ENB_

2N

B_2P

B_2C

EN

B_2N

16V

100n

F

C1

16V

100n

F

C2

GN

DS

PE

2kV

1nF

C3

R1

75R

275

R5

1M2k

V1n

F

C4 PE

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T2 47

uH

A_1

P

A_1

CEN

A_1

N

A_2

P

A_2

CEN

A_2

N

16V

100n

F

C5

16V

100n

F

C6

GN

DS

PE

2kV

1nF

C7

R6

75R

775

A_1

PA

_1C

ENA

_1N

A_2

PA

_2C

ENA

_2N

DL1

R18

220

R17

220

R16

220

R15

220

R14

220

R13

220

R12

220

R11

220

DL2

DL3

DL4

GN

DS

3210 4

J4 Clic

k_M

ate

5x1

3210 4

J5 Clic

k_M

ate

5x1

Net

wor

k co

nnec

tor

1

3

4B

arto

sz k

aczo

r

PI03

PI04

PI05

PI07

PI08

PI09

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PI020

PI021

PI022

PI023

PI024

PI025

PI026

PI029

PI030

PI031

PI033

PI034

PI035

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

COA

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIC301 PIC302

COC3

PIC401 PIC402

COC4

PIC501 PIC502

COC5

PIC601 PIC602

COC6

PIC701 PIC702

COC7

PIDL101 PIDL102

PIDL103 PIDL104 CODL1

PIDL201 PIDL202

PIDL203 PIDL204 CODL2

PIDL301 PIDL302

PIDL303 PIDL304 CODL3

PIDL401 PIDL402

PIDL403 PIDL404 CODL4

PIJ400

PIJ401

PIJ402

PIJ403

PIJ404

COJ4 PIJ500

PIJ501

PIJ502

PIJ503

PIJ504

COJ5

PIR101 PIR102 COR

1 PIR201 PIR202 COR

2

PIR501 PIR502 COR

5

PIR601 PIR602 COR

6 PIR701 PIR702 COR

7

PIR1101 PIR1102 COR1

1

PIR1201 PIR1202 COR12

PIR1301 PIR1302 COR13

PIR1401 PIR1402 COR14

PIR1501 PIR1502 COR15

PIR1601 PIR1602 COR16

PIR1701 PIR1702 COR17 PIR1801 PIR1802

COR1

8

PIT101

PIT102

PIT103

PIT104

PIT105

PIT106

PIT107

PIT108

PIT109

PIT1010

PIT1011

PIT1012

COT1

PIT201

PIT202

PIT203

PIT204

PIT205

PIT206

PIT207

PIT208

PIT209

PIT2010

PIT2011

PIT2012

COT2

PI030

PIC501

PIT203

NLA01CEN

PI031

PIT202

NLA0

1N

PI029

PIT201

NLA0

1P

PI034

PIC601

PIT204

NLA02CEN

PI035

PIT206

NLA0

2N

PI033

PIT205

NLA0

2P

PI04

PIC101

PIT103

NLB01CEN

PI05

PIT102

NLB0

1N

PI03

PIT101

NLB0

1P

PI08

PIC201

PIT104

NLB02CEN

PI09

PIT106

NLB0

2N

PI07

PIT105

NLB0

2P

PIC102 PIC202

PIC502 PIC602

PIDL101 PIDL103

PIDL201 PIDL203

PIDL301 PIDL303

PIDL401 PIDL403

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PIR1102

PI020

PIR1202

PI021

PIR1302

PI022

PIR1402

PI023

PIR1502

PI024

PIR1602 PI025

PIR1702 PI026

PIR1802

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

PIC301 PIR101

PIR201

PIC401

PIJ404 PIJ504

PIR502

PIC701 PIR601

PIR701

PIDL102

PIR1701

PIDL104

PIR1801

PIDL202

PIR1501

PIDL204

PIR1601

PIDL302

PIR1301

PIDL304

PIR1401

PIDL402

PIR1101

PIDL404

PIR1201

PIJ400

PIT1012

PIJ401

PIT1011

PIJ402

PIT108

PIJ403

PIT107

PIJ500

PIT2012

PIJ501

PIT2011

PIJ502

PIT208

PIJ503

PIT207 PIR102

PIT1010

PIR202

PIT109

PIR602

PIT2010

PIR702

PIT209

PIC302

PIC402

PIC702

PIR501

54 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

P

ower

Supp

lyS

chD

ocD

raw

n B

y

5V GN

D

GN

D

3V3

GN

DS

GN

DS

3V3 R

pu10

k 16V

10nF

C21

GN

DS

Rs

470

TAC

TD60

B1

GN

DS

16V

10uF

Cin

R20

510k

Rpg

499k

R21

162k

16V

22uF

Cou

t

16V

4uF

Cld

o

3V3

GN

DS

GN

DS

16V

12pF

Cx2

16V

12pF

Cx1

GN

DS

3V3

1

GN

D2

GN

D6

GN

D10

GN

D14

GN

D18

3V3

27

GN

D28

GN

D32

GN

D36

GN

D40

GN

D44

3V3

51

GN

D52

Pow

er p

ins

3V3

101

GN

D10

2

GN

D10

8

GN

D11

4

GN

D11

8

GN

D12

4

GN

D12

8

GN

D13

4G

ND

140

GN

D14

4G

ND

150

3V3

155

GN

D15

6

C Any

bus_

CC

_B40

3V3

GN

DS

GN

DG

ND

S

WA

KE_

N64

HIB

_N65

XO

SC0

66

XO

SC1

67

RST

_N70

OSC

088

OSC

189

U1C

TM4C

1292

NC

PDTI

3R

VD

D7

VD

DA

8

VR

EFA

+9

GN

DA

10

VD

D16

GN

D17

VD

D26

VD

D28

VD

D39

VD

D47

GN

D48

VD

D63

VB

AT

68

VD

D69

VD

D79

GN

D80

VD

DC

87

VD

D90

VD

D10

1

VD

D11

3

GN

D11

4

VD

DC

115

VD

D12

2

U1D

TM4C

1292

NC

PDTI

3R

RST

GN

DX

18M

Hz

GN

D

EN1

PG2

VIN

3

VIN

4G

ND

5G

ND

6FB

7V

OU

T8

TPS8

2085

GN

D9

U2

TPS8

2085

GN

D

R66

500k

2kV

1nF

C55 PE

GN

DR

5550

0k2k

V1n

F

C44 PE

GN

D

R77

500k

2kV

1nF

C66 PE

GN

D

16V

100n

F

CV

116

V10

0nF

CV

216

V10

0nF

CV

316

V10

0nF

CV

4

3V3

16V

100n

F

CV

516

V10

0nF

CV

616

V10

0nF

CV

716

V10

0nF

CV

816

V10

0nF

CV

916

V10

0nF

CV

1016

V10

0nF

CV

1116

V10

0nF

CV

1216

V10

0nF

CV

13

GN

D

16V

1uF

CV

ref1

16V

10nF

CV

ref2

GN

DG

ND Po

wer

supp

ly

1

4

4 B

arto

sz K

aczo

r

PI01

PI02

PI06

PI010

PI014

PI018

PI027

PI028

PI032

PI036

PI040

PI044

PI051

PI052

PI0101

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0155

PI0156

COC

PIB101

PIB102

PIB103 PI

B104 COB1

PIC2101 PIC2102

COC2

1

PIC4401 PIC4402

COC44

PIC5501 PIC5502

COC55

PIC6601 PIC6602

COC66

PICin01 PICin02

COCin

PICldo01 PICldo02

COCl

do

PICout01 PICout02

COCo

ut

PICV101 PICV102

COCV1

PICV201 PICV202

COCV

2 PICV301 PICV302

COCV

3 PICV401 PICV402

COCV

4 PICV501 PICV502

COCV5

PICV601 PICV602

COCV

6 PICV701 PICV702

COCV

7 PICV801 PICV802

COCV

8 PICV901 PICV902

COCV

9 PICV1001 PICV1002

COCV10

PICV1101 PICV1102

COCV11

PICV1201 PICV1202

COCV12

PICV1301 PICV1302

COCV13

PICVref101 PICVref102

COCV

ref1

PICVref201 PICVref202

COCVref2

PICx101 PICx102

COCx1

PICx201 PICx202

COCx2

PIR2001 PIR2002 COR20

PIR2

101

PIR2

102 COR

21

PIR5501 PIR5502 COR55

PIR6601 PIR6602 COR

66

PIR7701 PIR7702 CO

R77

PIRpg01 PIRpg02 CO

Rpg

PIRpu01 PIRpu02 CORpu

PIRs01

PIRs

02

CORs

PIU1064

PIU1065

PIU1066

PIU1067

PIU1070

PIU1088

PIU1089

COU1

C

PIU107

PIU108

PIU109

PIU1010

PIU1016

PIU1017

PIU1026

PIU1028

PIU1039

PIU1047

PIU1048

PIU1063

PIU1068

PIU1069

PIU1079

PIU1080

PIU1087

PIU1090

PIU10101

PIU10113

PIU10114

PIU10115

PIU10122

COU1

D

PIU201

PIU202

PIU203

PIU204

PIU205

PIU206

PIU207

PIU208

PIU209

COU2

PIX101 PIX102 PIX10GND

PIX10GND2 COX

1

PI01

PI027

PI051

PI0101

PI0155

PICout01

PICV101 PICV201

PICV301 PICV401

PICV501 PICV601

PICV701 PICV801

PICV901 PICV1001

PICV1101 PICV1201

PICV1301

PICVref101 PICVref201

PIR2002 PIRpg02

PIRpu02

PIU107

PIU108

PIU109

PIU1016

PIU1026

PIU1028

PIU1039

PIU1047

PIU1063

PIU1068

PIU1069

PIU1079

PIU1090

PIU10101

PIU10113

PIU10122

PIU208

PICin01

PIU201

PIU203

PIU204

PI02

PI06

PI010

PI014

PI018

PI028

PI032

PI036

PI040

PI044

PI052

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0156

PIB101

PIB102

PIC2102

PIC4401 PIC5501

PIC6601

PICin02

PICldo02

PICout02

PICV102 PICV202

PICV302 PICV402

PICV502 PICV602

PICV702 PICV802

PICV902 PICV1002

PICV1102 PICV1202

PICV1302

PICVref102 PICVref202

PICx102 PICx202

PIR2

101

PIR5502 PIR6602

PIR7702

PIU1010

PIU1017

PIU1048

PIU1064

PIU1080

PIU1088

PIU10114

PIU205

PIU206

PIU209

PIX10GND

PIX10GND2

PIB103 PI

B104 PIRs01

PICldo01

PIU1087

PIU10115

PICx101

PIU1067

PIX101

PICx201

PIU1066

PIX102

PIR2001 PIR2

102

PIU207

PIRpg01

PIU202

PIU1065

PIU1089

PIC4402 PIC5502

PIC6602

PIR5501 PIR6601

PIR7701

PIC2101 PIRpu01 PI

Rs02

PIU1070

NLRST

Dodatek A Schematy modułoacutew urządzenia 55

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-15

Shee

t o

fFi

le

CU

sers

IM

UO

ldS

chD

ocD

raw

n B

y

8642 16141210 18 20 22 24

17 19 21 239 11 13 157531J5 H

eade

r 12x

2

5V

GN

D

5V

16V

100n

F

C1

16V

100n

F

C2

GN

DRS

TCSD

OU

TD

IO4

DIO

3SC

LKD

IND

IO1

DIO

2

PA2

PA3

PA4

PA5

PB1

PB2

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

1 23 4

J2

5V

GN

D

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIJ201

PIJ202

PIJ203

PIJ204

COJ2

PIJ501

PIJ502

PIJ503

PIJ504

PIJ505

PIJ506

PIJ507

PIJ508

PIJ509

PIJ5

010

PIJ5011

PIJ5012

PIJ5013

PIJ5014

PIJ5

015

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

COJ5

PI1041

PI1080

PI1059

PI1060

PI1061

PI1062

PIC101 PIC201

PIJ201

PIJ5

010

PIJ5011

PIJ5012

PI1066

PIJ506

NLCS

NLPA3

PI1067

PIJ505

NLDIN

NLPA4

PIJ507

NLDI

O1 PIJ509

NLDI

O2 PIJ501

NLDI

O3

PI1045

PIJ502

NLDI

O4

NLPB2

PI1058

PIJ504

NLDOUT

NLPA5

PI1042

PI1043

PI1078

PI1079

PIC102 PIC202

PIJ204

PIJ5013

PIJ5014

PIJ5

015

PIJ202

PIJ203

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

PI1063

PIJ503

NLPA2

NLSCLK

PI102

PIJ508

NLPB1

NLRS

T

PI1029

NLPB4

PI1030

NLPB5

PI103

NLPC0

PI1038

NLPC1

PI1039

NLPC2

PI1040

NLPC3

PI1019

NLPD0

PI1022

NLPD1

PI1021

NLPD2

PI1020

NLPD3

PI1026

NLPD4

PI1025

NLPD5

PI1024

NLPD6

PI1023

NLPD7

PI1014

NLPE0

PI1015

NLPE

1

PI1016

NLPE2

PI1017

NLPE3

PI1028

NLPE4

PI1027

NLPE

5

PI1057

NLPF

0

PI1068

NLPF

1

PI1056

NLPF2

PI1069

NLPF3

PI1055

NLPF

4

PI1054

NLPG2

PI1070

NLPG3

PI1071

NLPG4

PI1053

NLPG5

PI1052

NLPG6

PI1072

NLPG7

PI1010

NLPH0

PI109

NLPH1

PI1065

NLPH2

PI1064

NLPH3

PI1032

NLPJ0

PI1011

NLPJ1

PI1076

NLPK4

PI1049

NLPK5

PI1075

NLPK6

PI1050

NLPK7

PI1077

NLPL5

PI101

NLPL6

PI1044

NLPL7

PI1046

NLPM

4

PI1047

NLPM

5

PI1048

NLPM

6

PI1074

NLPM

7

PI1035

NLPN0

PI1034

NLPN1

PI106

NLPN2

PI107

NLPN3

PI1033

NLPN4

PI108

NLPN5

PI1031

NLPP0

PI1012

NLPP1

PI104

NLPP3

PI1036

NLPP4

PI105

NLPP5

PI1018

NLPQ2

PI1013

NLPQ3

PI1037

NLPQ4

PI1051

NLPQ5

PI1073

NLPQ6

56 Dodatek A Schematy modułoacutew urządzenia

Dodatek B

Wykresy pobranych trajektorii

Dodatek B Wykresy pobranych trajektorii 57

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B1 Trajektoria ucząca z minimalnym zastosowaniem filtru

58 Dodatek B Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

A

nybu

s-B

ase

SchD

ocD

raw

n B

y

~RES

ET10

3

A0

~WEH

DIP

1_0

104

A1

DIP

1_1

105

A2

DIP

1_2

106

A3

DIP

1_3

107

A4

DIP

1_4

109

A5

DIP

1_5

110

A6

DIP

1_6

111

A7

DIP

1_7

112

A8

~LD

~SS

113

A9

SCLK

115

A10

DO

MIS

O11

6

A11

DIM

OSI

117

A12

ASM

_RX

119

A13

ASM

_TX

120

D7

DIP

2_7

121

D6

DIP

2_6

122

D5

DIP

2_5

123

D4

DIP

2_4

125

D3

DIP

2_3

126

D2

DIP

2_2

127

D1

DIP

2_1

129

MD

013

0D

0D

IP2_

013

1O

M0

132

OM

113

3

OM

213

5~C

S13

6~W

E~W

ELC

T13

7~I

RQ

PA

138

~OE

139

~LED

4AD

1514

1~L

ED4B

D14

142

~LED

3AD

1314

3

~LED

3BD

1214

5LE

D2A

D11

146

LED

2BD

1014

7LE

D1A

D9

148

LED

1BD

814

9

RX

ASM

_RX

151

TXA

SM_T

XO

M3

152

MI1

153

MI0

SY

NC

154

Hos

t Con

nect

or

B Any

bus_

CC

_B40

PK0

18

PK1

19

PK2

20

PK3

21

PC7

22

PC6

23

PC5

24

PC4

25

Dat

aBus

Pin

sU

1E

TM4C

1292

NC

PDTI

3R

PA6

40

PA7

41

PG0

49PG

150

PB3

92

PQ0

5

PQ1

6

PM3

75

PM2

76

PM1

77

PM0

78

PL0

81

PL1

82

PL2

83

PL3

84

PL4

85

PP2

103

Adr

esB

us P

ins

U1F

TM4C

1292

NC

PDTI

3R

D0

D1

D2

D3

D4

D5

D6

D7

D0

D1

D2

D3

D4

D5

D6

D7A

0A

1A

2A

3A

4A

5A

6A

7A

8A

9A

10A

11A

12A

13

A0

A1

A2

A3

A4

A5

A6

A7

A8

A9

A10

A11

A12

A13

PA0

33

PA1

34

PB0

95

U1A

TM4C

1292

NC

PDTI

3R

GN

D

R33

10k

R34

10k

R35

10k

Any

bus

Page

15

Any

bus

uC c

onne

ctio

n

1

Bar

osz

Kac

zor

1

4

PI0103

PI0104

PI0105

PI0106

PI0107

PI0109

PI0110

PI0111

PI0112

PI0113

PI0115

PI0116

PI0117

PI0119

PI0120

PI0121

PI0122

PI0123

PI0125

PI0126

PI0127

PI0129

PI0130

PI0131

PI0132

PI0133

PI0135

PI0136

PI0137

PI0138

PI0139

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

COB

PIR3301 PIR3302 CO

R33

PIR3401 PIR3402 COR34

PIR3501 PIR3502 CO

R35

PIU1033

PIU1034

PIU1095 COU1

A

PIU1018

PIU1019

PIU1020

PIU1021

PIU1022

PIU1023

PIU1024

PIU1025

COU1

E PIU105

PIU106

PIU1040

PIU1041

PIU1049

PIU1050

PIU1075

PIU1076

PIU1077

PIU1078

PIU1081

PIU1082

PIU1083

PIU1084

PIU1085

PIU1092

PIU10103

COU1

F

PI0104

PIU1040

NLA0

PI0105

PIU1041

NLA1

PI0106

PIU1050

NLA2

PI0107

PIU1049

NLA3

PI0109

PIU1075

NLA4

PI0110

PIU1076

NLA5

PI0111

PIU1077

NLA6

PI0112

PIU1078

NLA7

PI0113

PIU1081

NLA8

PI0115

PIU1082

NLA9

PI0116

PIU1083

NLA10

PI0117

PIU1084

NLA11

PI0119

PIU105

NLA12

PI0120

PIU106

NLA13

PI0131

PIU1018

NLD0

PI0129

PIU1019

NLD1

PI0127

PIU1020

NLD2

PI0126

PIU1021

NLD3 PI0125

PIU1022

NLD4

PI0123

PIU1023

NLD5 PI0122

PIU1024

NLD6

PI0121

PIU1025

NLD7

PIR3301 PIR3401

PIR3501

PI0103

PIU1033

PI0130

PI0132

PIR3302

PI0133

PIR3402

PI0135

PIR3502

PI0136

PIU1085

PI0137

PIU10103

PI0138

PIU1095

PI0139

PIU1092

PI0141

PI0142

PI0143

PI0145

PI0146

PI0147

PI0148

PI0149

PI0151

PI0152

PI0153

PI0154

PIU1034

52 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

C

onne

ctor

sSch

Doc

Dra

wn

By

PQ2

11

PQ3

27

PQ5

57

PQ6

58

PM7

71PM

672

PM5

73PM

474

PL5

86

PL7

93PL

694

PQ4

102

PP3

104

PP4

105

PP5

106

PN0

107

PN1

108

PN2

109

PN3

110

PN4

111

PN5

112

PP0

118

PP1

119

U1B

TM4C

1292

NC

PDTI

3R

PD0

1

PD1

2

PD2

3

PD3

4

PE3

12PE

213

PE1

14PE

015

PH0

29

PH1

30

PH2

31

PH3

32

PA2

35

PA3

36

PA4

37

PA5

38

PF0

42

PF1

43

PF2

44

PF3

45

PF4

46

PG2

51

PG3

52

PG4

53

PG5

54

PG6

55

PG7

56

PK7

59PK

660

PK5

61PK

462

PB2

91PB

196

PC3_

TDO

_SW

O97

PC2_

TDI

98PC

1_TM

S_SW

DIO

99PC

0_TC

K_S

WC

LK10

0

PJ0

116

PJ1

117

PB5

120

PB4

121

PE4

123

PE5

124

PD4

125

PD5

126

PD6

127

PD7

128

U1G

TM4C

1292

NC

PDTI

3R

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PH0

PH1

PH2

PH3

PJ0

PJ1

PK4

PK5

PK6

PK7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

PC0

PC1

PC2

PC3

3V3

GN

D

RST

92

101

74

83 5

6

J2 Pin

200

mm

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

Con

nect

or s

chem

a

1

Bar

tosz

Kac

zor

2

4

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIJ201

PIJ202

PIJ203

PIJ204

PIJ205

PIJ206

PIJ207

PIJ208

PIJ209

PIJ2010

COJ2

PIU1011

PIU1027

PIU1057

PIU1058

PIU1071

PIU1072

PIU1073

PIU1074

PIU1086

PIU1093

PIU1094

PIU10102

PIU10104

PIU10105

PIU10106

PIU10107

PIU10108

PIU10109

PIU10110

PIU10111

PIU10112

PIU10118

PIU10119

COU1B

PIU101

PIU102

PIU103

PIU104

PIU1012

PIU1013

PIU1014

PIU1015

PIU1029

PIU1030

PIU1031

PIU1032

PIU1035

PIU1036

PIU1037

PIU1038

PIU1042

PIU1043

PIU1044

PIU1045

PIU1046

PIU1051

PIU1052

PIU1053

PIU1054

PIU1055

PIU1056

PIU1059

PIU1060

PIU1061

PIU1062

PIU1091

PIU1096

PIU1097

PIU1098

PIU1099

PIU10100

PIU10116

PIU10117

PIU10120

PIU10121

PIU10123

PIU10124

PIU10125

PIU10126

PIU10127

PIU10128

COU1G

PI1041

PI1080

PIJ201

PI1059

PI1060

PI1061

PI1062

PI1042

PI1043

PI1078

PI1079

PIJ202

PIJ203

PIJ205

PIJ204

PI1063

PIU1035

NLPA2

PI1066

PIU1036

NLPA3

PI1067

PIU1037

NLPA4

PI1058

PIU1038

NLPA5

PI102

PIU1096

NLPB1

PI1045

PIU1091

NLPB2

PI1029

PIU10121

NLPB4

PI1030

PIU10120

NLPB5

PI103

PIJ209

PIU10100

NLPC0

PI1038

PIJ2010

PIU1099

NLPC

1

PI1039

PIJ207

PIU1098

NLPC2

PI1040

PIJ208

PIU1097

NLPC3

PI1019

PIU101

NLPD

0

PI1022

PIU102

NLPD1

PI1021

PIU103

NLPD

2

PI1020

PIU104

NLPD3

PI1026

PIU10125

NLPD

4

PI1025

PIU10126

NLPD5

PI1024

PIU10127

NLPD

6

PI1023

PIU10128

NLPD

7

PI1014

PIU1015

NLPE0

PI1015

PIU1014

NLPE1

PI1016

PIU1013

NLPE2

PI1017

PIU1012

NLPE3

PI1028

PIU10123

NLPE4

PI1027

PIU10124

NLPE5

PI1057

PIU1042

NLPF0

PI1068

PIU1043

NLPF1

PI1056

PIU1044

NLPF2

PI1069

PIU1045

NLPF3

PI1055

PIU1046

NLPF4

PI1054

PIU1051

NLPG2

PI1070

PIU1052

NLPG3

PI1071

PIU1053

NLPG4

PI1053

PIU1054

NLPG5

PI1052

PIU1055

NLPG6

PI1072

PIU1056

NLPG7

PI1010

PIU1029

NLPH0

PI109

PIU1030

NLPH1

PI1065

PIU1031

NLPH2

PI1064

PIU1032

NLPH3

PI1032

PIU10116

NLPJ0

PI1011

PIU10117

NLPJ1

PI1076

PIU1062

NLPK4

PI1049

PIU1061

NLPK5

PI1075

PIU1060

NLPK6

PI1050

PIU1059

NLPK7

PI1077

PIU1086

NLPL5

PI101

PIU1094

NLPL6

PI1044

PIU1093

NLPL7

PI1046

PIU1074

NLPM

4

PI1047

PIU1073

NLPM

5

PI1048

PIU1072

NLPM

6

PI1074

PIU1071

NLPM

7

PI1035

PIU10107

NLPN0

PI1034

PIU10108

NLPN1

PI106

PIU10109

NLPN2

PI107

PIU10110

NLPN3

PI1033

PIU10111

NLPN4

PI108

PIU10112

NLPN5

PI1031

PIU10118

NLPP0

PI1012

PIU10119

NLPP1

PI104

PIU10104

NLPP3

PI1036

PIU10105

NLPP4

PI105

PIU10106

NLPP5

PI1018

PIU1011

NLPQ

2

PI1013

PIU1027

NLPQ3

PI1037

PIU10102

NLPQ

4

PI1051

PIU1057

NLPQ5

PI1073

PIU1058

NLPQ

6

PIJ206

NLRST

Dodatek A Schematy modułoacutew urządzenia 53

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

N

etw

ork

SchD

ocD

raw

n B

y

B_1

PR

XP

3

B_1

CEN

SD

A4

B_1

NR

XN

5

B_2

PSD

P7

B_2

CEN

SC

L8

B_2

NS

DN

9

B_3

PTX

EN11

B_3

CEN

12

B_3

NT

XD

IS13

B_4

PTX

P15

B_4

CEN

~B

USP

16

B_4

NT

XN

17

NW

_LED

4B19

NW

_LED

4A20

NW

_LED

3B21

NW

_LED

3A22

NW

_LED

2B23

NW

_LED

2A24

NW

_LED

1B25

NW

_LED

1A26

A_1

PR

XP

29A

_1C

ENS

DA

30A

_1N

RX

N31

A_2

PSD

P33

A_2

CEN

SC

L34

A_2

NS

DN

35

A_3

PTX

EN37

A_3

CEN

38A

_3N

TX

DIS

39

A_4

PTX

P41

A_4

CEN

~B

USP

42A

_4N

TX

N43

C_T

X45

C_R

X46

C_T

XEN

47~C

_BU

SP48

GA

TE1

49G

ATE

250

Net

wor

k C

onne

ctor

A Any

bus_

CC

_B40

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T1 47

uHB_

1PB_

1CEN

B_1N

B_1P

B_1C

EN

B_1N

B_2P

B_2C

ENB_

2N

B_2P

B_2C

EN

B_2N

16V

100n

F

C1

16V

100n

F

C2

GN

DS

PE

2kV

1nF

C3

R1

75R

275

R5

1M2k

V1n

F

C4 PE

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T2 47

uH

A_1

P

A_1

CEN

A_1

N

A_2

P

A_2

CEN

A_2

N

16V

100n

F

C5

16V

100n

F

C6

GN

DS

PE

2kV

1nF

C7

R6

75R

775

A_1

PA

_1C

ENA

_1N

A_2

PA

_2C

ENA

_2N

DL1

R18

220

R17

220

R16

220

R15

220

R14

220

R13

220

R12

220

R11

220

DL2

DL3

DL4

GN

DS

3210 4

J4 Clic

k_M

ate

5x1

3210 4

J5 Clic

k_M

ate

5x1

Net

wor

k co

nnec

tor

1

3

4B

arto

sz k

aczo

r

PI03

PI04

PI05

PI07

PI08

PI09

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PI020

PI021

PI022

PI023

PI024

PI025

PI026

PI029

PI030

PI031

PI033

PI034

PI035

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

COA

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIC301 PIC302

COC3

PIC401 PIC402

COC4

PIC501 PIC502

COC5

PIC601 PIC602

COC6

PIC701 PIC702

COC7

PIDL101 PIDL102

PIDL103 PIDL104 CODL1

PIDL201 PIDL202

PIDL203 PIDL204 CODL2

PIDL301 PIDL302

PIDL303 PIDL304 CODL3

PIDL401 PIDL402

PIDL403 PIDL404 CODL4

PIJ400

PIJ401

PIJ402

PIJ403

PIJ404

COJ4 PIJ500

PIJ501

PIJ502

PIJ503

PIJ504

COJ5

PIR101 PIR102 COR

1 PIR201 PIR202 COR

2

PIR501 PIR502 COR

5

PIR601 PIR602 COR

6 PIR701 PIR702 COR

7

PIR1101 PIR1102 COR1

1

PIR1201 PIR1202 COR12

PIR1301 PIR1302 COR13

PIR1401 PIR1402 COR14

PIR1501 PIR1502 COR15

PIR1601 PIR1602 COR16

PIR1701 PIR1702 COR17 PIR1801 PIR1802

COR1

8

PIT101

PIT102

PIT103

PIT104

PIT105

PIT106

PIT107

PIT108

PIT109

PIT1010

PIT1011

PIT1012

COT1

PIT201

PIT202

PIT203

PIT204

PIT205

PIT206

PIT207

PIT208

PIT209

PIT2010

PIT2011

PIT2012

COT2

PI030

PIC501

PIT203

NLA01CEN

PI031

PIT202

NLA0

1N

PI029

PIT201

NLA0

1P

PI034

PIC601

PIT204

NLA02CEN

PI035

PIT206

NLA0

2N

PI033

PIT205

NLA0

2P

PI04

PIC101

PIT103

NLB01CEN

PI05

PIT102

NLB0

1N

PI03

PIT101

NLB0

1P

PI08

PIC201

PIT104

NLB02CEN

PI09

PIT106

NLB0

2N

PI07

PIT105

NLB0

2P

PIC102 PIC202

PIC502 PIC602

PIDL101 PIDL103

PIDL201 PIDL203

PIDL301 PIDL303

PIDL401 PIDL403

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PIR1102

PI020

PIR1202

PI021

PIR1302

PI022

PIR1402

PI023

PIR1502

PI024

PIR1602 PI025

PIR1702 PI026

PIR1802

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

PIC301 PIR101

PIR201

PIC401

PIJ404 PIJ504

PIR502

PIC701 PIR601

PIR701

PIDL102

PIR1701

PIDL104

PIR1801

PIDL202

PIR1501

PIDL204

PIR1601

PIDL302

PIR1301

PIDL304

PIR1401

PIDL402

PIR1101

PIDL404

PIR1201

PIJ400

PIT1012

PIJ401

PIT1011

PIJ402

PIT108

PIJ403

PIT107

PIJ500

PIT2012

PIJ501

PIT2011

PIJ502

PIT208

PIJ503

PIT207 PIR102

PIT1010

PIR202

PIT109

PIR602

PIT2010

PIR702

PIT209

PIC302

PIC402

PIC702

PIR501

54 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

P

ower

Supp

lyS

chD

ocD

raw

n B

y

5V GN

D

GN

D

3V3

GN

DS

GN

DS

3V3 R

pu10

k 16V

10nF

C21

GN

DS

Rs

470

TAC

TD60

B1

GN

DS

16V

10uF

Cin

R20

510k

Rpg

499k

R21

162k

16V

22uF

Cou

t

16V

4uF

Cld

o

3V3

GN

DS

GN

DS

16V

12pF

Cx2

16V

12pF

Cx1

GN

DS

3V3

1

GN

D2

GN

D6

GN

D10

GN

D14

GN

D18

3V3

27

GN

D28

GN

D32

GN

D36

GN

D40

GN

D44

3V3

51

GN

D52

Pow

er p

ins

3V3

101

GN

D10

2

GN

D10

8

GN

D11

4

GN

D11

8

GN

D12

4

GN

D12

8

GN

D13

4G

ND

140

GN

D14

4G

ND

150

3V3

155

GN

D15

6

C Any

bus_

CC

_B40

3V3

GN

DS

GN

DG

ND

S

WA

KE_

N64

HIB

_N65

XO

SC0

66

XO

SC1

67

RST

_N70

OSC

088

OSC

189

U1C

TM4C

1292

NC

PDTI

3R

VD

D7

VD

DA

8

VR

EFA

+9

GN

DA

10

VD

D16

GN

D17

VD

D26

VD

D28

VD

D39

VD

D47

GN

D48

VD

D63

VB

AT

68

VD

D69

VD

D79

GN

D80

VD

DC

87

VD

D90

VD

D10

1

VD

D11

3

GN

D11

4

VD

DC

115

VD

D12

2

U1D

TM4C

1292

NC

PDTI

3R

RST

GN

DX

18M

Hz

GN

D

EN1

PG2

VIN

3

VIN

4G

ND

5G

ND

6FB

7V

OU

T8

TPS8

2085

GN

D9

U2

TPS8

2085

GN

D

R66

500k

2kV

1nF

C55 PE

GN

DR

5550

0k2k

V1n

F

C44 PE

GN

D

R77

500k

2kV

1nF

C66 PE

GN

D

16V

100n

F

CV

116

V10

0nF

CV

216

V10

0nF

CV

316

V10

0nF

CV

4

3V3

16V

100n

F

CV

516

V10

0nF

CV

616

V10

0nF

CV

716

V10

0nF

CV

816

V10

0nF

CV

916

V10

0nF

CV

1016

V10

0nF

CV

1116

V10

0nF

CV

1216

V10

0nF

CV

13

GN

D

16V

1uF

CV

ref1

16V

10nF

CV

ref2

GN

DG

ND Po

wer

supp

ly

1

4

4 B

arto

sz K

aczo

r

PI01

PI02

PI06

PI010

PI014

PI018

PI027

PI028

PI032

PI036

PI040

PI044

PI051

PI052

PI0101

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0155

PI0156

COC

PIB101

PIB102

PIB103 PI

B104 COB1

PIC2101 PIC2102

COC2

1

PIC4401 PIC4402

COC44

PIC5501 PIC5502

COC55

PIC6601 PIC6602

COC66

PICin01 PICin02

COCin

PICldo01 PICldo02

COCl

do

PICout01 PICout02

COCo

ut

PICV101 PICV102

COCV1

PICV201 PICV202

COCV

2 PICV301 PICV302

COCV

3 PICV401 PICV402

COCV

4 PICV501 PICV502

COCV5

PICV601 PICV602

COCV

6 PICV701 PICV702

COCV

7 PICV801 PICV802

COCV

8 PICV901 PICV902

COCV

9 PICV1001 PICV1002

COCV10

PICV1101 PICV1102

COCV11

PICV1201 PICV1202

COCV12

PICV1301 PICV1302

COCV13

PICVref101 PICVref102

COCV

ref1

PICVref201 PICVref202

COCVref2

PICx101 PICx102

COCx1

PICx201 PICx202

COCx2

PIR2001 PIR2002 COR20

PIR2

101

PIR2

102 COR

21

PIR5501 PIR5502 COR55

PIR6601 PIR6602 COR

66

PIR7701 PIR7702 CO

R77

PIRpg01 PIRpg02 CO

Rpg

PIRpu01 PIRpu02 CORpu

PIRs01

PIRs

02

CORs

PIU1064

PIU1065

PIU1066

PIU1067

PIU1070

PIU1088

PIU1089

COU1

C

PIU107

PIU108

PIU109

PIU1010

PIU1016

PIU1017

PIU1026

PIU1028

PIU1039

PIU1047

PIU1048

PIU1063

PIU1068

PIU1069

PIU1079

PIU1080

PIU1087

PIU1090

PIU10101

PIU10113

PIU10114

PIU10115

PIU10122

COU1

D

PIU201

PIU202

PIU203

PIU204

PIU205

PIU206

PIU207

PIU208

PIU209

COU2

PIX101 PIX102 PIX10GND

PIX10GND2 COX

1

PI01

PI027

PI051

PI0101

PI0155

PICout01

PICV101 PICV201

PICV301 PICV401

PICV501 PICV601

PICV701 PICV801

PICV901 PICV1001

PICV1101 PICV1201

PICV1301

PICVref101 PICVref201

PIR2002 PIRpg02

PIRpu02

PIU107

PIU108

PIU109

PIU1016

PIU1026

PIU1028

PIU1039

PIU1047

PIU1063

PIU1068

PIU1069

PIU1079

PIU1090

PIU10101

PIU10113

PIU10122

PIU208

PICin01

PIU201

PIU203

PIU204

PI02

PI06

PI010

PI014

PI018

PI028

PI032

PI036

PI040

PI044

PI052

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0156

PIB101

PIB102

PIC2102

PIC4401 PIC5501

PIC6601

PICin02

PICldo02

PICout02

PICV102 PICV202

PICV302 PICV402

PICV502 PICV602

PICV702 PICV802

PICV902 PICV1002

PICV1102 PICV1202

PICV1302

PICVref102 PICVref202

PICx102 PICx202

PIR2

101

PIR5502 PIR6602

PIR7702

PIU1010

PIU1017

PIU1048

PIU1064

PIU1080

PIU1088

PIU10114

PIU205

PIU206

PIU209

PIX10GND

PIX10GND2

PIB103 PI

B104 PIRs01

PICldo01

PIU1087

PIU10115

PICx101

PIU1067

PIX101

PICx201

PIU1066

PIX102

PIR2001 PIR2

102

PIU207

PIRpg01

PIU202

PIU1065

PIU1089

PIC4402 PIC5502

PIC6602

PIR5501 PIR6601

PIR7701

PIC2101 PIRpu01 PI

Rs02

PIU1070

NLRST

Dodatek A Schematy modułoacutew urządzenia 55

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-15

Shee

t o

fFi

le

CU

sers

IM

UO

ldS

chD

ocD

raw

n B

y

8642 16141210 18 20 22 24

17 19 21 239 11 13 157531J5 H

eade

r 12x

2

5V

GN

D

5V

16V

100n

F

C1

16V

100n

F

C2

GN

DRS

TCSD

OU

TD

IO4

DIO

3SC

LKD

IND

IO1

DIO

2

PA2

PA3

PA4

PA5

PB1

PB2

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

1 23 4

J2

5V

GN

D

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIJ201

PIJ202

PIJ203

PIJ204

COJ2

PIJ501

PIJ502

PIJ503

PIJ504

PIJ505

PIJ506

PIJ507

PIJ508

PIJ509

PIJ5

010

PIJ5011

PIJ5012

PIJ5013

PIJ5014

PIJ5

015

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

COJ5

PI1041

PI1080

PI1059

PI1060

PI1061

PI1062

PIC101 PIC201

PIJ201

PIJ5

010

PIJ5011

PIJ5012

PI1066

PIJ506

NLCS

NLPA3

PI1067

PIJ505

NLDIN

NLPA4

PIJ507

NLDI

O1 PIJ509

NLDI

O2 PIJ501

NLDI

O3

PI1045

PIJ502

NLDI

O4

NLPB2

PI1058

PIJ504

NLDOUT

NLPA5

PI1042

PI1043

PI1078

PI1079

PIC102 PIC202

PIJ204

PIJ5013

PIJ5014

PIJ5

015

PIJ202

PIJ203

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

PI1063

PIJ503

NLPA2

NLSCLK

PI102

PIJ508

NLPB1

NLRS

T

PI1029

NLPB4

PI1030

NLPB5

PI103

NLPC0

PI1038

NLPC1

PI1039

NLPC2

PI1040

NLPC3

PI1019

NLPD0

PI1022

NLPD1

PI1021

NLPD2

PI1020

NLPD3

PI1026

NLPD4

PI1025

NLPD5

PI1024

NLPD6

PI1023

NLPD7

PI1014

NLPE0

PI1015

NLPE

1

PI1016

NLPE2

PI1017

NLPE3

PI1028

NLPE4

PI1027

NLPE

5

PI1057

NLPF

0

PI1068

NLPF

1

PI1056

NLPF2

PI1069

NLPF3

PI1055

NLPF

4

PI1054

NLPG2

PI1070

NLPG3

PI1071

NLPG4

PI1053

NLPG5

PI1052

NLPG6

PI1072

NLPG7

PI1010

NLPH0

PI109

NLPH1

PI1065

NLPH2

PI1064

NLPH3

PI1032

NLPJ0

PI1011

NLPJ1

PI1076

NLPK4

PI1049

NLPK5

PI1075

NLPK6

PI1050

NLPK7

PI1077

NLPL5

PI101

NLPL6

PI1044

NLPL7

PI1046

NLPM

4

PI1047

NLPM

5

PI1048

NLPM

6

PI1074

NLPM

7

PI1035

NLPN0

PI1034

NLPN1

PI106

NLPN2

PI107

NLPN3

PI1033

NLPN4

PI108

NLPN5

PI1031

NLPP0

PI1012

NLPP1

PI104

NLPP3

PI1036

NLPP4

PI105

NLPP5

PI1018

NLPQ2

PI1013

NLPQ3

PI1037

NLPQ4

PI1051

NLPQ5

PI1073

NLPQ6

56 Dodatek A Schematy modułoacutew urządzenia

Dodatek B

Wykresy pobranych trajektorii

Dodatek B Wykresy pobranych trajektorii 57

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B1 Trajektoria ucząca z minimalnym zastosowaniem filtru

58 Dodatek B Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

C

onne

ctor

sSch

Doc

Dra

wn

By

PQ2

11

PQ3

27

PQ5

57

PQ6

58

PM7

71PM

672

PM5

73PM

474

PL5

86

PL7

93PL

694

PQ4

102

PP3

104

PP4

105

PP5

106

PN0

107

PN1

108

PN2

109

PN3

110

PN4

111

PN5

112

PP0

118

PP1

119

U1B

TM4C

1292

NC

PDTI

3R

PD0

1

PD1

2

PD2

3

PD3

4

PE3

12PE

213

PE1

14PE

015

PH0

29

PH1

30

PH2

31

PH3

32

PA2

35

PA3

36

PA4

37

PA5

38

PF0

42

PF1

43

PF2

44

PF3

45

PF4

46

PG2

51

PG3

52

PG4

53

PG5

54

PG6

55

PG7

56

PK7

59PK

660

PK5

61PK

462

PB2

91PB

196

PC3_

TDO

_SW

O97

PC2_

TDI

98PC

1_TM

S_SW

DIO

99PC

0_TC

K_S

WC

LK10

0

PJ0

116

PJ1

117

PB5

120

PB4

121

PE4

123

PE5

124

PD4

125

PD5

126

PD6

127

PD7

128

U1G

TM4C

1292

NC

PDTI

3R

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PH0

PH1

PH2

PH3

PJ0

PJ1

PK4

PK5

PK6

PK7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

PC0

PC1

PC2

PC3

3V3

GN

D

RST

92

101

74

83 5

6

J2 Pin

200

mm

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

Con

nect

or s

chem

a

1

Bar

tosz

Kac

zor

2

4

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIJ201

PIJ202

PIJ203

PIJ204

PIJ205

PIJ206

PIJ207

PIJ208

PIJ209

PIJ2010

COJ2

PIU1011

PIU1027

PIU1057

PIU1058

PIU1071

PIU1072

PIU1073

PIU1074

PIU1086

PIU1093

PIU1094

PIU10102

PIU10104

PIU10105

PIU10106

PIU10107

PIU10108

PIU10109

PIU10110

PIU10111

PIU10112

PIU10118

PIU10119

COU1B

PIU101

PIU102

PIU103

PIU104

PIU1012

PIU1013

PIU1014

PIU1015

PIU1029

PIU1030

PIU1031

PIU1032

PIU1035

PIU1036

PIU1037

PIU1038

PIU1042

PIU1043

PIU1044

PIU1045

PIU1046

PIU1051

PIU1052

PIU1053

PIU1054

PIU1055

PIU1056

PIU1059

PIU1060

PIU1061

PIU1062

PIU1091

PIU1096

PIU1097

PIU1098

PIU1099

PIU10100

PIU10116

PIU10117

PIU10120

PIU10121

PIU10123

PIU10124

PIU10125

PIU10126

PIU10127

PIU10128

COU1G

PI1041

PI1080

PIJ201

PI1059

PI1060

PI1061

PI1062

PI1042

PI1043

PI1078

PI1079

PIJ202

PIJ203

PIJ205

PIJ204

PI1063

PIU1035

NLPA2

PI1066

PIU1036

NLPA3

PI1067

PIU1037

NLPA4

PI1058

PIU1038

NLPA5

PI102

PIU1096

NLPB1

PI1045

PIU1091

NLPB2

PI1029

PIU10121

NLPB4

PI1030

PIU10120

NLPB5

PI103

PIJ209

PIU10100

NLPC0

PI1038

PIJ2010

PIU1099

NLPC

1

PI1039

PIJ207

PIU1098

NLPC2

PI1040

PIJ208

PIU1097

NLPC3

PI1019

PIU101

NLPD

0

PI1022

PIU102

NLPD1

PI1021

PIU103

NLPD

2

PI1020

PIU104

NLPD3

PI1026

PIU10125

NLPD

4

PI1025

PIU10126

NLPD5

PI1024

PIU10127

NLPD

6

PI1023

PIU10128

NLPD

7

PI1014

PIU1015

NLPE0

PI1015

PIU1014

NLPE1

PI1016

PIU1013

NLPE2

PI1017

PIU1012

NLPE3

PI1028

PIU10123

NLPE4

PI1027

PIU10124

NLPE5

PI1057

PIU1042

NLPF0

PI1068

PIU1043

NLPF1

PI1056

PIU1044

NLPF2

PI1069

PIU1045

NLPF3

PI1055

PIU1046

NLPF4

PI1054

PIU1051

NLPG2

PI1070

PIU1052

NLPG3

PI1071

PIU1053

NLPG4

PI1053

PIU1054

NLPG5

PI1052

PIU1055

NLPG6

PI1072

PIU1056

NLPG7

PI1010

PIU1029

NLPH0

PI109

PIU1030

NLPH1

PI1065

PIU1031

NLPH2

PI1064

PIU1032

NLPH3

PI1032

PIU10116

NLPJ0

PI1011

PIU10117

NLPJ1

PI1076

PIU1062

NLPK4

PI1049

PIU1061

NLPK5

PI1075

PIU1060

NLPK6

PI1050

PIU1059

NLPK7

PI1077

PIU1086

NLPL5

PI101

PIU1094

NLPL6

PI1044

PIU1093

NLPL7

PI1046

PIU1074

NLPM

4

PI1047

PIU1073

NLPM

5

PI1048

PIU1072

NLPM

6

PI1074

PIU1071

NLPM

7

PI1035

PIU10107

NLPN0

PI1034

PIU10108

NLPN1

PI106

PIU10109

NLPN2

PI107

PIU10110

NLPN3

PI1033

PIU10111

NLPN4

PI108

PIU10112

NLPN5

PI1031

PIU10118

NLPP0

PI1012

PIU10119

NLPP1

PI104

PIU10104

NLPP3

PI1036

PIU10105

NLPP4

PI105

PIU10106

NLPP5

PI1018

PIU1011

NLPQ

2

PI1013

PIU1027

NLPQ3

PI1037

PIU10102

NLPQ

4

PI1051

PIU1057

NLPQ5

PI1073

PIU1058

NLPQ

6

PIJ206

NLRST

Dodatek A Schematy modułoacutew urządzenia 53

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

N

etw

ork

SchD

ocD

raw

n B

y

B_1

PR

XP

3

B_1

CEN

SD

A4

B_1

NR

XN

5

B_2

PSD

P7

B_2

CEN

SC

L8

B_2

NS

DN

9

B_3

PTX

EN11

B_3

CEN

12

B_3

NT

XD

IS13

B_4

PTX

P15

B_4

CEN

~B

USP

16

B_4

NT

XN

17

NW

_LED

4B19

NW

_LED

4A20

NW

_LED

3B21

NW

_LED

3A22

NW

_LED

2B23

NW

_LED

2A24

NW

_LED

1B25

NW

_LED

1A26

A_1

PR

XP

29A

_1C

ENS

DA

30A

_1N

RX

N31

A_2

PSD

P33

A_2

CEN

SC

L34

A_2

NS

DN

35

A_3

PTX

EN37

A_3

CEN

38A

_3N

TX

DIS

39

A_4

PTX

P41

A_4

CEN

~B

USP

42A

_4N

TX

N43

C_T

X45

C_R

X46

C_T

XEN

47~C

_BU

SP48

GA

TE1

49G

ATE

250

Net

wor

k C

onne

ctor

A Any

bus_

CC

_B40

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T1 47

uHB_

1PB_

1CEN

B_1N

B_1P

B_1C

EN

B_1N

B_2P

B_2C

ENB_

2N

B_2P

B_2C

EN

B_2N

16V

100n

F

C1

16V

100n

F

C2

GN

DS

PE

2kV

1nF

C3

R1

75R

275

R5

1M2k

V1n

F

C4 PE

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T2 47

uH

A_1

P

A_1

CEN

A_1

N

A_2

P

A_2

CEN

A_2

N

16V

100n

F

C5

16V

100n

F

C6

GN

DS

PE

2kV

1nF

C7

R6

75R

775

A_1

PA

_1C

ENA

_1N

A_2

PA

_2C

ENA

_2N

DL1

R18

220

R17

220

R16

220

R15

220

R14

220

R13

220

R12

220

R11

220

DL2

DL3

DL4

GN

DS

3210 4

J4 Clic

k_M

ate

5x1

3210 4

J5 Clic

k_M

ate

5x1

Net

wor

k co

nnec

tor

1

3

4B

arto

sz k

aczo

r

PI03

PI04

PI05

PI07

PI08

PI09

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PI020

PI021

PI022

PI023

PI024

PI025

PI026

PI029

PI030

PI031

PI033

PI034

PI035

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

COA

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIC301 PIC302

COC3

PIC401 PIC402

COC4

PIC501 PIC502

COC5

PIC601 PIC602

COC6

PIC701 PIC702

COC7

PIDL101 PIDL102

PIDL103 PIDL104 CODL1

PIDL201 PIDL202

PIDL203 PIDL204 CODL2

PIDL301 PIDL302

PIDL303 PIDL304 CODL3

PIDL401 PIDL402

PIDL403 PIDL404 CODL4

PIJ400

PIJ401

PIJ402

PIJ403

PIJ404

COJ4 PIJ500

PIJ501

PIJ502

PIJ503

PIJ504

COJ5

PIR101 PIR102 COR

1 PIR201 PIR202 COR

2

PIR501 PIR502 COR

5

PIR601 PIR602 COR

6 PIR701 PIR702 COR

7

PIR1101 PIR1102 COR1

1

PIR1201 PIR1202 COR12

PIR1301 PIR1302 COR13

PIR1401 PIR1402 COR14

PIR1501 PIR1502 COR15

PIR1601 PIR1602 COR16

PIR1701 PIR1702 COR17 PIR1801 PIR1802

COR1

8

PIT101

PIT102

PIT103

PIT104

PIT105

PIT106

PIT107

PIT108

PIT109

PIT1010

PIT1011

PIT1012

COT1

PIT201

PIT202

PIT203

PIT204

PIT205

PIT206

PIT207

PIT208

PIT209

PIT2010

PIT2011

PIT2012

COT2

PI030

PIC501

PIT203

NLA01CEN

PI031

PIT202

NLA0

1N

PI029

PIT201

NLA0

1P

PI034

PIC601

PIT204

NLA02CEN

PI035

PIT206

NLA0

2N

PI033

PIT205

NLA0

2P

PI04

PIC101

PIT103

NLB01CEN

PI05

PIT102

NLB0

1N

PI03

PIT101

NLB0

1P

PI08

PIC201

PIT104

NLB02CEN

PI09

PIT106

NLB0

2N

PI07

PIT105

NLB0

2P

PIC102 PIC202

PIC502 PIC602

PIDL101 PIDL103

PIDL201 PIDL203

PIDL301 PIDL303

PIDL401 PIDL403

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PIR1102

PI020

PIR1202

PI021

PIR1302

PI022

PIR1402

PI023

PIR1502

PI024

PIR1602 PI025

PIR1702 PI026

PIR1802

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

PIC301 PIR101

PIR201

PIC401

PIJ404 PIJ504

PIR502

PIC701 PIR601

PIR701

PIDL102

PIR1701

PIDL104

PIR1801

PIDL202

PIR1501

PIDL204

PIR1601

PIDL302

PIR1301

PIDL304

PIR1401

PIDL402

PIR1101

PIDL404

PIR1201

PIJ400

PIT1012

PIJ401

PIT1011

PIJ402

PIT108

PIJ403

PIT107

PIJ500

PIT2012

PIJ501

PIT2011

PIJ502

PIT208

PIJ503

PIT207 PIR102

PIT1010

PIR202

PIT109

PIR602

PIT2010

PIR702

PIT209

PIC302

PIC402

PIC702

PIR501

54 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

P

ower

Supp

lyS

chD

ocD

raw

n B

y

5V GN

D

GN

D

3V3

GN

DS

GN

DS

3V3 R

pu10

k 16V

10nF

C21

GN

DS

Rs

470

TAC

TD60

B1

GN

DS

16V

10uF

Cin

R20

510k

Rpg

499k

R21

162k

16V

22uF

Cou

t

16V

4uF

Cld

o

3V3

GN

DS

GN

DS

16V

12pF

Cx2

16V

12pF

Cx1

GN

DS

3V3

1

GN

D2

GN

D6

GN

D10

GN

D14

GN

D18

3V3

27

GN

D28

GN

D32

GN

D36

GN

D40

GN

D44

3V3

51

GN

D52

Pow

er p

ins

3V3

101

GN

D10

2

GN

D10

8

GN

D11

4

GN

D11

8

GN

D12

4

GN

D12

8

GN

D13

4G

ND

140

GN

D14

4G

ND

150

3V3

155

GN

D15

6

C Any

bus_

CC

_B40

3V3

GN

DS

GN

DG

ND

S

WA

KE_

N64

HIB

_N65

XO

SC0

66

XO

SC1

67

RST

_N70

OSC

088

OSC

189

U1C

TM4C

1292

NC

PDTI

3R

VD

D7

VD

DA

8

VR

EFA

+9

GN

DA

10

VD

D16

GN

D17

VD

D26

VD

D28

VD

D39

VD

D47

GN

D48

VD

D63

VB

AT

68

VD

D69

VD

D79

GN

D80

VD

DC

87

VD

D90

VD

D10

1

VD

D11

3

GN

D11

4

VD

DC

115

VD

D12

2

U1D

TM4C

1292

NC

PDTI

3R

RST

GN

DX

18M

Hz

GN

D

EN1

PG2

VIN

3

VIN

4G

ND

5G

ND

6FB

7V

OU

T8

TPS8

2085

GN

D9

U2

TPS8

2085

GN

D

R66

500k

2kV

1nF

C55 PE

GN

DR

5550

0k2k

V1n

F

C44 PE

GN

D

R77

500k

2kV

1nF

C66 PE

GN

D

16V

100n

F

CV

116

V10

0nF

CV

216

V10

0nF

CV

316

V10

0nF

CV

4

3V3

16V

100n

F

CV

516

V10

0nF

CV

616

V10

0nF

CV

716

V10

0nF

CV

816

V10

0nF

CV

916

V10

0nF

CV

1016

V10

0nF

CV

1116

V10

0nF

CV

1216

V10

0nF

CV

13

GN

D

16V

1uF

CV

ref1

16V

10nF

CV

ref2

GN

DG

ND Po

wer

supp

ly

1

4

4 B

arto

sz K

aczo

r

PI01

PI02

PI06

PI010

PI014

PI018

PI027

PI028

PI032

PI036

PI040

PI044

PI051

PI052

PI0101

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0155

PI0156

COC

PIB101

PIB102

PIB103 PI

B104 COB1

PIC2101 PIC2102

COC2

1

PIC4401 PIC4402

COC44

PIC5501 PIC5502

COC55

PIC6601 PIC6602

COC66

PICin01 PICin02

COCin

PICldo01 PICldo02

COCl

do

PICout01 PICout02

COCo

ut

PICV101 PICV102

COCV1

PICV201 PICV202

COCV

2 PICV301 PICV302

COCV

3 PICV401 PICV402

COCV

4 PICV501 PICV502

COCV5

PICV601 PICV602

COCV

6 PICV701 PICV702

COCV

7 PICV801 PICV802

COCV

8 PICV901 PICV902

COCV

9 PICV1001 PICV1002

COCV10

PICV1101 PICV1102

COCV11

PICV1201 PICV1202

COCV12

PICV1301 PICV1302

COCV13

PICVref101 PICVref102

COCV

ref1

PICVref201 PICVref202

COCVref2

PICx101 PICx102

COCx1

PICx201 PICx202

COCx2

PIR2001 PIR2002 COR20

PIR2

101

PIR2

102 COR

21

PIR5501 PIR5502 COR55

PIR6601 PIR6602 COR

66

PIR7701 PIR7702 CO

R77

PIRpg01 PIRpg02 CO

Rpg

PIRpu01 PIRpu02 CORpu

PIRs01

PIRs

02

CORs

PIU1064

PIU1065

PIU1066

PIU1067

PIU1070

PIU1088

PIU1089

COU1

C

PIU107

PIU108

PIU109

PIU1010

PIU1016

PIU1017

PIU1026

PIU1028

PIU1039

PIU1047

PIU1048

PIU1063

PIU1068

PIU1069

PIU1079

PIU1080

PIU1087

PIU1090

PIU10101

PIU10113

PIU10114

PIU10115

PIU10122

COU1

D

PIU201

PIU202

PIU203

PIU204

PIU205

PIU206

PIU207

PIU208

PIU209

COU2

PIX101 PIX102 PIX10GND

PIX10GND2 COX

1

PI01

PI027

PI051

PI0101

PI0155

PICout01

PICV101 PICV201

PICV301 PICV401

PICV501 PICV601

PICV701 PICV801

PICV901 PICV1001

PICV1101 PICV1201

PICV1301

PICVref101 PICVref201

PIR2002 PIRpg02

PIRpu02

PIU107

PIU108

PIU109

PIU1016

PIU1026

PIU1028

PIU1039

PIU1047

PIU1063

PIU1068

PIU1069

PIU1079

PIU1090

PIU10101

PIU10113

PIU10122

PIU208

PICin01

PIU201

PIU203

PIU204

PI02

PI06

PI010

PI014

PI018

PI028

PI032

PI036

PI040

PI044

PI052

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0156

PIB101

PIB102

PIC2102

PIC4401 PIC5501

PIC6601

PICin02

PICldo02

PICout02

PICV102 PICV202

PICV302 PICV402

PICV502 PICV602

PICV702 PICV802

PICV902 PICV1002

PICV1102 PICV1202

PICV1302

PICVref102 PICVref202

PICx102 PICx202

PIR2

101

PIR5502 PIR6602

PIR7702

PIU1010

PIU1017

PIU1048

PIU1064

PIU1080

PIU1088

PIU10114

PIU205

PIU206

PIU209

PIX10GND

PIX10GND2

PIB103 PI

B104 PIRs01

PICldo01

PIU1087

PIU10115

PICx101

PIU1067

PIX101

PICx201

PIU1066

PIX102

PIR2001 PIR2

102

PIU207

PIRpg01

PIU202

PIU1065

PIU1089

PIC4402 PIC5502

PIC6602

PIR5501 PIR6601

PIR7701

PIC2101 PIRpu01 PI

Rs02

PIU1070

NLRST

Dodatek A Schematy modułoacutew urządzenia 55

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-15

Shee

t o

fFi

le

CU

sers

IM

UO

ldS

chD

ocD

raw

n B

y

8642 16141210 18 20 22 24

17 19 21 239 11 13 157531J5 H

eade

r 12x

2

5V

GN

D

5V

16V

100n

F

C1

16V

100n

F

C2

GN

DRS

TCSD

OU

TD

IO4

DIO

3SC

LKD

IND

IO1

DIO

2

PA2

PA3

PA4

PA5

PB1

PB2

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

1 23 4

J2

5V

GN

D

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIJ201

PIJ202

PIJ203

PIJ204

COJ2

PIJ501

PIJ502

PIJ503

PIJ504

PIJ505

PIJ506

PIJ507

PIJ508

PIJ509

PIJ5

010

PIJ5011

PIJ5012

PIJ5013

PIJ5014

PIJ5

015

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

COJ5

PI1041

PI1080

PI1059

PI1060

PI1061

PI1062

PIC101 PIC201

PIJ201

PIJ5

010

PIJ5011

PIJ5012

PI1066

PIJ506

NLCS

NLPA3

PI1067

PIJ505

NLDIN

NLPA4

PIJ507

NLDI

O1 PIJ509

NLDI

O2 PIJ501

NLDI

O3

PI1045

PIJ502

NLDI

O4

NLPB2

PI1058

PIJ504

NLDOUT

NLPA5

PI1042

PI1043

PI1078

PI1079

PIC102 PIC202

PIJ204

PIJ5013

PIJ5014

PIJ5

015

PIJ202

PIJ203

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

PI1063

PIJ503

NLPA2

NLSCLK

PI102

PIJ508

NLPB1

NLRS

T

PI1029

NLPB4

PI1030

NLPB5

PI103

NLPC0

PI1038

NLPC1

PI1039

NLPC2

PI1040

NLPC3

PI1019

NLPD0

PI1022

NLPD1

PI1021

NLPD2

PI1020

NLPD3

PI1026

NLPD4

PI1025

NLPD5

PI1024

NLPD6

PI1023

NLPD7

PI1014

NLPE0

PI1015

NLPE

1

PI1016

NLPE2

PI1017

NLPE3

PI1028

NLPE4

PI1027

NLPE

5

PI1057

NLPF

0

PI1068

NLPF

1

PI1056

NLPF2

PI1069

NLPF3

PI1055

NLPF

4

PI1054

NLPG2

PI1070

NLPG3

PI1071

NLPG4

PI1053

NLPG5

PI1052

NLPG6

PI1072

NLPG7

PI1010

NLPH0

PI109

NLPH1

PI1065

NLPH2

PI1064

NLPH3

PI1032

NLPJ0

PI1011

NLPJ1

PI1076

NLPK4

PI1049

NLPK5

PI1075

NLPK6

PI1050

NLPK7

PI1077

NLPL5

PI101

NLPL6

PI1044

NLPL7

PI1046

NLPM

4

PI1047

NLPM

5

PI1048

NLPM

6

PI1074

NLPM

7

PI1035

NLPN0

PI1034

NLPN1

PI106

NLPN2

PI107

NLPN3

PI1033

NLPN4

PI108

NLPN5

PI1031

NLPP0

PI1012

NLPP1

PI104

NLPP3

PI1036

NLPP4

PI105

NLPP5

PI1018

NLPQ2

PI1013

NLPQ3

PI1037

NLPQ4

PI1051

NLPQ5

PI1073

NLPQ6

56 Dodatek A Schematy modułoacutew urządzenia

Dodatek B

Wykresy pobranych trajektorii

Dodatek B Wykresy pobranych trajektorii 57

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B1 Trajektoria ucząca z minimalnym zastosowaniem filtru

58 Dodatek B Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

N

etw

ork

SchD

ocD

raw

n B

y

B_1

PR

XP

3

B_1

CEN

SD

A4

B_1

NR

XN

5

B_2

PSD

P7

B_2

CEN

SC

L8

B_2

NS

DN

9

B_3

PTX

EN11

B_3

CEN

12

B_3

NT

XD

IS13

B_4

PTX

P15

B_4

CEN

~B

USP

16

B_4

NT

XN

17

NW

_LED

4B19

NW

_LED

4A20

NW

_LED

3B21

NW

_LED

3A22

NW

_LED

2B23

NW

_LED

2A24

NW

_LED

1B25

NW

_LED

1A26

A_1

PR

XP

29A

_1C

ENS

DA

30A

_1N

RX

N31

A_2

PSD

P33

A_2

CEN

SC

L34

A_2

NS

DN

35

A_3

PTX

EN37

A_3

CEN

38A

_3N

TX

DIS

39

A_4

PTX

P41

A_4

CEN

~B

USP

42A

_4N

TX

N43

C_T

X45

C_R

X46

C_T

XEN

47~C

_BU

SP48

GA

TE1

49G

ATE

250

Net

wor

k C

onne

ctor

A Any

bus_

CC

_B40

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T1 47

uHB_

1PB_

1CEN

B_1N

B_1P

B_1C

EN

B_1N

B_2P

B_2C

ENB_

2N

B_2P

B_2C

EN

B_2N

16V

100n

F

C1

16V

100n

F

C2

GN

DS

PE

2kV

1nF

C3

R1

75R

275

R5

1M2k

V1n

F

C4 PE

CSM

0645

D

TD+

1

TD-

2

TDC

3

RD

C4

RD

+5

RD

-6

RX

-7

RX

+8

RC

T9

TCT

10TX

-11

TX+

12T2 47

uH

A_1

P

A_1

CEN

A_1

N

A_2

P

A_2

CEN

A_2

N

16V

100n

F

C5

16V

100n

F

C6

GN

DS

PE

2kV

1nF

C7

R6

75R

775

A_1

PA

_1C

ENA

_1N

A_2

PA

_2C

ENA

_2N

DL1

R18

220

R17

220

R16

220

R15

220

R14

220

R13

220

R12

220

R11

220

DL2

DL3

DL4

GN

DS

3210 4

J4 Clic

k_M

ate

5x1

3210 4

J5 Clic

k_M

ate

5x1

Net

wor

k co

nnec

tor

1

3

4B

arto

sz k

aczo

r

PI03

PI04

PI05

PI07

PI08

PI09

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PI020

PI021

PI022

PI023

PI024

PI025

PI026

PI029

PI030

PI031

PI033

PI034

PI035

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

COA

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIC301 PIC302

COC3

PIC401 PIC402

COC4

PIC501 PIC502

COC5

PIC601 PIC602

COC6

PIC701 PIC702

COC7

PIDL101 PIDL102

PIDL103 PIDL104 CODL1

PIDL201 PIDL202

PIDL203 PIDL204 CODL2

PIDL301 PIDL302

PIDL303 PIDL304 CODL3

PIDL401 PIDL402

PIDL403 PIDL404 CODL4

PIJ400

PIJ401

PIJ402

PIJ403

PIJ404

COJ4 PIJ500

PIJ501

PIJ502

PIJ503

PIJ504

COJ5

PIR101 PIR102 COR

1 PIR201 PIR202 COR

2

PIR501 PIR502 COR

5

PIR601 PIR602 COR

6 PIR701 PIR702 COR

7

PIR1101 PIR1102 COR1

1

PIR1201 PIR1202 COR12

PIR1301 PIR1302 COR13

PIR1401 PIR1402 COR14

PIR1501 PIR1502 COR15

PIR1601 PIR1602 COR16

PIR1701 PIR1702 COR17 PIR1801 PIR1802

COR1

8

PIT101

PIT102

PIT103

PIT104

PIT105

PIT106

PIT107

PIT108

PIT109

PIT1010

PIT1011

PIT1012

COT1

PIT201

PIT202

PIT203

PIT204

PIT205

PIT206

PIT207

PIT208

PIT209

PIT2010

PIT2011

PIT2012

COT2

PI030

PIC501

PIT203

NLA01CEN

PI031

PIT202

NLA0

1N

PI029

PIT201

NLA0

1P

PI034

PIC601

PIT204

NLA02CEN

PI035

PIT206

NLA0

2N

PI033

PIT205

NLA0

2P

PI04

PIC101

PIT103

NLB01CEN

PI05

PIT102

NLB0

1N

PI03

PIT101

NLB0

1P

PI08

PIC201

PIT104

NLB02CEN

PI09

PIT106

NLB0

2N

PI07

PIT105

NLB0

2P

PIC102 PIC202

PIC502 PIC602

PIDL101 PIDL103

PIDL201 PIDL203

PIDL301 PIDL303

PIDL401 PIDL403

PI011

PI012

PI013

PI015

PI016

PI017

PI019

PIR1102

PI020

PIR1202

PI021

PIR1302

PI022

PIR1402

PI023

PIR1502

PI024

PIR1602 PI025

PIR1702 PI026

PIR1802

PI037

PI038

PI039

PI041

PI042

PI043

PI045

PI046

PI047

PI048

PI049

PI050

PIC301 PIR101

PIR201

PIC401

PIJ404 PIJ504

PIR502

PIC701 PIR601

PIR701

PIDL102

PIR1701

PIDL104

PIR1801

PIDL202

PIR1501

PIDL204

PIR1601

PIDL302

PIR1301

PIDL304

PIR1401

PIDL402

PIR1101

PIDL404

PIR1201

PIJ400

PIT1012

PIJ401

PIT1011

PIJ402

PIT108

PIJ403

PIT107

PIJ500

PIT2012

PIJ501

PIT2011

PIJ502

PIT208

PIJ503

PIT207 PIR102

PIT1010

PIR202

PIT109

PIR602

PIT2010

PIR702

PIT209

PIC302

PIC402

PIC702

PIR501

54 Dodatek A Schematy modułoacutew urządzenia

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

P

ower

Supp

lyS

chD

ocD

raw

n B

y

5V GN

D

GN

D

3V3

GN

DS

GN

DS

3V3 R

pu10

k 16V

10nF

C21

GN

DS

Rs

470

TAC

TD60

B1

GN

DS

16V

10uF

Cin

R20

510k

Rpg

499k

R21

162k

16V

22uF

Cou

t

16V

4uF

Cld

o

3V3

GN

DS

GN

DS

16V

12pF

Cx2

16V

12pF

Cx1

GN

DS

3V3

1

GN

D2

GN

D6

GN

D10

GN

D14

GN

D18

3V3

27

GN

D28

GN

D32

GN

D36

GN

D40

GN

D44

3V3

51

GN

D52

Pow

er p

ins

3V3

101

GN

D10

2

GN

D10

8

GN

D11

4

GN

D11

8

GN

D12

4

GN

D12

8

GN

D13

4G

ND

140

GN

D14

4G

ND

150

3V3

155

GN

D15

6

C Any

bus_

CC

_B40

3V3

GN

DS

GN

DG

ND

S

WA

KE_

N64

HIB

_N65

XO

SC0

66

XO

SC1

67

RST

_N70

OSC

088

OSC

189

U1C

TM4C

1292

NC

PDTI

3R

VD

D7

VD

DA

8

VR

EFA

+9

GN

DA

10

VD

D16

GN

D17

VD

D26

VD

D28

VD

D39

VD

D47

GN

D48

VD

D63

VB

AT

68

VD

D69

VD

D79

GN

D80

VD

DC

87

VD

D90

VD

D10

1

VD

D11

3

GN

D11

4

VD

DC

115

VD

D12

2

U1D

TM4C

1292

NC

PDTI

3R

RST

GN

DX

18M

Hz

GN

D

EN1

PG2

VIN

3

VIN

4G

ND

5G

ND

6FB

7V

OU

T8

TPS8

2085

GN

D9

U2

TPS8

2085

GN

D

R66

500k

2kV

1nF

C55 PE

GN

DR

5550

0k2k

V1n

F

C44 PE

GN

D

R77

500k

2kV

1nF

C66 PE

GN

D

16V

100n

F

CV

116

V10

0nF

CV

216

V10

0nF

CV

316

V10

0nF

CV

4

3V3

16V

100n

F

CV

516

V10

0nF

CV

616

V10

0nF

CV

716

V10

0nF

CV

816

V10

0nF

CV

916

V10

0nF

CV

1016

V10

0nF

CV

1116

V10

0nF

CV

1216

V10

0nF

CV

13

GN

D

16V

1uF

CV

ref1

16V

10nF

CV

ref2

GN

DG

ND Po

wer

supp

ly

1

4

4 B

arto

sz K

aczo

r

PI01

PI02

PI06

PI010

PI014

PI018

PI027

PI028

PI032

PI036

PI040

PI044

PI051

PI052

PI0101

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0155

PI0156

COC

PIB101

PIB102

PIB103 PI

B104 COB1

PIC2101 PIC2102

COC2

1

PIC4401 PIC4402

COC44

PIC5501 PIC5502

COC55

PIC6601 PIC6602

COC66

PICin01 PICin02

COCin

PICldo01 PICldo02

COCl

do

PICout01 PICout02

COCo

ut

PICV101 PICV102

COCV1

PICV201 PICV202

COCV

2 PICV301 PICV302

COCV

3 PICV401 PICV402

COCV

4 PICV501 PICV502

COCV5

PICV601 PICV602

COCV

6 PICV701 PICV702

COCV

7 PICV801 PICV802

COCV

8 PICV901 PICV902

COCV

9 PICV1001 PICV1002

COCV10

PICV1101 PICV1102

COCV11

PICV1201 PICV1202

COCV12

PICV1301 PICV1302

COCV13

PICVref101 PICVref102

COCV

ref1

PICVref201 PICVref202

COCVref2

PICx101 PICx102

COCx1

PICx201 PICx202

COCx2

PIR2001 PIR2002 COR20

PIR2

101

PIR2

102 COR

21

PIR5501 PIR5502 COR55

PIR6601 PIR6602 COR

66

PIR7701 PIR7702 CO

R77

PIRpg01 PIRpg02 CO

Rpg

PIRpu01 PIRpu02 CORpu

PIRs01

PIRs

02

CORs

PIU1064

PIU1065

PIU1066

PIU1067

PIU1070

PIU1088

PIU1089

COU1

C

PIU107

PIU108

PIU109

PIU1010

PIU1016

PIU1017

PIU1026

PIU1028

PIU1039

PIU1047

PIU1048

PIU1063

PIU1068

PIU1069

PIU1079

PIU1080

PIU1087

PIU1090

PIU10101

PIU10113

PIU10114

PIU10115

PIU10122

COU1

D

PIU201

PIU202

PIU203

PIU204

PIU205

PIU206

PIU207

PIU208

PIU209

COU2

PIX101 PIX102 PIX10GND

PIX10GND2 COX

1

PI01

PI027

PI051

PI0101

PI0155

PICout01

PICV101 PICV201

PICV301 PICV401

PICV501 PICV601

PICV701 PICV801

PICV901 PICV1001

PICV1101 PICV1201

PICV1301

PICVref101 PICVref201

PIR2002 PIRpg02

PIRpu02

PIU107

PIU108

PIU109

PIU1016

PIU1026

PIU1028

PIU1039

PIU1047

PIU1063

PIU1068

PIU1069

PIU1079

PIU1090

PIU10101

PIU10113

PIU10122

PIU208

PICin01

PIU201

PIU203

PIU204

PI02

PI06

PI010

PI014

PI018

PI028

PI032

PI036

PI040

PI044

PI052

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0156

PIB101

PIB102

PIC2102

PIC4401 PIC5501

PIC6601

PICin02

PICldo02

PICout02

PICV102 PICV202

PICV302 PICV402

PICV502 PICV602

PICV702 PICV802

PICV902 PICV1002

PICV1102 PICV1202

PICV1302

PICVref102 PICVref202

PICx102 PICx202

PIR2

101

PIR5502 PIR6602

PIR7702

PIU1010

PIU1017

PIU1048

PIU1064

PIU1080

PIU1088

PIU10114

PIU205

PIU206

PIU209

PIX10GND

PIX10GND2

PIB103 PI

B104 PIRs01

PICldo01

PIU1087

PIU10115

PICx101

PIU1067

PIX101

PICx201

PIU1066

PIX102

PIR2001 PIR2

102

PIU207

PIRpg01

PIU202

PIU1065

PIU1089

PIC4402 PIC5502

PIC6602

PIR5501 PIR6601

PIR7701

PIC2101 PIRpu01 PI

Rs02

PIU1070

NLRST

Dodatek A Schematy modułoacutew urządzenia 55

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-15

Shee

t o

fFi

le

CU

sers

IM

UO

ldS

chD

ocD

raw

n B

y

8642 16141210 18 20 22 24

17 19 21 239 11 13 157531J5 H

eade

r 12x

2

5V

GN

D

5V

16V

100n

F

C1

16V

100n

F

C2

GN

DRS

TCSD

OU

TD

IO4

DIO

3SC

LKD

IND

IO1

DIO

2

PA2

PA3

PA4

PA5

PB1

PB2

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

1 23 4

J2

5V

GN

D

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIJ201

PIJ202

PIJ203

PIJ204

COJ2

PIJ501

PIJ502

PIJ503

PIJ504

PIJ505

PIJ506

PIJ507

PIJ508

PIJ509

PIJ5

010

PIJ5011

PIJ5012

PIJ5013

PIJ5014

PIJ5

015

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

COJ5

PI1041

PI1080

PI1059

PI1060

PI1061

PI1062

PIC101 PIC201

PIJ201

PIJ5

010

PIJ5011

PIJ5012

PI1066

PIJ506

NLCS

NLPA3

PI1067

PIJ505

NLDIN

NLPA4

PIJ507

NLDI

O1 PIJ509

NLDI

O2 PIJ501

NLDI

O3

PI1045

PIJ502

NLDI

O4

NLPB2

PI1058

PIJ504

NLDOUT

NLPA5

PI1042

PI1043

PI1078

PI1079

PIC102 PIC202

PIJ204

PIJ5013

PIJ5014

PIJ5

015

PIJ202

PIJ203

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

PI1063

PIJ503

NLPA2

NLSCLK

PI102

PIJ508

NLPB1

NLRS

T

PI1029

NLPB4

PI1030

NLPB5

PI103

NLPC0

PI1038

NLPC1

PI1039

NLPC2

PI1040

NLPC3

PI1019

NLPD0

PI1022

NLPD1

PI1021

NLPD2

PI1020

NLPD3

PI1026

NLPD4

PI1025

NLPD5

PI1024

NLPD6

PI1023

NLPD7

PI1014

NLPE0

PI1015

NLPE

1

PI1016

NLPE2

PI1017

NLPE3

PI1028

NLPE4

PI1027

NLPE

5

PI1057

NLPF

0

PI1068

NLPF

1

PI1056

NLPF2

PI1069

NLPF3

PI1055

NLPF

4

PI1054

NLPG2

PI1070

NLPG3

PI1071

NLPG4

PI1053

NLPG5

PI1052

NLPG6

PI1072

NLPG7

PI1010

NLPH0

PI109

NLPH1

PI1065

NLPH2

PI1064

NLPH3

PI1032

NLPJ0

PI1011

NLPJ1

PI1076

NLPK4

PI1049

NLPK5

PI1075

NLPK6

PI1050

NLPK7

PI1077

NLPL5

PI101

NLPL6

PI1044

NLPL7

PI1046

NLPM

4

PI1047

NLPM

5

PI1048

NLPM

6

PI1074

NLPM

7

PI1035

NLPN0

PI1034

NLPN1

PI106

NLPN2

PI107

NLPN3

PI1033

NLPN4

PI108

NLPN5

PI1031

NLPP0

PI1012

NLPP1

PI104

NLPP3

PI1036

NLPP4

PI105

NLPP5

PI1018

NLPQ2

PI1013

NLPQ3

PI1037

NLPQ4

PI1051

NLPQ5

PI1073

NLPQ6

56 Dodatek A Schematy modułoacutew urządzenia

Dodatek B

Wykresy pobranych trajektorii

Dodatek B Wykresy pobranych trajektorii 57

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B1 Trajektoria ucząca z minimalnym zastosowaniem filtru

58 Dodatek B Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-13

Shee

t o

fFi

le

CU

sers

P

ower

Supp

lyS

chD

ocD

raw

n B

y

5V GN

D

GN

D

3V3

GN

DS

GN

DS

3V3 R

pu10

k 16V

10nF

C21

GN

DS

Rs

470

TAC

TD60

B1

GN

DS

16V

10uF

Cin

R20

510k

Rpg

499k

R21

162k

16V

22uF

Cou

t

16V

4uF

Cld

o

3V3

GN

DS

GN

DS

16V

12pF

Cx2

16V

12pF

Cx1

GN

DS

3V3

1

GN

D2

GN

D6

GN

D10

GN

D14

GN

D18

3V3

27

GN

D28

GN

D32

GN

D36

GN

D40

GN

D44

3V3

51

GN

D52

Pow

er p

ins

3V3

101

GN

D10

2

GN

D10

8

GN

D11

4

GN

D11

8

GN

D12

4

GN

D12

8

GN

D13

4G

ND

140

GN

D14

4G

ND

150

3V3

155

GN

D15

6

C Any

bus_

CC

_B40

3V3

GN

DS

GN

DG

ND

S

WA

KE_

N64

HIB

_N65

XO

SC0

66

XO

SC1

67

RST

_N70

OSC

088

OSC

189

U1C

TM4C

1292

NC

PDTI

3R

VD

D7

VD

DA

8

VR

EFA

+9

GN

DA

10

VD

D16

GN

D17

VD

D26

VD

D28

VD

D39

VD

D47

GN

D48

VD

D63

VB

AT

68

VD

D69

VD

D79

GN

D80

VD

DC

87

VD

D90

VD

D10

1

VD

D11

3

GN

D11

4

VD

DC

115

VD

D12

2

U1D

TM4C

1292

NC

PDTI

3R

RST

GN

DX

18M

Hz

GN

D

EN1

PG2

VIN

3

VIN

4G

ND

5G

ND

6FB

7V

OU

T8

TPS8

2085

GN

D9

U2

TPS8

2085

GN

D

R66

500k

2kV

1nF

C55 PE

GN

DR

5550

0k2k

V1n

F

C44 PE

GN

D

R77

500k

2kV

1nF

C66 PE

GN

D

16V

100n

F

CV

116

V10

0nF

CV

216

V10

0nF

CV

316

V10

0nF

CV

4

3V3

16V

100n

F

CV

516

V10

0nF

CV

616

V10

0nF

CV

716

V10

0nF

CV

816

V10

0nF

CV

916

V10

0nF

CV

1016

V10

0nF

CV

1116

V10

0nF

CV

1216

V10

0nF

CV

13

GN

D

16V

1uF

CV

ref1

16V

10nF

CV

ref2

GN

DG

ND Po

wer

supp

ly

1

4

4 B

arto

sz K

aczo

r

PI01

PI02

PI06

PI010

PI014

PI018

PI027

PI028

PI032

PI036

PI040

PI044

PI051

PI052

PI0101

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0155

PI0156

COC

PIB101

PIB102

PIB103 PI

B104 COB1

PIC2101 PIC2102

COC2

1

PIC4401 PIC4402

COC44

PIC5501 PIC5502

COC55

PIC6601 PIC6602

COC66

PICin01 PICin02

COCin

PICldo01 PICldo02

COCl

do

PICout01 PICout02

COCo

ut

PICV101 PICV102

COCV1

PICV201 PICV202

COCV

2 PICV301 PICV302

COCV

3 PICV401 PICV402

COCV

4 PICV501 PICV502

COCV5

PICV601 PICV602

COCV

6 PICV701 PICV702

COCV

7 PICV801 PICV802

COCV

8 PICV901 PICV902

COCV

9 PICV1001 PICV1002

COCV10

PICV1101 PICV1102

COCV11

PICV1201 PICV1202

COCV12

PICV1301 PICV1302

COCV13

PICVref101 PICVref102

COCV

ref1

PICVref201 PICVref202

COCVref2

PICx101 PICx102

COCx1

PICx201 PICx202

COCx2

PIR2001 PIR2002 COR20

PIR2

101

PIR2

102 COR

21

PIR5501 PIR5502 COR55

PIR6601 PIR6602 COR

66

PIR7701 PIR7702 CO

R77

PIRpg01 PIRpg02 CO

Rpg

PIRpu01 PIRpu02 CORpu

PIRs01

PIRs

02

CORs

PIU1064

PIU1065

PIU1066

PIU1067

PIU1070

PIU1088

PIU1089

COU1

C

PIU107

PIU108

PIU109

PIU1010

PIU1016

PIU1017

PIU1026

PIU1028

PIU1039

PIU1047

PIU1048

PIU1063

PIU1068

PIU1069

PIU1079

PIU1080

PIU1087

PIU1090

PIU10101

PIU10113

PIU10114

PIU10115

PIU10122

COU1

D

PIU201

PIU202

PIU203

PIU204

PIU205

PIU206

PIU207

PIU208

PIU209

COU2

PIX101 PIX102 PIX10GND

PIX10GND2 COX

1

PI01

PI027

PI051

PI0101

PI0155

PICout01

PICV101 PICV201

PICV301 PICV401

PICV501 PICV601

PICV701 PICV801

PICV901 PICV1001

PICV1101 PICV1201

PICV1301

PICVref101 PICVref201

PIR2002 PIRpg02

PIRpu02

PIU107

PIU108

PIU109

PIU1016

PIU1026

PIU1028

PIU1039

PIU1047

PIU1063

PIU1068

PIU1069

PIU1079

PIU1090

PIU10101

PIU10113

PIU10122

PIU208

PICin01

PIU201

PIU203

PIU204

PI02

PI06

PI010

PI014

PI018

PI028

PI032

PI036

PI040

PI044

PI052

PI0102

PI0108

PI0114

PI0118

PI0124

PI0128

PI0134

PI0140

PI0144

PI0150

PI0156

PIB101

PIB102

PIC2102

PIC4401 PIC5501

PIC6601

PICin02

PICldo02

PICout02

PICV102 PICV202

PICV302 PICV402

PICV502 PICV602

PICV702 PICV802

PICV902 PICV1002

PICV1102 PICV1202

PICV1302

PICVref102 PICVref202

PICx102 PICx202

PIR2

101

PIR5502 PIR6602

PIR7702

PIU1010

PIU1017

PIU1048

PIU1064

PIU1080

PIU1088

PIU10114

PIU205

PIU206

PIU209

PIX10GND

PIX10GND2

PIB103 PI

B104 PIRs01

PICldo01

PIU1087

PIU10115

PICx101

PIU1067

PIX101

PICx201

PIU1066

PIX102

PIR2001 PIR2

102

PIU207

PIRpg01

PIU202

PIU1065

PIU1089

PIC4402 PIC5502

PIC6602

PIR5501 PIR6601

PIR7701

PIC2101 PIRpu01 PI

Rs02

PIU1070

NLRST

Dodatek A Schematy modułoacutew urządzenia 55

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-15

Shee

t o

fFi

le

CU

sers

IM

UO

ldS

chD

ocD

raw

n B

y

8642 16141210 18 20 22 24

17 19 21 239 11 13 157531J5 H

eade

r 12x

2

5V

GN

D

5V

16V

100n

F

C1

16V

100n

F

C2

GN

DRS

TCSD

OU

TD

IO4

DIO

3SC

LKD

IND

IO1

DIO

2

PA2

PA3

PA4

PA5

PB1

PB2

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

1 23 4

J2

5V

GN

D

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIJ201

PIJ202

PIJ203

PIJ204

COJ2

PIJ501

PIJ502

PIJ503

PIJ504

PIJ505

PIJ506

PIJ507

PIJ508

PIJ509

PIJ5

010

PIJ5011

PIJ5012

PIJ5013

PIJ5014

PIJ5

015

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

COJ5

PI1041

PI1080

PI1059

PI1060

PI1061

PI1062

PIC101 PIC201

PIJ201

PIJ5

010

PIJ5011

PIJ5012

PI1066

PIJ506

NLCS

NLPA3

PI1067

PIJ505

NLDIN

NLPA4

PIJ507

NLDI

O1 PIJ509

NLDI

O2 PIJ501

NLDI

O3

PI1045

PIJ502

NLDI

O4

NLPB2

PI1058

PIJ504

NLDOUT

NLPA5

PI1042

PI1043

PI1078

PI1079

PIC102 PIC202

PIJ204

PIJ5013

PIJ5014

PIJ5

015

PIJ202

PIJ203

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

PI1063

PIJ503

NLPA2

NLSCLK

PI102

PIJ508

NLPB1

NLRS

T

PI1029

NLPB4

PI1030

NLPB5

PI103

NLPC0

PI1038

NLPC1

PI1039

NLPC2

PI1040

NLPC3

PI1019

NLPD0

PI1022

NLPD1

PI1021

NLPD2

PI1020

NLPD3

PI1026

NLPD4

PI1025

NLPD5

PI1024

NLPD6

PI1023

NLPD7

PI1014

NLPE0

PI1015

NLPE

1

PI1016

NLPE2

PI1017

NLPE3

PI1028

NLPE4

PI1027

NLPE

5

PI1057

NLPF

0

PI1068

NLPF

1

PI1056

NLPF2

PI1069

NLPF3

PI1055

NLPF

4

PI1054

NLPG2

PI1070

NLPG3

PI1071

NLPG4

PI1053

NLPG5

PI1052

NLPG6

PI1072

NLPG7

PI1010

NLPH0

PI109

NLPH1

PI1065

NLPH2

PI1064

NLPH3

PI1032

NLPJ0

PI1011

NLPJ1

PI1076

NLPK4

PI1049

NLPK5

PI1075

NLPK6

PI1050

NLPK7

PI1077

NLPL5

PI101

NLPL6

PI1044

NLPL7

PI1046

NLPM

4

PI1047

NLPM

5

PI1048

NLPM

6

PI1074

NLPM

7

PI1035

NLPN0

PI1034

NLPN1

PI106

NLPN2

PI107

NLPN3

PI1033

NLPN4

PI108

NLPN5

PI1031

NLPP0

PI1012

NLPP1

PI104

NLPP3

PI1036

NLPP4

PI105

NLPP5

PI1018

NLPQ2

PI1013

NLPQ3

PI1037

NLPQ4

PI1051

NLPQ5

PI1073

NLPQ6

56 Dodatek A Schematy modułoacutew urządzenia

Dodatek B

Wykresy pobranych trajektorii

Dodatek B Wykresy pobranych trajektorii 57

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B1 Trajektoria ucząca z minimalnym zastosowaniem filtru

58 Dodatek B Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

11

22

33

44

DD

CC

BB

AA

Title

Num

ber

Rev

isio

nSi

ze A4

Dat

e20

16-0

6-15

Shee

t o

fFi

le

CU

sers

IM

UO

ldS

chD

ocD

raw

n B

y

8642 16141210 18 20 22 24

17 19 21 239 11 13 157531J5 H

eade

r 12x

2

5V

GN

D

5V

16V

100n

F

C1

16V

100n

F

C2

GN

DRS

TCSD

OU

TD

IO4

DIO

3SC

LKD

IND

IO1

DIO

2

PA2

PA3

PA4

PA5

PB1

PB2

GN

DG

ND

5V5V

PA2

PA3

PA4

PA5

PB1

PB2

PB4

PB5

PC0

PC1

PC2

PC3

PD0

PD1

PD2

PD3

PD4

PD5

PD6

PD7

PH0

PH1

PH2

PH3

PK4

PK5

PK6

PK7

PE0

PE1

PE2

PE3

PE4

PE5

PF0

PF1

PF2

PF3

PF4

PG2

PG3

PG4

PG5

PG6

PG7

PL5

PL6

PL7

PM4

PM5

PM6

PM7

PN0

PN1

PN2

PN3

PN4

PN5

PQ2

PQ3

PQ4

PQ5

PQ6

PP0

PP1

PP3

PP4

PP5

PJ0

PJ1

GN

DG

ND

3V3

3V3

2161

1959

2363

2666

2767

1858

22

545

2929

3030

33

3838

3939

4040

3676

949

3575

1050

3232

1111

242

141

55

3636

44

1212

3131

2464

2565

99

1010

2323

2424

2525

2626

2020

2121

2222

1919

3878

1A TE-F

ine-

Stac

k-40

-S_e

th

3979

1454

3070

3171

1353

1252

3272

1757

2868

1656

2969

1555

1414

1515

1616

1717

2828

2727

4080

343

3373

1151

3737

1313

1818

88

3333

77

66

3434

3535

3474

848

747

646

444

11

3777

2060

2262

1B TE-F

ine-

Stac

k-40

-S_e

th

5V5V

Any

bus

1A c

onsu

mpt

ion

1 23 4

J2

5V

GN

D

PI102

PI103

PI104

PI105

PI109

PI1010

PI1011

PI1012

PI1019

PI1020

PI1021

PI1022

PI1023

PI1024

PI1025

PI1026

PI1029

PI1030

PI1031

PI1032

PI1036

PI1038

PI1039

PI1040

PI1041

PI1042

PI1045

PI1049

PI1050

PI1058

PI1059

PI1061

PI1063

PI1064

PI1065

PI1066

PI1067

PI1075

PI1076

PI1078

CO1A

PI101

PI106

PI107

PI108

PI1013

PI1014

PI1015

PI1016

PI1017

PI1018

PI1027

PI1028

PI1033

PI1034

PI1035

PI1037

PI1043

PI1044

PI1046

PI1047

PI1048

PI1051

PI1052

PI1053

PI1054

PI1055

PI1056

PI1057

PI1060

PI1062

PI1068

PI1069

PI1070

PI1071

PI1072

PI1073

PI1074

PI1077

PI1079

PI1080

CO1B

PIC101 PIC102

COC1

PIC201 PIC202

COC2

PIJ201

PIJ202

PIJ203

PIJ204

COJ2

PIJ501

PIJ502

PIJ503

PIJ504

PIJ505

PIJ506

PIJ507

PIJ508

PIJ509

PIJ5

010

PIJ5011

PIJ5012

PIJ5013

PIJ5014

PIJ5

015

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

COJ5

PI1041

PI1080

PI1059

PI1060

PI1061

PI1062

PIC101 PIC201

PIJ201

PIJ5

010

PIJ5011

PIJ5012

PI1066

PIJ506

NLCS

NLPA3

PI1067

PIJ505

NLDIN

NLPA4

PIJ507

NLDI

O1 PIJ509

NLDI

O2 PIJ501

NLDI

O3

PI1045

PIJ502

NLDI

O4

NLPB2

PI1058

PIJ504

NLDOUT

NLPA5

PI1042

PI1043

PI1078

PI1079

PIC102 PIC202

PIJ204

PIJ5013

PIJ5014

PIJ5

015

PIJ202

PIJ203

PIJ5

016

PIJ5

017

PIJ5

018

PIJ5019

PIJ5020

PIJ5021

PIJ5022

PIJ5023

PIJ5024

PI1063

PIJ503

NLPA2

NLSCLK

PI102

PIJ508

NLPB1

NLRS

T

PI1029

NLPB4

PI1030

NLPB5

PI103

NLPC0

PI1038

NLPC1

PI1039

NLPC2

PI1040

NLPC3

PI1019

NLPD0

PI1022

NLPD1

PI1021

NLPD2

PI1020

NLPD3

PI1026

NLPD4

PI1025

NLPD5

PI1024

NLPD6

PI1023

NLPD7

PI1014

NLPE0

PI1015

NLPE

1

PI1016

NLPE2

PI1017

NLPE3

PI1028

NLPE4

PI1027

NLPE

5

PI1057

NLPF

0

PI1068

NLPF

1

PI1056

NLPF2

PI1069

NLPF3

PI1055

NLPF

4

PI1054

NLPG2

PI1070

NLPG3

PI1071

NLPG4

PI1053

NLPG5

PI1052

NLPG6

PI1072

NLPG7

PI1010

NLPH0

PI109

NLPH1

PI1065

NLPH2

PI1064

NLPH3

PI1032

NLPJ0

PI1011

NLPJ1

PI1076

NLPK4

PI1049

NLPK5

PI1075

NLPK6

PI1050

NLPK7

PI1077

NLPL5

PI101

NLPL6

PI1044

NLPL7

PI1046

NLPM

4

PI1047

NLPM

5

PI1048

NLPM

6

PI1074

NLPM

7

PI1035

NLPN0

PI1034

NLPN1

PI106

NLPN2

PI107

NLPN3

PI1033

NLPN4

PI108

NLPN5

PI1031

NLPP0

PI1012

NLPP1

PI104

NLPP3

PI1036

NLPP4

PI105

NLPP5

PI1018

NLPQ2

PI1013

NLPQ3

PI1037

NLPQ4

PI1051

NLPQ5

PI1073

NLPQ6

56 Dodatek A Schematy modułoacutew urządzenia

Dodatek B

Wykresy pobranych trajektorii

Dodatek B Wykresy pobranych trajektorii 57

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B1 Trajektoria ucząca z minimalnym zastosowaniem filtru

58 Dodatek B Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

Dodatek B

Wykresy pobranych trajektorii

Dodatek B Wykresy pobranych trajektorii 57

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B1 Trajektoria ucząca z minimalnym zastosowaniem filtru

58 Dodatek B Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B1 Trajektoria ucząca z minimalnym zastosowaniem filtru

58 Dodatek B Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B2 Trajektoria ucząca z zastosowaniem filtru o średnim ustawieniu

Dodatek B Wykresy pobranych trajektorii 59

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(a) Siła zmieżona w osi x

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(b) przyspieszenie zmieżonew osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(c) Siła skorygowana w osi x

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(d) Siła zmieżona w osi y

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(e) przyspieszenie zmieżonew osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(f) Siła skorygowana w osi y

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(g) Siła zmieżona w osi z

czas (ns) 1010

0 1 2 3 4 5 6

Prz

yspi

esze

nie

(ms

2)

-20

-15

-10

-5

0

5

10

15

20

(h) przyspieszenie zmieżonew osi z

czas (ns) 1010

0 1 2 3 4 5 6-20

-15

-10

-5

0

5

10

15

20

(i) Siła skorygowana w osi z

Rysunek B3 Trajektoria weryfikująca z minimalnym zastosowaniem filtru

60 Dodatek B Wykresy pobranych trajektorii

• Wstep
• • Zalozenia i problematyka pracy
  • Uklad Pracy
  • • Wstep teoretyczny przeglad dostepnych rozwiazan oraz mozliwe zastosowania ukladu
    • • Definicje
      • Dzialanie jednostki inercyjnej
      • • Akcelerometry MEMS
        • Zyroskopy MEMS
        • Zyroskopy swiatlowodowe
        • • Dzialanie magistrali Ethercat
          • • COE
            • Synchronizacja zegara
            • • Jednostki inercyjne ze zlaczami I2C oraz SPI
              • Jednostki inercyjne ze zlaczami przemyslowymi
              • Mozliwe zastosowania proponowanego ukladu
              • • Nawigacja Robota Mobilnego o napedzie roacuteznicowym
                • Korekcja odczytu sily z czujnika nadgarstkowego
                • • Opis budowanego ukladu
                  • • Opis Ukladu
                    • • Modul gloacutewny
                      • Modul jednostki inercyjnej (IMU)
                      • Anybus M40 EtherCAT
                      • • Opis zlacz
                        • • Zlacze miedzyplytkowe
                          • Zlacze zasilania
                          • Zlacze interfejsu EtherCAT
                          • Zlacze Jtag
                          • Zlacze Anybus B40-CPU
                          • • Opis oprogramowania
                            • • Protokoly komunikacyjne
                              • • Standard EtherCAT
                                • Can over EtherCAT
                                • • Oprogramowanie MCU
                                  • • Konfiguracja peryferioacutew
                                    • Sterownik Anybus
                                    • Przerwania
                                    • • Sterownik w strukturze OROCOS
                                      • • Badania weryfikacyjne
                                        • • Opis stanowiska eksperymentalnego
                                          • Projekt eksperymentoacutew
                                          • Wyniki eksperymentoacutew
                                          • Podsumowanie
                                          • • Schematy moduloacutew urzadzenia
                                            • Wykresy pobranych trajektorii