Projekt układu AHRS dla małego quadrotora - KoNaR
Transkrypt
Projekt układu AHRS dla małego quadrotora - KoNaR
POLITECHNIKA WROCŁAWSKA WYDZIAŁ ELEKTRONIKI Kierunek: Specjalność: Automatyka i Robotyka (AIR) Robotyka (ARR) PROJEKT INŻYNIERSKI Projekt układu AHRS dla małego quadrotora AHRS system design for small quadrotor Autor: Jacek Drewniak Prowadzący projekt: dr inż. Janusz Jakubiak, I-6 Ocena projektu: WROCŁAW 2013 Spis treści 1 Wstęp 1.1 Multirotory . . . . . 1.2 AHRS . . . . . . . . 1.3 Założenia projektowe 1.4 Projekt . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 Czujniki 2.1 Żyroskop . . . . . . 2.2 Akcelerometr . . . 2.3 Magnetometr . . . 2.4 Dodatkowe czujniki . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 . 9 . 10 . 11 . 12 . . . . . . . . . . . . 15 15 16 16 16 17 17 . . . . 3 Elektronika 3.1 Mikrokontroler . . . . . . . . 3.2 Radio . . . . . . . . . . . . . 3.3 Dodatkowe funkcje . . . . . . 3.3.1 Translator USB-UART 3.3.2 Zarządzanie baterią . . 3.3.3 Znaczniki . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 3 4 4 5 4 Algorytm 4.1 Reprezentacja orientacji . . . . . . . . . . . . . . . . . . . . . . 4.1.1 Kąty Eulera . . . . . . . . . . . . . . . . . . . . . . . . . 4.1.2 Kwaternion . . . . . . . . . . . . . . . . . . . . . . . . . 4.2 Wyznaczanie orientacji . . . . . . . . . . . . . . . . . . . . . . . 4.2.1 Na podstawie wektorów grawitacji i pola magnetycznego 4.2.2 Na podstawie prędkości kątowej . . . . . . . . . . . . . . 4.3 Filtr Madgwicka . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3.1 Konwersja kwaternionu do kątów Eulera . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19 19 19 19 21 21 23 23 25 5 Oprogramowanie 5.1 Oprogramowanie mikrokontrolera . . . . . 5.2 Oprogramowanie na PC . . . . . . . . . . 5.3 Komunikacja pomiędzy PC i quadrotorem 5.4 Narzędzia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 27 29 30 30 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6 Testy 35 6.1 Stabilność pomiarowa czujników . . . . . . . . . . . . . . . . . . . . . . . . 35 6.2 Dokładność systemu AHRS . . . . . . . . . . . . . . . . . . . . . . . . . . 39 2 SPIS TREŚCI 7 Wnioski 47 A Schematy 49 B PCB 55 C Zestawienie czujników 59 D CD 61 Bibliografia 61 Rozdział 1 Wstęp 1.1 Multirotory Multirotory (multikoptery) są szczególną odmianą wiropłatów, czyli statków powietrznych, które siłę nośną potrzebną do lotu wytwarzają za pomocą wirujących powierzchni nośnych. Najbardziej znanym przykładem wiropłatu jest helikopter. Cechą charakterystyczną multirotorów jest posiadanie więcej niż dwóch śmigieł. Od ich liczby bierze swoją nazwę typ multirotoru. Quadrotor (quadrokopter) posiada cztery śmigła, a hexrotor (hexakopter) – sześć. W porównaniu do rozwiązań jedno- lub dwuśmigłowych, multirotory posiadają następujące zalety. Pierwszą z nich jest większa stabilność lotu, spowodowana symetrycznym rozmieszczeniem śmigieł w pewnej odległości od środka statku. Dla łatwego zobrazowania tego zjawiska możemy posłużyć się analogią: obiekty na ziemi posiadające jedno podparcie łatwiej przewrócić niż te, które posiadają więcej nóg i mają je umieszczone dalej od środka masy. W przypadku multirotorów z parzystą liczbą śmigieł upraszcza się konstrukcja mechaniczna statku. Wirniki są mocowane na stałe i nie muszą mieć możliwości wychylania osi obrotu ani zmiany kąta natarcia śmigieł. Kolejną ich zaletą jest łatwiejsze sterowanie multirotorem, które jest ściśle związane z konstrukcją mechaniczną. Zmianę wysokości, obrót i wychylenie uzyskuje się za pomocą sterowania tylko ciągiem silników. Wadą stosowania większej ilości śmigieł jest wzrost kosztu budowy statku. Zwiększa się również prawdopodobieństwo awarii jednego z napędów. Zwykle powoduje to unieruchomienie całej maszyny, co z kolei ponownie podnosi koszty utrzymania. Rozproszony system napędowy może znacznie utrudnić i ograniczyć swobodę w projektowaniu elektroniki multirotora. Jak już wspomniano powyżej, system sterowania multirotorów od strony algorytmu oraz mechaniki, jest zazwyczaj prostszy niż w przypadku innych rozwiązań. Jest to szczególnie istotne w przypadku małych statków powietrznych. Pomimo większej ilości motorów roboczych napędzających wirniki, konstrukcja mechaniczna jest mniej skomplikowana, a całkowita liczba napędów – mniejsza niż w przypadku helikoptera. Wyeliminowane zostają dodatkowe dźwignie bądź serwonapędy, potrzebne do sterowania łopatami wirnika. W dużym uproszczeniu trzy podstawowe typy zachowań można uzyskać według następującego algorytmu: • wznoszenie i opadanie przez jednoczesne zwiększenie lub zmniejszenie ciągu wszystkich silników 4 1. Wstęp • obrót – w wyniku zwiększenia różnicy ciągu pomiędzy wirnikami obracającymi się w przeciwnych kierunkach • pochylenie – poprzez zwiększenie różnicy ciągu pomiędzy wirnikami umieszczonymi po przeciwnych stronach statku 1.2 AHRS Wiedza na temat położenia i orientacji w przestrzeni gra jedną z głównych ról w wielu aspektach techniki, takich jak: robotyka, awionika, nawigacja, kosmonautyka, czyli wszędzie tam, gdzie trudno o stały punkt odniesienia, a jego obserwacja czy określenie jest kosztowne lub niemożliwe. Całkowitą niezależność i samowystarczalność dostarczają czujniki inercyjne, niestety za cenę dokładności. AHRS (Attitude and Heading Reference System) jest systemem służącym do określania orientacji w przestrzeni. Składają się na niego trzy typy czujników: żyroskop, akcelerometr oraz magnetometr. Teoretycznie posiadając idealny czujnik oraz znając początkową orientację można wyznaczyć orientację bieżącą za pomocą samego żyroskopu. W praktyce żyroskopy posiadają duży dryf, zwłaszcza egzemplarze stosowane w rozwiązaniach konsumenckich. Potrzebne jest więc wykorzystanie wyniku innych pomiarów. W klasycznych rozwiązaniach dodatkowo stosuje się akcelerometr, coraz częściej wraz z magnetometrem. Mierzą one ziemskie pole grawitacyjne i magnetyczne, dostarczając stałego odniesienia. Jednak i te czujniki posiadają wady. Pierwszy wrażliwy jest na przyspieszenia wynikające z poruszania się urządzenia oraz często posiada stosunkowo duży szum. W drugim przypadku problemem jest niejednorodność pola magnetycznego i nierówne rozmieszczenie jego linii nad powierzchnią ziemi oraz lokalne fluktuacje wynikające z zakłóceń powodowanych przez urządzenia elektryczne. Przeprowadzenie skutecznej fuzji pomiarów z wymienionych czujników jest więc zadaniem kluczowym i wraz z właściwościami samych elementów elektronicznych stanowi o dokładności wyznaczonej orientacji, a tym samym – jakości systemu AHRS. 1.3 Założenia projektowe Celem tego projektu jest zbudowanie kompletnego systemu AHRS dla małego quadrotora. Wynikają z tego następujące wymagania oraz ograniczenia: • System musi zajmować jak najmniejszą powierzchnię oraz jak najmniej ważyć. • Musi minimalizować ilość zużywanej energii elektrycznej. • Nie może wymagać dużej mocy obliczeniowej, tj. obliczenia powinny być wykonywane na mikrokontrolerze umieszczonym na statku. • Powinien być przystosowany do współpracy z quadrotorem, tzn. dostarczyć orientację: – w formie łatwej do adaptacji w sterowniku stabilizacji lotu – ze względu na małą inercję quadrotora – z częstotliwością co najmniej 100 Hz – stabilną i dokładną, szczególnie dla małego wychyłu 1.4. Projekt 5 Rysunek 1.1 Schemat funkcyjny. 1.4 Projekt System AHRS wykonany w ramach tej pracy jest częścią kompletnego projektu układu sterowania quadrotora przedstawionego na rysunku 1.3 i 1.4. Na rysunkach 1.1 i 1.2 przedstawione są jego schematy: funkcyjny i ideowy. Na podstawie danych z tego systemu sterownik stabilizuje lot quadrotora. Jest to więc obok sterownika najważniejszy składnik multirotora, bez którego nie wzniesie się on w powietrze. Najprostsze konstrukcje mogą zawierać tylko te dwa elementy. Konstrukcja składa się z jednej płytki drukowanej, która jest także całym szkieletem mechanicznym. Ma kształt kwadratu o wymiarach 4 cm x 4 cm z ramionami o długości 4 cm, na których znajdują się otwory do mocowania silników. Cały model waży 25 g plus waga baterii. Płytka mieści się w pudełku o wymiarach 8 cm x 8 cm x 4 cm, a wraz ze śmigłami 8 cm x 15 cm x 4 cm. Wszystkie elementy elektroniczne wykonane są w technologii SMD. Akcelerometr, żyroskop i magnetometr to scalone, cyfrowe czujniki MEMS1 . Napędy w quadrotorze to miniaturowe silniki BLDC o wadze 2 g każdy. Mogą być zasilane maksymalnym napięciem wynoszącym 4,2 V, a w szczycie pobierają 2 A prądu. Śmigła mają wymiary 30202 i wykonane są z plastiku. Sterowniki silników to TGY 3A, zasilane maksymalnym napięciem 4,2 V i wytrzymujące natężenie prądu o wartości 3 A. Nadają się więc idealnie do współpracy z wyżej wymienionymi silnikami. Sterowniki są przylutowane, a napędy przyklejone do PCB za pomocą kleju cyjanoakrylowego w żelu. 1 2 Micro Electro-Mechanical Systems W notacji modelarskiej oznacza 3 cale szerokości i 2 cale skoku śmigła. 6 1. Wstęp Rysunek 1.2 Schemat ideowy. Rysunek 1.3 Quadrotor. 1.4. Projekt 7 Rysunek 1.4 Quadrotor przyrównany do linijki. Rozdział 2 Czujniki 2.1 Żyroskop Żyroskopy mikro-elektromechaniczne [9, 10, 12] działają w oparciu o efekt Coriolisa. Występuje on w obracających się układach i objawia zakrzywieniem toru ciał, które się w nim poruszają. Zakrzywienie tego toru zdaje się być spowodowane pozorną siłą, którą można zmierzyć tylko wewnątrz tego układu, ponieważ dla zewnętrznego obserwatora tor poruszania się ciała nie jest zakrzywiony. Siła ta, zwana siłą Coriolisa, jest wprost proporcjonalna do prędkości obrotowej. Wewnątrz czujnika na płytce krzemowej wytrawione są bloki krzemu z dołączonymi elektrodami, będącymi jedną z okładek kondensatora. Masy te, połączone w pary, poruszają się w przeciwnych kierunkach za pomocą efektu piezoelektrycznego. Pod wpływem obrotu siła Coriolisa zakrzywia tor bloków w przeciwnych kierunkach, zbliżając lub oddalając okładki od siebie. Gdyby na żyroskop zadziałała inna siła wywołana przyspieszeniem całego układu, tor bloków zostałby zakrzywiony w tym samym kierunku, kompensując błąd. Bezpośrednio prędkość obrotowa wpływa więc na pojemność wbudowanego kondensatora. Ładowane ciągle tym samym prądem, pod wpływem obrotu, posiadają różne napięcia na okładkach. Napięcie to, po odfiltrowaniu składowej zmiennej wynikającej z cyklicznie poruszających się mas, można zmierzyć za pomocą przetwornika analogowo-cyfrowego. Cyfrowe żyroskopy posiadają już wbudowane takie przetworniki wraz z filtrami. Wynik przechowywany w jednym z rejestrów można odczytać za pomocą jednego z cyfrowych protokołów transmisji. Żyroskop posiada wiele parametrów, między innymi zakres i czułość pomiarową. Jednak najistotniejszym parametrem ze względu na dokładność w zastosowaniu dla tego projektu, jest dryf żyroskopu. Parametr ten mówi nam o tym, że w pewnym zakresie możemy spodziewać się niezerowego pomiaru, pomimo zerowej prędkości. Dryf wynika najczęściej z naprężeń powstałych w procesie produkcji oraz podczas montowania w urządzeniu docelowym. Można w jakimś stopniu zniwelować jego wpływ podczas kalibracji, jednak nie całkowicie. Dlatego żyroskopy MEMS nie mogą być samodzielnie wykorzystywane do wyznaczenia orientacji. W tym projekcie wybrany został żyroskop mikro-elektromechaniczny L3G4200D produkowany przez firmę ST Microelectronics. Żyroskop ten posiada dryf na poziomie ± 10 dps1 dla 250 dps/s i ± 15 dps dla 500 dps/s oraz czułość do 8,75 mdps przy 16 bitowej reprezentacji danych. W dodatku C przedstawione są jego ważniejsze parametry. Zestawienie to zawiera tylko żyroskopy MEMS w niedużej obudowie. Wyboru dokonano na podstawie 1 degrees per second 10 2. Czujniki dryfu jaki posiadają oraz możliwości współpracy z całym systemem (oferowane protokoły transmisji, zakres napięć pracy). W quadrotorze żyroskop połączony jest do mikrokontrolera za pomocą magistrali SPI. Dla poprawy wyników stabilizowane zasilanie jest filtrowane za pomocą dwóch filtrów dolnoprzepustowych. Dodatkowy filtr umieszczony jest blisko pinów samego czujnika, są to dwa kondensatory 100 nF i 2,2 uF pomiędzy linią VCC i GND. Żyroskop znajduje się na jednej płytce razem z innymi elementami, między innymi stabilizatorem i układem ładowania baterii. Podgrzewają one stopniowo układ, zwiększając dryf żyroskopu. Po włączeniu silników strumień powietrza chłodzi całość, przeciwdziałając problemowi. Zazwyczaj w tego typu zagadnieniu działa się w inny sposób: umieszcza się na płytce rezystor mocy, który ma za zadanie kontrolowane podgrzewanie całości. Wraz z odpowiednim sterowaniem otrzymuje się układ regulacji temperatury, za pomocą którego utrzymuje się jej stałą wartość. Wiadomo wtedy, że pomiary odbywają się w niezmiennych warunkach. Wybrano zakres pomiarowy od -500 do 500 stopni na sekundę, ponieważ nie przewiduje się większych prędkości kątowych. Wykorzystywane jest tylko 10 najbardziej znaczących bitów spośród 16 dostępnych, by zminimalizować ten obszar pomiarowy, który nie jest stabilny ze względu na dryf. 2.2 Akcelerometr Akcelerometr mikro-elektromechaniczny działa podobnie do żyroskopu. Gdy na strukturę (składającą się z bloku masy wraz ze sprężyną i dołączonymi okładkami kondensatora wytrawionymi w krzemie) działa siła, powoduje ona zmianę pojemności, a wraz z tym zmianę mierzonego napięcia. Napięcie jest wprost proporcjonalne do działającej siły, a tym samym przyspieszenia. Największym problemem akcelerometrów MEMS jest szum temperaturowy, który przy tak małych wielkościach elementów roboczych ma znaczący wpływ na wynik pomiaru. Ma on zazwyczaj wartość na poziomie kilkudziesięciu mg 2 . Innym problemem jest wpływ przyspieszeń wynikających z poruszania się samego urządzenia. Wielkości te należy odfiltrować poprzez odpowiedni algorytm fuzji sygnałów. W tym projekcie został wybrany akcelerometr mikro-elektromechaniczny BMA180 firmy Bosch. W dodatku C przedstawione są jego ważniejsze parametry. Wyboru dokonano na podstawie szumu przy zerowym przyspieszeniu. Przy odpowiedniej kalibracji szum ten może wynosić do 5 mg (50 mg bez kalibracji). Czułość dochodzi do 1/8192 g przy zakresie ± 1 g ( 1/4096 przy ± 2 g) W quadrotorze akcelerometr podłączony jest do mikrokontrolera na tej samej magistrali SPI co żyroskop. Zasilanie dostarczone jest również w taki sam sposób. Wybrano zakres pomiarowy ± 2 g. Wykorzystanych jest 10 najbardziej znaczących bitów. By zminimalizować szum pomiarowy w danym czujniku można zastosować dwa ulepszenia. Po pierwsze akcelerometr należy podpiąć osobno do jednej magistrali SPI oraz zapewnić synchronizację sekwencji odczytywania wyników pomiarów z konwersją wewnętrznego przetwornika analogowo-cyfrowego. Ma to na celu: • zapewnienie stałego przesunięcia fazowego pomiędzy rzeczywistym przyspieszeniem i jego wartością zapisaną cyfrowo 2 Wielkość g oznacza wartość normalnego przyspieszenie ziemskiego wynoszącego 9.81 oznacza więc tysiączną część tej wartości. m s2 . Symbol mg 2.3. Magnetometr 11 • eliminację szumów spowodowanych aktywnością na magistrali Po drugie BMA180 oferuje możliwość dodatkowej korekcji czujnika za pomocą zaimplementowanego algorytmu nazwanego w dokumentacji jako offset fine-tuning. Kalibracji tej dokonuje się w celu zredukowania szumów pomiarowych. Przeprowadza się ją dla każdej z osi osobno, gdy znajduje prostopadła do wektora przyspieszenia ziemskiego. Mierzona wartość powinna wynosić 0 g. Wyniki tej operacji przechowywane są w specjalnym rejestrze fine_offset_*. 2.3 Magnetometr Magnetometr mikro-elektromechaniczny działa na zasadzie pomiaru siły Lorentza. W mikrostrukturze wytrawione są przewodniki, przez które płynie prąd o znanym natężeniu. Połączone są elastycznie z resztą czujnika i dołączonymi okładkami kondensatora. Gdy taki przewodnik zostanie wystawiony na działanie pola magnetycznego, to iloczyn wektorowy pola elektrycznego i magnetycznego daje w wyniku wektor siły Lorentza, która odchyla przewodnik, zmieniając pojemność dołączonego kondensatora. Dużym problemem magnetometrów jest ogromny wpływ innych elementów elektronicznych lub przewodników na płytce drukowanej bądź w pobliżu urządzenia. Każdy magnetometr należy skalibrować po zamontowaniu w urządzeniu. Należy odczytać i wyeliminować składową stałą natężenia pola magnetycznego we wszystkich osiach, przesuwając późniejsze wyniki pomiaru o obliczony offset. W wymagających aplikacjach należy powtórzyć tą operację z każdą zmianą środowiska pracy, a w szczególnych przypadkach, z każdym włączeniem urządzenia. Innym problemem jest udar pola magnetycznego. Czujnik wystawiony na pole o zbyt dużym natężeniu przestaje pracować poprawnie. Sprawa ta jest istotna, nawet gdy nie planujemy użytkowania w tak ekstremalnych środowiskach. W tym przypadku, podczas włączania quadrotora, ładunek rozchodzący się po PCB w bardzo krótkim czasie generuje duży impuls natężenia prądu. Ten z kolei generuje duże pole magnetyczne, które mimo krótkiego okresu trwania, powoduje udar czujnika. Objawia się on nieprawidłowymi wartościami pomiarowymi, które przyjmują maksymalną lub minimalną możliwą wartość w osi zgodnej z wektorem pola. W tym projekcie został wybrany magnetometr mikro-elektromechaniczny MAG3310 firmy Freescale. W dodatku C przedstawione są jego ważniejsze parametry. Wyboru dokonano na podstawie zakresu i czułości magnetometru tak, by, zakres pokrywał się z wartością natężenia magnetycznego pola ziemskiego oraz, by rozdzielczość orientacji wnioskowana na podstawie pomiaru była jak największa. W przypadku tego czujnika wynosi ona ok. 2◦ . W quadrotorze magnetometr podłączony jest do mikrokontrolera za pomocą magistrali 2 I C. Zasilanie dostarczone jest w ten sam sposób jak dla w/w czujników. Problem udaru magnetycznego, podczas włączania całego urządzenia, rozwiązano w następujący sposób. Czujnik ten posiada rejestr CTRL_REG2, z polem bitowym Mag_RST. Po aktywowaniu tego pola, magnetometr przechodzi w tryb resetu, podczas którego zostaje przywrócona prawidłowa praca czujnika, gdyby został wystawiony na działanie pola magnetycznego, którego natężenie przekracza jego zakres pomiarowy, lecz jest mniejsze niż maksymalne dopuszczalne. Wykorzystując tę możliwość, każdorazowo po uruchomieniu magnetometr jest resetowany za pomocą tego bitu. Kwestia wpływu innych urządzeń na pomiar została rozwiązana za pomocą odpowiedniej kalibracji. Proces ten, dla stałego offsetu wynikającego z oddziaływania pola genero- 12 2. Czujniki wanego przez sam quadrotor, został przeprowadzony dla każdej osi jednocześnie w sposób przedstawiony na rysunku 2.1, założywszy wcześniej, że wpływ ten charakteryzuje się stałym przesunięciem. Podczas kalibracji należy ręcznie zmieniać orientację quadrotora, by pokryć możliwie największą ich różnorodność. Po zakończeniu procesu wynik dla każdej z osi przesyłany jest do komputera. Procedura ta dla pewności została powtórzona dziesięć razy. Uśrednione rezultaty zostały wpisane jako stałe w programie i są dodawana do każdego pomiaru. W ten sposób uzyskano pomiar równy zeru, gdy wektor ziemskiego pola magnetycznego jest prostopadły do osi pomiarowej. 2.4 Dodatkowe czujniki Na pokładzie quadrotora zostały umieszczone dodatkowe czujniki, które wspomagają sterownik w podjęciu właściwej decyzji. Na podstawie pomiaru ciśnienia przez barometr można z przybliżeniem ustalić wysokość na jaką statek wzniósł się. Za pomocą zamontowanego barometru BMP180 firmy Bosch teoretyczna dokładność jaką można osiągnąć to ok. 20 cm. Jest ona niewystarczająca podczas startu i lądowania, kiedy to względnie duża zmiana siły ciągu wywołuje dodatkową zmianę ciśnienia. By mieć pewność, że quadrotor uniósł się w powietrze oraz, że łagodnie wyląduje, dodane zostały dwa scalone, odbiciowe czujniki odległości VCNL4000 firmy Vishay. Umieszczone są na rogach płytki ze sterownikiem i skierowane w dół. Maksymalna odległość w jakiej są w stanie wykryć przeszkodę to ok. 20 cm, a najdokładniejszy pomiar następuje w odległości do 7 cm. Na podstawie wymienionych informacji można podjąć właściwą decyzję w tych ważnych etapach lotu. Prawie wszystkie czujniki (bez czujników odległości) umożliwiają pomiar temperatury. W każdym z nich pomiar ten służy do kompensacji błędu. W barometrze dodatkowo temperatura ta jest wykorzystywana w algorytmie wyznaczania bezwzględnej wysokości nad poziomem morza. Rozmieszczenie czujników na płytce quadrotora przedstawiają rysunki 2.2 i 2.3. 2.4. Dodatkowe czujniki Rysunek 2.1 Kalibracja magnetometru 13 14 2. Czujniki Rysunek 2.2 Rozmieszczenie czujników na górnej stronie PCB. Rysunek 2.3 Rozmieszczenie czujników na dolnej stronie PCB. Rozdział 3 Elektronika 3.1 Mikrokontroler Mikrokontrolerem wybranym do celów tej pracy został STM32F405RGT6 firmy ST Microelectronics. Posiada on rdzeń ARM Cortex-M4F zaprojektowany w 32 bitowej architekturze ARMv7E-M ze zredukowaną ilością instrukcji (RISC) Thumb/Thumb2. Rdzeń może być taktowany zegarem z częstotliwością do 168 MHz. W benchmarku Dhrystone otrzymał wynik 1.25 DMIPS/MHz. Przy maksymalnym obciążeniu i częstotliwości pracy pobiera 100 mA prądu. Koprocesor FPU dostarczony razem z rdzeniem przez firmę ARM jako opcjonalne rozszerzenie na krzemie, posiada możliwość sprzętowego przetwarzania liczb zmiennoprzecinkowych z pojedynczą precyzją zgodną z normą IEEE 754. Rozszerzenie to nosi nazwę FPv4-SP. Rdzeń posiada także wsparcie dla DSP. Po pierwsze mnożenie dwóch liczb 32 bitowych zajmuje jeden cykl, a dzielenie od dwóch do dwunastu cykli. Po drugie w jednym cyklu może zostać wykonana pojedyncza 32 bitowa instrukcja MAC, dwie 16 bitowe instrukcje MAC lub 8/16 bitowa instrukcja typu SIMD. Po trzecie sprzętowe wsparcie dla pierwiastkowania liczby typu float, które to jest wykorzystywane podczas obliczania orientacji quadrotora (instrukcja VSQRT). STM32F405RGT6 został wyposażony w 1 MB pamięci FLASH i 112 kB pamięci SRAM. W projekcie do tej pory zostało użyte 42 kB FLASH i 2 kB SRAM. Dostępne zasoby są więc wykorzystane w mniej niż 5%. Ze szczególnie istotnych w tym projekcie peryferiów mikrokontroler posiada 3 interfejsy SPI (z czego wykorzystywane są 2), 3 I2 C (wykorzystywane są 2 ), 4 UART (wykorzystywane są 2), do 17 czterokanałowych timerów (wykorzystywane są 4) i 12 bitowy przetwornik analogowo-cyfrowy (wykorzystywany jest 1 kanał). Interfejs debugowania SWD potrzebuje tylko dwóch linii sygnałowych oraz masy. Obudowa mikrokontrolera to LQFP64, o wymiarach 1 cm x 1 cm. STM32F405RGT6 został wybrany z powodu wyżej wymienionych zalet. Konkurencyjne mikrokontrolery o podobnej wielkości posiadają znacznie gorsze parametry. W tabeli 3.1 przedstawione jest porównanie ich maksymalnych możliwości do użytego mikrokontrolera. Przeciw wyborowi STM32F405RGT6 opowiadały się dwie sprawy. Pierwszą z nich jest cena, która średnio jest cztery razy większa niż mikrokontrolerów z rodziny F1, F0 oraz AVR. Drugą jest stosunkowo większe skomplikowanie obsługi, które jest ściśle związane z większymi możliwościami. 16 3. Elektronika Rysunek 3.1 Porównanie podstawowych parametrów wybranych mikrokontrolerów. 3.2 Radio Do komunikacji bezprzewodowej został wybrany moduł radiowy RFM73 firmy HopeRF. Jest to kopia modułu nRF24 produkowanego przez firmę Nordic Semiconductor. RFM73 nadaje w paśmie 2,4 GHz z modulacją QFPSK. Producent podaje również, że może nadawać z prędkością do 2 Mb/s i mocą do 5 dBm. Komunikacja odbywa się w trybie simplex z możliwością przełączenia się pomiędzy nadawaniem, a odbieraniem. Podstawowymi zaletami RFM73 są jego niewielkie gabaryty oraz atrakcyjnie niska cena. W tym miejscu zalety tego modułu się kończą, ponieważ okazał się on mierną kopią. Najcięższym zarzutem jest kiepskiej jakości dokumentacja, która napisana chaotycznie nie jest w pełni zgodna z rzeczywistym produktem. Jako przykłady można podać błędne oznaczenia rejestrów czy nieprawdziwą wartość czasu przełączania trybów nadawania/odbierania, która to od rzeczywistej wartości różni się o trzy rzędy wielkości! Zasięg również pozostawia wiele do życzenia przy mocy, z którą moduły rzekomo nadają. Tak ma się również i z prędkością przesyłania danych. Wyżej wymienione wady skreśliły całkowicie założony wcześniej model komunikacji pomiędzy komputerem pokładowym oraz PC. Model zakładał wymianę danych, w której komputer osobisty nadawał w trybie odpytywania. Za pomocą specjalnego jednobitowego kodu znajdującego się na początku każdej wiadomości mógł wysyłać lub żądać danych. Niestety przy rzeczywistych czasach przełączania trybu transmitera jest to niemożliwe. Obecnie, w zależności od potrzeby, dane są tylko odbierane lub wysyłane, co znacznie utrudnia dalsze prace. 3.3 3.3.1 Dodatkowe funkcje Translator USB-UART Na pokładzie quadrotora został umieszczony czip CP2102 firmy Silicon Labs. Jest to translator USB-UART, który zapewnia interfejs pomiędzy tymi magistralami. Gdy na PC zainstalowane są odpowiednie sterowniki, po podłączeniu urządzenia tworzy się wirtualny port COM, z którym komunikacja obywa się w klasyczny sposób. Umieszczenie czipu na płytce wprowadza kilka usprawnień. Po pierwsze nie jest potrzebny dodatkowy konwerter, który należy za każdym razem wpinać pomiędzy quadrotor i komputer. Po drugie, dzięki obecności na płytce złącza micro USB, została wyeliminowana możliwość błędnego podłączenia, a zwłaszcza nieprawidłowej polaryzacji zasilania. Po trzecie podatny na zakłócenia sygnał UART przesyłany jest na niewielką odległość po samej płytce, a nie przez zewnętrzne przewody. Sygnał do komputera płynie poprzez ekranowany kabel USB. Sam standard USB jest także odporniejszy na zakłócenia. Udało się osiągnąć niezaburzoną komunikację z prędkością 921600 bitów na sekundę. 3.3. Dodatkowe funkcje 3.3.2 17 Zarządzanie baterią Dla wygody użytkowania quadrotora, na płytce znajduje się układ do ładowania jednoogniwowych baterii litowo-polimerowych. Jest to MCP1825 firmy Microchip. Dodatkowo dokonywany jest pomiar napięcia baterii przez sam mikrokontroler za pomocą przetwornika analogowo-cyfrowego, podłączonego do dzielnika napięcia, składającego się z oporników 100 i 330 kΩ. Gdy napięcie na ogniwie spadnie poniżej niebezpiecznego dla żywotności baterii poziomu, odcinane jest sterowanie dla silników, a użytkownik zostaje powiadomiony o zaistniałym problemie. Planowane jest dodanie algorytmu, który analizuje sytuację w jakiej znajduje się quadrotor i zmusza go najpierw do bezpiecznego lądowania, a później odcięcia zasilania. 3.3.3 Znaczniki Na płytce quadrotora umieszczonych jest osiem diod. Cztery z nich umieszczone na górnej stronie statku służą powiadamianiu użytkownika o aktualnym stanie programu oraz pełnią funkcję systemu znaczników. W późniejszych pracach zostanie on wykorzystany do analizy położenia quadrotora za pomocą systemu wizyjnego, by móc między innymi zbadać dokładność nawigacji bezwładnościowej. Diody są tak umieszczone, by możliwe było jednoznaczne określenie orientacji i środka robota. Rozdział 4 Algorytm 4.1 Reprezentacja orientacji W systemie dostępne są dwie reprezentacje orientacji quadrotora względem wektora grawitacji i magnetycznej północy ziemi. Jedną z nich jest znormalizowany kwaternion i pojawia się on pośrednio w systemie. Na jego podstawie obliczane są kąty yaw, pitch i roll. 4.1.1 Kąty Eulera Kąty Eulera, są to trzy kąty opisujące orientację w przestrzeni euklidesowej [8]. Kąty te mogą być przedstawione w wieloraki sposób. W klasycznym podejściu są to trzy osie, z czego pierwsza i trzecia powtarzają się. Drugim typem formalizmu są kąty Tait–Bryana wprowadzone przez Peter Guthrie Tait’a i George H. Bryana. Trzy podstawowe obroty opisane są na trzech osobnych osiach. Notacja ta jest jedyną różnicą pomiędzy klasyczną, a alternatywną reprezentacją. Inną spotykaną nazwą dla tej drugiej grupy są kąty yaw, pitch i roll, w Polsce znane jako myszkowanie, kiwanie i kołysanie. Ich rozmieszczenie względem quadrotora przedstawione jest na rysunku 4.1. Zaletą reprezentacji yaw, pitch, roll jest łatwość jej interpretacji, gdyż jest ona bliska ludzkiej intuicji i tego powodu wykorzystywana w tym projekcie sterowniku stabilizacji lotu oraz przesyłana do komputera i przedstawiona na wykresach. Z założeń projektowych wynikają małe wychylenia quadrotora, problem osobliwości pojawiający się przy tego typu reprezentacji jest więc pomijany. 4.1.2 Kwaternion W matematyce kwaterniony są zbiorem liczb rozszerzającym zbiór liczb zespolonych [5]. W praktyce kwaternion posiada cztery składowe: w q̂ = x y (4.1) z Składowa w jest częścią rzeczywistą kwaternionu, a x, y, z są częściami urojonymi. Kwaternion można zinterpretować jako wektor w przestrzeni R(3) i kąt obrotu wokół tego wektora. Wtedy w jest częścią skalarną, a x, y, z częścią wektorową kwaternionu. Zobrazowane to zostało na rysunku 4.2. 20 4. Algorytm Rysunek 4.1 Rozmieszczenie osi yaw, pitch i roll. Rysunek 4.2 Interpretacja kwaternionu jako wektor i kąt obrotu. 4.2. Wyznaczanie orientacji 21 Rysunek 4.3 Orientacja względem wektora grawitacji w jednej z osi. Obrotu wektora ~v do pozycji wektora ~u można dokonać za pomocą znormalizowanego kwaternionu obrotu ~u~v q̂ . Obrót ten przedstawia się za pomocą równania: ~u = q̂~v q̂ ∗ (4.2) gdzie q̂ ∗ oznacza kwaternion sprzężony. Reprezentacja ta posiada ważną zaletę, którą jest obecność dodatkowego czwartego parametru. Dzięki temu nie występuje niepożądane zjawisko osobliwości reprezentacji. Jednak, bez dobrej znajomości tematu, reprezentacja kwaternionowa nie jest intuicyjna. 4.2 4.2.1 Wyznaczanie orientacji Na podstawie wektorów grawitacji i pola magnetycznego Wyznaczanie orientacji za pomocą akcelerometru sprowadza się do wyznaczenia zmiany orientacji względem wektora ziemskiego, co pokazano na rysunku 4.3. Jednak, gdy jedna z osi jest równoległa do tego wektora, otrzymujemy nieskończenie wiele możliwych rozwiązań. Można wtedy przyjąć, że wartość tego kąta nie zmieniła się względem poprzedniej ramki. W uproszczonym przypadku, dla zobrazowania idei najprostszego algorytmu, sprowadza się to do równań (4.3) i (4.4) gdzie ax , ay i az są pomiarami siły z akcelerometru w odpowiednich osiach. ax √ (4.3) pitch = arctan 2 2 2 ay +az ay roll = arctan √ 2 a2x +a2z (4.4) Sposób ten nadaje się tylko wtedy, gdy przewidujemy małe wychylenia, ponieważ zawiera jedną poważną wadę. Kiedy np. wychylenie w osi pitch wyniesie 90◦ układ znajdzie się w położeniu osobliwym, tym samym zerując mianownik w równaniu (4.3). 22 4. Algorytm Aby uniknąć osobliwości można posłużyć się reprezentacją kwaternionową. Aktualną orientację w stosunku do układu globalnego można zapisać za pomocą kwaternionu przejścia z ziemskiego układu odniesienia E do lokalnego układu odniesienia L. Kwaternion taki będzie zapisany za pomocą EL q̂. Ziemski wektor grawitacji E g zdefiniowany jest następująco: h i E ĝ = 0 0 0 1 (4.5) Jeśli znamy orientację układu z poprzedniej iteracji, to możemy przewidywać, że wektor grawitacji znajduje się w pozycji L g. W tym momencie należy przeprowadzić korekcję naszego przewidywania za pomocą pomiarów z akcelerometru. Jeśli założymy, że na akcelerometr działa tylko siła grawitacji, to znormalizowany wektor wskazujący kierunek ziemskiego pola grawitacyjnego obliczony na podstawie wyniku pomiarów z tego czujnika można zapisać w postaci kwaternionu: L h â = 0 ax ay az i (4.6) Przeprowadza się minimalizację funkcji, która rotuje globalny wektor grawitacji do lokalnego układu i odejmuje od niego wektor wyznaczony za pomocą akcelerometru: f (EL q̂, E ĝ, L â) = EL q̂ E ĝ EL q̂ ∗ − L â (4.7) Gdy wynik wyniesie zero, oznacza to, że wnioskowana orientacja zgodna jest z orientacją wskazywaną przez akcelerometr. Jedną z metod minimalizacji jest metoda gradientu prostego. Jej zaletą jest łatwa implementacja i niewielkie nakłady obliczeniowe. Metoda ta jest iteracyjnym algorytmem, który opisuje równanie: L E qk+1 = L E qˆk 5f (EL qˆk , E ĝ, L â) , dla k = 0,1,2,3...n −µ k 5 f (EL qˆk , E ĝ, L â)k 5f (EL qˆk , E ĝ, L â) = J T (EL qˆk , E ĝ)f (EL qˆk , E ĝ, L â) (4.8) (4.9) µ jest wielkością kroku algorytmu. Gdy za EL q0 przyjmiemy orientację obliczoną w poprzedniej iteracji pętli głównej, to okaże się, że algorytm nie potrzebuje wielu kroków, by dać zadowalający efekt. Obserwując wektor natężenia pola magnetycznego, należy zauważyć, że nie jest on równoległy do powierzchni ziemi. Wektor ten można zapisać za pomocą równania: E h b̂ = 0 bx 0 bz i (4.10) Parametry bx oraz bz zależne są od kierunku i zwrotu wektora pola magnetycznego w danej lokalizacji. [11, 14] Znormalizowany kwaternion za pomocą pomiarów z magnetometru wygląda następująco: h i L m̂ = 0 mx my mz (4.11) Kolejne kroki postępowania są analogiczne, jak w przypadku wektora grawitacji. Obserwacja żadnego z wyżej wymienionych wektorów nie jest wystarczająca do wyznaczenia orientacji, gdy obserwujemy tylko jeden z nich. Na przykład korzystając z samego akcelerometru, kiedy jedna z osi stanie się równoległa do wektora grawitacji, otrzymujemy nieskończenie wiele rozwiązań. Jeśli jest to oś Z, to wyznaczenie kąta yaw staje się niemożliwe. Znów należało by przyjąć, że kąt ten nie zmienił się od poprzedniej ramki lub 4.3. Filtr Madgwicka 23 należy obserwować dwa wektory jednocześnie. W takim wypadku funkcja celu przyjmuje postać (4.12), a jakobian (4.13). " E fg,b (EL q̂, E ĝ, L â, b̂, L m̂) = E Jg,b (EL q̂, b̂) 4.2.2 " = fg (EL q̂, E ĝ, L â) E # fb (EL q̂, b̂, L m̂) JgT (EL q̂) (4.12) # E JbT (EL q̂, b̂) (4.13) Na podstawie prędkości kątowej Wyznaczenie orientacji w stosunku do orientacji początkowej za pomocą żyroskopu sprowadza się do całkowania prędkości obrotowej w każdej z osi. Jeśli prędkość tę wyrażoną w radianach na sekundę zapiszemy jako ω, to kąt α dany w radianach można otrzymać ze wzoru (4.14). W praktyce algorytm wykonywany jest sekwencyjnie w pewnych kwantach czasu, dlatego ma on charakter dyskretny. Jeśli kwant czasu jaki upłynął od ostatniego pomiaru zapiszemy za pomocą t i wyrazimy w sekundach, to równanie (4.14) przyjmuje postać (4.15). α= Z ω dt + αpoczatkowy αi = αi−1 + ωi ∗ t + αpoczatkowy (4.14) (4.15) To samo możemy wyrazić w notacji kwaternionowej. Jeśli za ωx , ωy i ωz przyjmiemy prędkości obrotowe wyrażone w radianach na sekundę, to kwaternion prędkości kątowej L ω będzie miał postać (4.16), a pochodna kwaternionu opisująca szybkość zmiany orientacji przyjmie postać (4.17). h i L ω = 0 ωx ωy ωz (4.16) 1L L q̂ ω (4.17) 2E Obliczanie wartości aktualnego kwaternionu orientacji EL qω wygląda bardzo podobnie jak w przypadku opisania jej za pomocą kątów Eulera. Jeśli zachowamy te same oznaczenia i dodatkowo za EL q̂est przyjmiemy ostatnio wyznaczoną orientację, to otrzymamy równania: L E q̇ L E q̇ω,k L E qω,k 4.3 = = 1L L E q̂est,k−1 ω 2 = EL q̂est,k−1 + EL q̇ω,k t (4.18) (4.19) Filtr Madgwicka Jak wcześniej zostało wspomniane, wyznaczenie dokładnej orientacji za pomocą czujników inercyjnych wymaga fuzji ich pomiarów. W praktyce sprowadza się to do eliminacji dryfu żyroskopu za pomocą czujników z pomiarem o stałym odniesieniu. Jednak i te posiadają wadę w postaci dużego szumu. Należy więc dobrać optymalną relację pomiędzy orientacją obliczoną za pomocą jednego i drugiego typu czujników. W tym celu w systemie został zaimplementowany filtr, który Sebastian Madgwick przedstawił w swojej pracy doktorskiej [13]. W obliczeniach wykorzystuje on reprezentację kwaternionową, tak, jak zostało to przedstawione w podrozdziałach 4.2.1 oraz 4.2.2. 24 4. Algorytm Rysunek 4.4 Schemat blokowy filtru S. Madgwicka wykorzystujący żyroskop, akcelerometr i magnetometr. Estymacja orientacji układu lokalnego względem ziemskiego układu odniesienia polega na odpowiedniej fuzji wyników otrzymanych z obserwacji wektorów grawitacji i pola magnetycznego oraz prędkości kątowej. Schemat blokowy filtru przedstawia rysunek 4.4. Filtr ten został zoptymalizowany pod kątem wydajności. Każdorazowe wywołanie filtru potrzebuje przeprowadzania 282 operacji arytmetycznych, w tym: 51 dodawań, 57 odejmowań, 155 pomnożeń, 14 dzieleń oraz 5 pierwiastków drugiego stopnia. Daje dobre wyniki na poziomie błędu wynoszącego 7◦ przy częstotliwości pętli już od 10 Hz. Od częstotliwości 50 Hz błąd wynosi mniej niż 2◦ . Filtr umożliwia dostrojenie go do własnych potrzeb za pomocą dwóch parametrów: β oraz ς. Wzmocnienie β określa, jak bardzo pod uwagę brana jest orientacja wyznaczona za pomocą akcelerometru i magnetometru w ostatecznej estymacji. W tym parametrze wyrażone są wszystkie błędy pochodzące od szumów żyroskopu, złej kalibracji, kwantyzacji, czy nieprostopadłego położenia osi względem siebie. Według pracy S. Madgwicka, wzmocnienie to powinno być wyrażone za pomocą wzoru: s β= 3 ω̃ 4 (4.20) gdzie ω̃ oznacza średni szum żyroskopu odczytany z dokumentacji. Wzmocnienie ς definiuje dryf żyroskopu. Nie zostało to jednak zaimplementowane w tej pracy. Z powodu ograniczenia czasowego wszystkie testy zostały wykonane tylko dla wzmocnienia β wynoszącego 1. Parametr ten został początkowo określony intuicyjnie za pomocą obserwacji wyznaczanych kątów podczas manualnego poruszania quadrotorem. 4.3. Filtr Madgwicka 4.3.1 25 Konwersja kwaternionu do kątów Eulera Jak wcześniej zostało wspomniane, kontroler do wysterowania silników przyjmuje kąty Eulera. Dostępne są one także dla użytkownika. Należy więc obliczyć wymienione kąty ze znormalizowanego kwaternionu orientacji [7]. Zależność pomiędzy tymi notacjami przedstawiona jest w równaniu (4.21). φ arctan 2(2(q0 q1 + q2 q3 ), 1 − 2(q12 + q22 )) arcsin(2(q0 q2 − q3 q1 )) θ = ψ arctan 2(2(q0 q3 + q1 q2 ), 1 − 2(q22 + q32 )) (4.21) Funkcje arcsin oraz arctan zostały zaimplementowane i są obliczane numerycznie za pomocą równań (4.22) i (4.23) z liczbą kroków równą odpowiednio 8 i 10 [6]. arcsin(z) = ∞ X n=0 arctan(z) = 2n 2n+1 z n , 4n (2n + 1) dla |z| ¬ 1 ∞ X (−1)n z 2n+1 , dla |z| ¬ 1 i z 6= i, −i 2n + 1 n=0 (4.22) (4.23) Funkcja arctan 2 [1] została zaimplementowana na podstawie poniższej zależności: arctan 2 = arctan( xy ) arctan( xy ) + π arctan( xy ) − π + π2 − π2 undefined gdy gdy gdy gdy gdy gdy x>0 y0, x<0 y < 0, x<0 y > 0, x=0 y < 0, x=0 y = 0, x=0 (4.24) Z powyższych równań można wyczytać, że gdy wyrażenia q0 q1 + q2 q3 lub q0 q3 + q1 q2 zbliżają się do wartości 0,5 lub -0,5, to jednocześnie zbliżamy się do konfiguracji osobliwej dla notacji yaw, pitch i roll. Rozdział 5 Oprogramowanie 5.1 Oprogramowanie mikrokontrolera Oprogramowanie quadrotora zostało skompilowane za pomocą kroskompilatora. Jest nim specjalnie skonfigurowany i skompilowany pakiet gcc działający pod architekturą x86_64, a kompilujący kod dla architektury ARMv7E-M, wchodzący w skład pakietu Linaro 12.06 w tak zwanej wersji bare metal z gcc 4.7.3 [2, 3, 4]. Kod został napisany w myśl zasady "dziel i zwyciężaj". Każdy problem został podzielony na mniejsze podproblemy, a te, na jeszcze mniejsze, aż każdy z nich okazał się stosunkowo prosty do rozwiązania. Podział programu na podsystemy ukazany został na rysunku 5.1. Podkatalog def zawiera pliki z kodem odpowiedzialnym za różne użyteczne definicje funkcji bądź struktur, które przydatne są w każdej części kodu. • delay – fukcje opóźniające wykonywanie programu na określony czas • dmath– implementacje złożonych funkcji matematycznych • printf – prostą implementacje punkcji printf • result – definicje struktur do przechowywania wyników pomiarów • std – implementacje funkcji takich jak: atoi, itoa, ftoa, reverse, pow, factorial, exp, atan, atan2 i asin Podkatalog dev zawiera pliki z kodem odpowiedzialnym za obsługę konkretnych układów umieszczonych na PCB. Każdy z nich zawiera makrodefinicje adresów rejestrów danego układu elektronicznego oraz pól z nimi stowarzyszonych. Dostępne są podstawowe funkcje uruchamiające odpowiednie procedury inicjalizujące peryferia potrzebne do skomunikowania się z danym układem oraz samą inicjalizację danego układu. Podczas tych kroków sprawdzana jest także poprawność połączenia. Wspólną funkcjonalnością jest możliwość wysłanie komendy do konkretnego układu. Każdy z plików zawiera także specjalizowane procedury służące np. do odczytu aktualnych pomiarów i zapisaniu ich do struktury skojarzonej z danym urządzeniem. • bma180 – zawiera kod do obsługi akcelerometru bma180 • l3g4200d – zawiera kod do obsługi żyroskopu l3g4200d • mag3110 – zwiera kod do obsługi magnetometru mag3110 28 5. Oprogramowanie • rfm73 – zawiera kod do obsługi modułu radiowego rfm73 • bmp180 – zawiera kod do obsługi barometru bmp180 • vcnl4000 - zawiera kod do obsługi czujnika odbiciowego vcnl4000 Podkatalog inc zawiera pliki nagłówkowe dostarczone przez firmę ARM wraz z CMSIS1 dla danego rdzenia. Zawiera także definicję bitbandingu2 oraz specjalnych rejestrów. Podkatalog mod zawiera pliki z kodem do obsługi konkretnych peryferiów mikrokontrolera. Funkcje służące do inicjalizacji danego peryferium występują w różnych wersjach w zależności od wymaganych ustawień. Każdy z nich posiada także wyspecjalizowane procedury do kontroli jego działania. • i2c – zawiera kod związany z obsługa magistrali I2 C • leds – zawiera kod związany z obsługa diod LED umieszczonych na PCB • rcc – zawiera kod związany z konfiguracją głównego zegara taktującego oraz jego źródła, zegara wewnętrznych magistrali, zegara taktującego pamięć FLASH i odblokowanie FPU • spi – zawiera kod związany z konfiguracją magistrali SPI • time – zawiera kod związany z obsługą timerów przeznaczonych dla użytkownika, min. funkcję zwracającą ilość ms, która minęła od ostatniego wywołania tej funkcji • usart – zawiera kod związany z obsługa magistrali UART W głównym katalogu znajdują się pliki, w których znajduje kod do obsługi bardziej ogólnych zadań oraz pliki dodatkowe. • ahrs – w tym pliku znajdują się globalne zmienne oraz implementację funkcji potrzebne do wyznaczenia aktualnej orientacji statku względem ziemi na podstawie aktualnie dostępnych pomiarów z czujników. Zawarta jest tutaj między innymi implementacja filtru Madgwicka oraz funkcja przeliczająca znormalizowany kwaternion orientacji na kąty yaw, pitch, roll. • controller – w pliku tym znajduje się aktualna implementacja sterownika lotu • engines – w tym pliku znajdują się funkcje do obsługi silników • radio – w tym pliku znajdują się funkcje do obsługi radia na wyższym poziomie abstrakcji, które umożliwiają np. spakowanie odpowiednich danych do pakietu i ich wysłanie. • main – główny plik, tutaj wywoływane są kolejne inicjalizacje podsystemów oraz znajduje się tutaj główna pętla sterowania. • f4g.rom.ld – skrypt linkera odpowiedzialny za obliczenie i ustawienie takich stałych jak ilość i miejsce znajdowania się konkretnego rodzaju pamięci w przestrzeni adresowej, końca i początku stosu, rozmieszczenia odpowiednich sekcji etc. 1 Cortex Microcontroller Software Interface Standard - niezależna sprzętowa warstwa abstrakcji dostarczona przez firmę ARM dla każdego rdzenia. 2 rdzeń mapuje całe słowa z pamięci z jednego obszaru pamięci na bity w innym regionie 5.2. Oprogramowanie na PC 29 • startup.S – kod napisany w assemblerze, który zostanie umieszczony na początku i wykona wszystkie niezbędne operacje, które potrzebne są do zadziałania kodu skompilowanego za pomocą kompilatora języka C. Kod ten umieszcza np. adresy obliczone przez linker w odpowiednich rejestrach oraz umieszcza tablicę wektorów na początku przestrzeni adresowej. • makefile – plik zawiera kod ze zdefiniowanymi flagami i zależnościami kompilacji, rozumiany przez polecenie make, które buduje cały projekt Pętla główna programu składa się z 6 etapów: 1. Pomiaru czasu, który upłynął od ostatniej iteracji 2. Pobraniu pomiarów z czujników i zapisaniu w użytecznej formie 3. Aktualizacji aktualnej orientacji 4. Zamianie reprezentacji orientacji na kąty Eulera 5. Kolejnej iteracji sterownika silników 6. Opcjonalne przesłanie lub otrzymanie danych do/z PC Dla lepszego zobrazowania zasady działania podsystemów na diagramach (5.2) i (5.3) zostały przedstawione przykładowe elementy pętli głównej. Na obecnym etapie pętla wykonuje się z częstotliwością od 300 do 500 Hz. 5.2 Oprogramowanie na PC Podczas działania quadrotora, można rysować wysyłane przez niego dane w czasie rzeczywistym. Program napisany w pythonie w nieskończonej pętli zapisuje dane do pliku bez jego zamykania. Gnuplot, połączony z komendą linuxową tail oraz opcją reread, dodaje do wykresu każdy nowo pojawiający się pomiar. Po zakończonej sesji pomiarowej, dane dalej znajdują się w pliku i mogą posłuży później. W języku python obsługa portu szeregowego,odczytanie danych i ich zapis do pliku zajmuje mniej niż 20 linijek. System ten z jednej strony jest bardzo prosty, a z drugiej wydajny. Kod napisany w pythonie został przedstawiony w listingu (5.1), a skrypt dla gnuplota w listingu (5.2). Listing 5.1 Kod w pythonie do obsługi portu szeregowego i plikuz danymi. import random import s e r i a l import time r s = s e r i a l . S e r i a l ( ’ / dev /ttyUSB0 ’ , 921600) d a t a _ f i l e = open ( " output . dat " , ’w ’ ) i =0 time = 0 while True : data = r s . r e a d l i n e ( ) . s p l i t ( "␣" ) [ : − 1 ] print data [ gx , gy , gz , mx, my, mz , ax , ay , az , y , p , r , t , f ] = data time = time + int ( t ) 30 5. Oprogramowanie i+=1 d a t a _ f i l e . w r i t e ( ( s t r ( "%s ␣%s ␣%s ␣%s \n" % ( time , y , p , r ) ) ) ) print ( s t r ( "%5s ␣%5s ␣%5s ␣%5s ␣\n" % ( time , mx, my, mz ) ) ) data_file . flush () Listing 5.2 Skrytp gnuplota do rysowania wykresu w czasie rzeczywistym. set terminal wxt s i z e 1200 ,600 set t i t l e "Quadro" set xlabel "Time" set ylabel " Angle " set grid set yrange [ − 1 8 0 : 1 8 0 ] plot \ ’< t a i l ␣−n␣ 2000 ␣ output . dat ’ using 1 : 2 with l i n e s t i t l e "Yaw" , \ ’< t a i l ␣−n␣ 2000 ␣ output . dat ’ using 1 : 3 with l i n e s t i t l e " P i t c h " , \ ’< t a i l ␣−n␣ 2000 ␣ output . dat ’ using 1 : 4 with l i n e s t i t l e " R o l l " reread pause mouse key 5.3 Komunikacja pomiędzy PC i quadrotorem Komunikacja pomiędzy quadrotorem a PC może odbywać się na kilka różnych sposobów: • Dwukierunkowo poprzez kabel USB • Z quadrotora do komputera poprzez radio • Do quadrotora z komputera poprzez radio 5.4 Narzędzia • Do narysowania schematów i zaprojektowania płytki drukowanej posłużył Altium Designer w 30-dniowej wersji testowej. • Diagramy zostały narysowane w programie Dia. • Rysunki poglądowe zostały narysowane za pomocą programu Inkscape. • Diagram sekwencji został narysowany w programie Papyrus. • Za klienta GDB posłużyło oprogramowanie OpenOCD • Do narysowania wykresów posłużył program gnuplot 5.4. Narzędzia 31 Rysunek 5.1 Struktura organizacyjna kodu źródłowego oprogramowania quadrotora. 32 5. Oprogramowanie Rysunek 5.2 Diagram sekwencji pobrania danych z akcelerometru 5.4. Narzędzia 33 Rysunek 5.3 Diagram sekwencji systemu AHRS Rozdział 6 Testy W rozdziale tym przedstawione zostały wyniki testów zbudowanego systemu AHRS. 6.1 Stabilność pomiarowa czujników Czujniki są pierwszym ogniwem w całym systemie AHRS, dlatego błędy pomiarowe wynikające z ich niedoskonałości towarzyszą systemowi od samego początku kumulując się. Na charakterystyce statycznej (kiedy czujnik jest w bezruchu) możemy zaobserwować szum towarzyszący sygnałowi. Charakterystyka dynamiczna mówi nam o tym jak szybko czujnik reaguje na zmiany. Obie te charakterystyki przedstawione są na rysunkach 6.1 dla akcelerometru, 6.2 dla magnetometru oraz 6.3 i 6.4 dla żyroskopu. Obserwowane błędy mają charakter szumu białego. Prócz błędów wynikających z niedoskonałości samego czujnika, pojawiają się błędy spowodowane zewnętrznymi zakłóceniami. Jeśli akcelerometr wykorzystywany jest do obserwacji ziemskiego wektora grawitacji, to każde inne przyspieszenie, nie wynikające z przyciągania ziemskiego traktowane jest jako zakłócenie. Zazwyczaj najpoważniejszym źródłem zakłóceń są elementy wykonawcze, takie jak silniki, którymi albo układ steruje albo pracują w ich okolicy. Innym źródłem zakłóceń, którego często nie można przewidzieć, są czynniki środowiskowe. Na wykresach 6.5 i 6.6 przedstawiono wpływ uruchomienia silnika na sygnał z czujników. Tak jak widać na wykresie 6.5, pomiar z akcelerometru jest całkowicie zaszumiony zmiennym sygnałem o duże amplitudzie i wysokiej częstotliwości. Jego źródłem są wibracje silnika, które przenoszą się przez płytkę drukowaną na czujnik. Wpływ silnika na pomiary magnetometru nie jest tak znaczny. Fakt ten nie jest zgodny z początkowymi przewidywaniami. Spodziewano się, że pole magnetyczne, indukowane z dużą częstotliwością przez prąd o wysokim natężeniu, przemiennie płynący do silników, będzie miało duży wpływ na magnetometr. Dlatego quadrotor został zaprojektowany tak aby magnetometr umieścić z dala od ścieżek miedzi i przewodów. Nie spodziewano się za to wibracji, gdyż są niewyczuwalne dla człowieka z powodu ich dużej częstotliwości. Zakłócenia środowiskowe, których pochodzenia można się tylko domyślać, widoczne są dobrze na pomiarze magnetometru w osi X. Pojawiają się one zawsze, bez względu na pracę silnika. Oba z zakłóceń mają charakter wysokoczęstotliwościowy. Zakłócenia na magnetometrze są akceptowalne, lecz błąd pomiaru z akcelerometru przy włączonych silnikach jest niedopuszczalny, a sygnał w tej formie–bezużyteczny. Zdecydowano się więc na zaimplementowanie filtra Kalmana dla każdego z pomiarów. Filtr ten opiera się na poprzednich 36 6. Testy Rysunek 6.1 Charakterystyka dynamiczna (początek) i statyczna (od 39 sekundy) akcelerometru. Rysunek 6.2 Charakterystyka dynamiczna (początek) i statyczna (od 39 sekundy) magnetometru. 6.1. Stabilność pomiarowa czujników Rysunek 6.3 Charakterystyka dynamiczna żyroskopu. Rysunek 6.4 Charakterystyka statyczna żyroskopu. 37 38 6. Testy Rysunek 6.5 Wpływ pracy silnika na pomiar akcelerometru. Pod koniec pomiaru silniki zostały wyłączone. Rysunek 6.6 Wpływ pracy silnika na pomiar magnetometru. Pod koniec pomiaru silniki zostały wyłączone. 6.2. Dokładność systemu AHRS 39 Rysunek 6.7 Wpływ filtracji Kalmana na wynik pomiaru z akcelerometru przy włączonych silnikach. Pod koniec pomiarów silniki zostały wyłączone. pomiarach z dobieranym wzmocnieniem. Dla pojedynczego sygnału ma charakter filtra dolnoprzepustowego. Filtr Kalmana zaimplementowano dla akcelerometru i magnetometru. Szum z żyroskopu usuwany jest w filtrze Madgwicka, a stopień filtracji ustawiany jest za pomocą parametru β. Dryf żyroskopu jest sygnałem o małej częstotliwości i potrzebny w tym wypadku jest filtr górnoprzepustowy. Włączony został on w samym czujniku poprzez wybranie odpowiedniego pola w rejestrze konfiguracyjnym. Wpływ filtru Kalmana na wyniki pomiaru przy włączonych silnikach został pokazany na wykresach 6.7 i 6.8. Jak można zaobserwować, stosunek sygnału do szumu poprawił się znacząco. Wpływ filtru Kalmana na charakterystykę statyczną można zobaczyć na wykresach 6.9 i 6.10. Szum biały został znacząco zredukowany, a na sygnale z magnetometru nie pojawiają się już charakterystyczne "szpilki". 6.2 Dokładność systemu AHRS Jakość całego systemu określono na dwa sposoby. Pierwszym jest stabilność orientacji, gdy quadrotor znajduje się w stanie spoczynku. Drugim, różnica pomiędzy orientacją zadaną quadrotorowi przez robota przemysłowego, a orientacją z systemu AHRS. W pierwszym przypadku pomiary zostały dokonane w różnych długościach czasu. Wyniki przedstawione są na wykresach 6.11, 6.12 i 6.13. Możemy z nich wyczytać, że w osi pitch i roll maksymalny błąd wynosi 1◦ , prócz jednego miejsca gdzie quadrotor prawdopodobnie przekręcił się nieznacznie, gdyż posiada on bardzo nieregularnej budowy spód. Jest to bardzo dobry wynik z tego powodu, że gdyby wzorować się na nieprzetworzonych pomiarach z czujników, to według dokumentacji, żyroskop w każdej iteracji generował by ◦ błąd na poziomie 10 s , a akcelerometr z magnetometrem na poziomie 5%. Prawidłowe wyznaczenie orientacji zostało zmierzone poprzez zamontowanie quadrotora na robocie przemysłowym IRB firmy ABB. Wygląd stanowiska przedstawiony jest na zdjęciu 6.14. W czasie testów, za pomocą programu, który był wykonywany przez robota, quadrotor został wychylony w osi roll i pitch o 10◦ i -10◦ , oraz obrócony w osi yaw o 90◦ i -90◦ . W tym czasie, przesyłana poprzez radio wyznaczana orientacja, została zapisana na 40 6. Testy Rysunek 6.8 Wpływ filtracji Kalmana na wynik pomiaru z magnetometru przy włączonych silnikach. Pod koniec pomiarów silniki zostały wyłączone. Rysunek 6.9 Wpływ filtracji Kalmana na charakterystykę statyczną akcelerometru. 6.2. Dokładność systemu AHRS 41 Rysunek 6.10 Wpływ filtracji Kalmana na charakterystykę statyczną magnetometru. Rysunek 6.11 Orientacja wyznaczona przez system AHRS podczas spoczynku quadrotora. Czas pomiaru: 10 s. 42 6. Testy Rysunek 6.12 Orientacja wyznaczona przez system AHRS podczas spoczynku quadrotora. Czas pomiaru: 15 min. Rysunek 6.13 Orientacja wyznaczona przez system AHRS podczas spoczynku quadrotora. Czas pomiaru: 1 h. 6.2. Dokładność systemu AHRS 43 Tabela. 6.1 Nastawy przegubów robota przemysłowego w czasie pomiarów dla poszczególnych kątów. Wartość w przegubie Mierzony kąt Pierwsza pozycja Druga pozycja Trzecia pozycja Roll -0,46◦ 9,92◦ -9,98◦ Pitch -0,46◦ 9,92◦ -9,98◦ Yaw -180,03◦ -269,90◦ -89,97◦ komputerze. Nastawy robota IRB zapisane są w tabeli 6.1. Wyniki pomiarów przedstawione są na wykresach 6.15, 6.16 i 6.17. Z wykresów można odczytać, że dokładność wyniosła kolejno: 90% (dla osi pitch), 85% (dla osi roll) i 95% (dla osi yaw) (zawsze z niedomiarem, wiec jest to prawdopodobnie wynik złego skalowania danych z czujników). Wyniki testów pokazują, że udało się spełnić postawione na początku założenia. Za pomocą filtracji Kalmana osiągnięto stabilne, niezakłócone pomiary z akcelerometru i magnetometru, a za pomocą filtracji Madgwicka odfiltrowano błędy żyroskopu. Potwierdzają to charakterystyki statyczne wymienionych czujników oraz eksperyment z nieruchomym quadrotorem. Eksperyment ten pokazał, że wyznaczana orientacja również jest stabilna. Uzyskana dokładność jest na zadowalającym poziomie, ponieważ dla sterowania quadrotorem większa nie jest wymagana. System AHRS wymaga lepszej kalibracji i należałoby znaleźć takie parametry filtrów by optymalizować dynamikę i stabilność lotu robota. Magnetometr powinien korygować swój offset w czasie rzeczywistym co jest możliwe do zaimplementowania. Powinno także postarać się o całkowite zniwelowanie wpływu dryfu żyroskopu. 44 6. Testy Rysunek 6.14 Stanowisko do pomiaru dokładności wyznaczonej orientacji. 6.2. Dokładność systemu AHRS 45 Rysunek 6.15 Orientacja wyznaczona przez system podczas testów z robotem przemysłowym, dla wychyłu w osi roll. Rysunek 6.16 Orientacja wyznaczona przez system podczas testów z robotem przemysłowym, dla wychyłu w osi pitch. 46 6. Testy Rysunek 6.17 Orientacja wyznaczona przez system podczas testów z robotem przemysłowym, dla obrotu w osi yaw. Rozdział 7 Wnioski W pracy tej udało się zrealizować kompletny, działający system AHRS. Wszystkie założenia projektowe zostały zrealizowane. Błąd stabilności wyznaczanej orientacji nie przekracza 1◦ w osi pitch i osi roll, co zostało potwierdzone eksperymentami z nieruchomym quadrotorem. Eksperyment z robotem przemysłowym wykazał, że niezerowe kąty wyznaczane są z dokładnością od 85% do 95%. Dzięki zastosowaniu dwóch filtrów, orientacja jest wyznaczana przez system z wystarczającą szybkością i dokładnością potrzebną do korekcji lotu quadrotora. Precyzja w wyznaczaniu orientacji większa niż 95% nie jest istotna dla tego typu zastosowań, a stabilność w punkcie zero okazała się więcej niż zadowalająca. Całość została zaprojektowana w sposób elastyczny, umożliwia łatwą modyfikację i rozbudowę systemu. System ten zostanie w późniejszym czasie lepiej skalibrowany. Zostaną wykonane testy wskazujące optymalne ustawienie wzmocnień filtrów. Następnie system stanie się kompletnym podzespołem quadrotora. Celem wszystkich prac jest w pełni autonomiczny, mały robot latający. W projekcie da się zauważyć ogromną rolę wpływu filtracji na osiągnięte wyniki. Można wysnuć wniosek, że jeśli czujniki są oczami na świat, to filtr jest sposobem jego postrzegania. Tak jak w przypadku ludzi: widząc tę samą rzecz i kierując się tymi samymi zasadami, każdy człowiek zareaguje inaczej, gdyż inaczej postrzega to co widzi. Jeden zauważy to czego nie zauważył drugi, a trzeciego rozproszy nieistotne tło. Tak w przypadku robotów wyposażonych w identyczne sensory i z zapisanymi w algorytmie takimi samymi regułami. Jeden będzie zachowywał się poprawnie, a drugi nie, gdyż może przeoczyć to co jest ważne. Dodatek A Schematy D C B A 1 HEAD 1 HEAD 1 HEAD 1 HEAD 1 HEAD HEAD HEAD 2u2 After remap GND GBAT DD+ 1 ADC_IN8 1 VBAT_L H3 1 HEAD 1 H1 1 GNDA HEAD USB1 GND H2 HEAD 1 HEAD 1 HEAD 1 HEAD 1 HEAD 1 HEAD 1 HEAD 1 HEAD 1 HEAD 1 HEAD 1 M1 M2 VBUS DD+ 1 VBUS C39 Head 4 3 2 1 J10 ADC_IN8 GND GND STM32F405RGT6 PB7 PB6 PB5 PB4/JNTRST PB3/JTDO PB2/BOOT1 PB1 PB0 PB15 PB14 PB13 PB12 PB11 PB10 PB9 PB8 PC7 PC6 PC5 PC4 PC3 PC2 PC1 PC0 PD2 OSC_OUT/PD1 OSC_IN/PD0 BOOT0 NRST VBAT VDD_4 VDD_3 VDD_2 VDD_1 VDDA 2 PA7 PA6 PA5 PA4 PA3 PA2 PA1 PA0-WKUP PA15/JTDI PA14/JTCK/SWCLK PA13/JTMS/SWDIO PA12 PA11 PA10 PA9 PA8 SWCLK SWDIO VCC PC15-OSC32_OUT PC14-OSC32_IN PC13-TAMPER-RTC PC12 PC11 PC10 PC9 PC8 VSS_4 VSS_3 VSS_2 VSS_1 VSSA USART3_TX USART3_RX VCC 59 58 57 56 55 28 27 26 36 35 34 33 30 29 62 61 SPI2_MOSI SPI2_MISO SPI2_SCK RF_CE I2C2_SDA I2C2_SCL LED3 LED4 I2C1_SDA I2C1_SCL BLDC_ENG4 38 37 25 24 11 10 9 8 4 3 2 53 52 51 40 39 18 63 47 31 LED1 LED2 SPI1_GYRO SPI1_ACCE USART3_RX USART3_TX RF_CS RF_IRQ C38 2u2 12 U2 4 3 2 1 Head J3 2 23 22 21 20 17 16 15 14 50 49 46 45 44 43 42 41 54 6 5 60 7 1 19 64 48 32 13 BLDC_PWM1 BLDC_ENG3 BLDC_PWM3 SPI1_MOSI SPI1_MISO SPI1_SCK BLDC_ENG1 SWCLK SWDIO BLDC_ENG2 BLDC_PWM4 USART1_RX USART1_TX BLDC_PWM2 RESET 5 4 D+ 3 6 8 7 D- C11 100n C32 100n GND C33 100n VCC GND CP2102 D+ D- GND VDD VBUS REGIN U1 8MHz C19 22p Y1 SW-PB S1 R27 10k VCCA VCC GND C10 4u7 VBUS C34 100n C20 22p 3 C35 100n 24 23 25 26 27 28 11 12 1 2 9 Date: File: A4 Size Title GND Q3 N-MOS D3 Led R13 1k VBAT VCC LED3 LED4 GND Q2 N-MOS D5 Led R15 1k VBAT GND Q8 N-MOS D9 Led R1 1k VBAT 2013-10-13 D:\workspace\..\duszolap_main.SchDoc Number Q1 N-MOS D4 Led R14 1k 10k R26 USART1_TX USART1_RX C36 100n VCCA LED2 LED1 GND RTS CTS RXD TXD DSR DTR ~SUSPEND SUSPEND DCD RI RST 3 4 Sheet of Drawn By: RF_IRQ RF_CE RF_CS I2C1_SCL I2C1_SDA I2C2_SCL I2C2_SDA RF_IRQ RF_CE RF_CS SPI2_SCK SPI2_MOSI SPI2_MISO SPI1_SCK SPI1_MOSI SPI1_MISO SPI1_GYRO SPI1_ACCE BLDC_ENG4 BLDC_ENG3 BLDC_ENG2 BLDC_ENG1 BLDC_PWM4 BLDC_PWM3 BLDC_PWM2 BLDC_PWM1 Revision SPI2_SCK SPI2_MOSI SPI2_MISO SPI1_SCK SPI1_MOSI SPI1_MISO SPI1_GYRO SPI1_ACCE BLDC_ENG4 BLDC_ENG3 BLDC_ENG2 BLDC_ENG1 BLDC_PWM4 BLDC_PWM3 BLDC_PWM2 BLDC_PWM1 I2C1_SCL I2C1_SDA I2C2_SCL I2C2_SDA 4 D C B A D C B A VBAT 1 D7 Led D6 Led D8 Led R23 470R VBUS 3 7 3 4 1 2 GBAT ONOFF spdt Q6 P-MOS 2 VIN VBAT VBAT D1 VSS MCP73833-AMI/UN PG STAT1 THERM STAT2 PROG VDD VDD U7 VBAT 1 R22 470R S3 GNDA GND Fuse 2A F1 ONOFF Q5 GBAT IRLML6244 R12 470R Header 2 Header 2 R10 470R 2 1 P1 2 1 BATT 1 5 6 8 10 9 VBAT 2 GBAT GND 2 R24 1k9 C29 100n 1 R25 10k MCP1825S GND GND VIN VOUT U5 2 4 3 GND GND D2 Led R9 1k VBAT 3 GND C37 T491B 100pF 3 Date: File: A4 Size Title VBAT C31 10u GND 100k R8 VCC 2013-10-13 D:\workspace\..\zasilanie.SchDoc Number 330k R7 1uH/400mA L2 BAT_L 4 C30 100n VCCA Revision GNDA Sheet of Drawn By: GND 1uH/100mA L3 4 D C B A D C B A I2C1_SDA I2C1_SCL I2C2_SDA I2C2_SCL SPI1_SCK SPI1_MOSI SPI1_MISO SPI1_GYRO SPI1_ACCE 1 I2C1_SDA I2C1_SCL I2C2_SDA I2C2_SCL SPI1_SCK SPI1_MOSI SPI1_MISO 1 R18 2k2 R2 10k U10 VDDIO VDD Magnetometr MAG3110 INT1 GND GND C26 100n C5 100n GND GND C25 100n GND VCNL4000 IR CATHODE IR CATHODE IR ANODE 2 C22 100n C23 100n GND C24 10u VCCA C6 10u C7 100n GND C8 10u VCC C1 100n R16 2k2 GND 3 SPI1_SCK SPI1_MOSI SPI1_MISO SPI1_GYRO VCC 6 7 2 3 4 5 15 Date: File: A4 Size Title L3G4200D Number GND RES RES RES RES RES PLLFILT VDDIO VDD 13 8 9 10 11 12 14 1 16 C9 100n SPI1_SCK SPI1_MOSI SPI1_MISO SPI1_ACCE C12 10u VCCA R20 10k 4 C16 100n C3 100n VCC Revision C15 100n VCC GND C14 10u VCC C27 10n GND C13 100n GND C21 470n BMA180 INT SCK VDDIO SDI VDD SDO CSB U4 Akcelerometr 4 2013-10-13 Sheet of D:\workspace\Altium\INZ\czujniki.SchDocDrawn By: GND GND DRDY/INT2 INT1 SCL/SPC SDA/SDI SDO/SA0 CS RST U9 Żyroskop VCNL4000 VDD IR ANODE IR CATHODE IR CATHODE I2C2_SCL SCL I2C2_SDA SDA R17 2k2 VCC C2 100n VCCA U8 VCCA BMP180 SDO CSB GND VCC U6 VCC Cisnieniomierz U3 VDDIO I2C2_SCL SCL VDD I2C2_SDA SDA R5 2k2 Odleglosciomierz VDD R4 2k2 VCC 3 Odleglosciomierz I2C1_SCL SCL I2C1_SDA SDA R11 2k2 VCC I2C1_SCL SCL CapA I2C1_SDA SDA CapR R19 2k2 VCC R6 2k2 SPI1_GYRO SPI1_ACCE R3 10k VCC 2 GND C17 10u VCCA GND C4 100n VCCA D C B A D C B A 1 1 RF_CE RF_IRQ RF_SCK RF_MISO RF_MOSI RF_CS 2 2 3 8 5 7 6 4 RFM73 CE IRQ SCK MISO MOSI CSN RF1 3 GND VDD 3 1 2 Date: File: A4 Size Title GND C18 100n VCC 4 2013-10-13 Sheet of D:\workspace\Altium\INZ\remote.SchDoc Drawn By: Number C28 10u 4 Revision D C B A D C B A 1 1 BLDC_ENG BGND 1k R21 (R21_S1, ...) VBAT BLDC_PWM IRLML6244 Q4 (Q4_S1, ...) 2 2 3 2 1 Turnigy 3A 4,2V SIG VBAT GND S2 (S2_S1, ...) A B C 6 4 5 3 3 Date: File: A4 Size Title 2013-10-13 D:\workspace\..\sterownik_bldc.SchDoc Number 4 Sheet of Drawn By: 4 Revision D C B A Dodatek B PCB Dodatek C Zestawienie czujników baro magne gyro accele BMP085 BMP180 HP02S LPS331AP MPL3115A HMC5583L MAG3110 BMC050 ITG3205 L3G4200D L3GD20 LSM330DLC MMA8450Q MMA8451Q MMA8452Q MMA8453Q MMA8652FC LIS35DE LIS3DH LIS331DLH AIS328DQ LSM330DLC ADXL343 BMA180 Jacek Drewniak bosh bosh hope rf st freescale honeywell freescale bosh InventSense st st st freescale freescale freescale freescale freescale st st st st st analog devices bosh manufacturer QFN24 4x4 LGA16 4x4 LGA16 3x3 LGA28L 4x5 QFN16 3x3 QFN16 3x3 QFN16 3x3 QFN16 3x3 DFN10 2x2 LGA14 3x5 LGA16 3x3 LGA16 3x3 QFN24 4x4 LGA28L 4x5 LGA14 3x5 LGA12 3x3 package A A A A A 3V3 3V3 3V3 3V3 3V3 3V3 3V3 1V8 3V3 3V3 3V3 3V3 3V3 3V3 3V3 3V3 3V3 3V3 3V3 30uA 30uA 500uA 30uA 265uA 100uA 900uA 5mA 6,5mA 6,1mA 6,1mA 6,1mA 42 uA 24uA 24uA 24uA 20uA 300uA 11uA 250uA 200uA 11uA 23uA 975uA voltage current (50hz) LCC8 5X5 3V3 LGA7 4X4 3V3 spec 3V3 & 5 LGA16L 3x3 3V3 LGA8 5X3 3V3 XYZ LCC16 3x3 XYZ DFN10 2x2 XYZ LGA16 3x3 XYZ XYZ XYZ XYZ XYZ XYZ XYZ XYZ XYZ XYZ XYZ XYZ XYZ XYZ XYZ XYZ axis kamami kamami kamami kamami kamami kamami kamami kamami kamami kamami kamami kamami pl.farnell pl.farnell pl.farnell pl.farnell digikey kamami pl.farnell pl.farnell uk.farnell kamami pl.farnell kamami supplier i2c i2c i2c i2c/spi i2c i2c i2c i2c/spi i2c i2c/spi i2c/spi i2c/spi i2c i2c i2c i2c i2c i2c/spi i2c/spi i2c/spi i2c/spi i2c/spi i2c/spi i2c/spi long long 14bit 24bit 20bit 12bit 16bit 10bit 16bit 16bit 16bit 16bit 12bit 14bit 12bit 10bit 12bit 8bit 16bit 16bit 12bit 16bit 10bit 14bit interface digita output Zestawienie czujników 0,03 0,02 nm 0,02 * noise ±hPa range hPa 300.. 1100 300.. 1100 300.. 1100 260.. 1260 500.. 1100 nm 0,25 rms nm noise ±uT range ±uT 8000 1000 1000 0,7 0,03 0,03 0,03 noise dps range dps 2000 250 to 2000 250 to 2000 250 to 2000 375ug 99ug 99ug 99ug 182ug nm 220ug 218ug 218ug 220ug 40ug 150ug noise/√hz 2 to 8 2 to 8 2 to 8 2 to 8 2 to 8 2 to 8 2 to 16 2 to 8 2 to 8 2 to 16 2 to 16 1 to 16 range g 0,25 0,17 nm 0,17 * noise ±m nm 1000 10 ±uT 60 10 10 10 0g dps 50 20 20 30 33,5 60 40 20 20 60 30 5 nm nm nm nm nm nm nm nm 1,00 0,03 0,03 0,02 t/0g dps/°C 0,50 0,15 0,15 0,15 0,27 0,50 0,5 0,1 0,80 0,5 0,80 0,5 0,5 0,1 1 uT 0,1 8,8 8,8 8,75 mdps 1,0 0,2 1,0 4,0 1,0 72,0 1 1,1 1,0 1 4,0 1 nm 0,01 0,0001 ± %/°C 0,003 0,16 0,16 0,16 0,05 0,008 0,008 0,008 0,007 0,07 0,001 0,001 nm 0,0005 0,004 0,001 nm nm nm nm nm 0,01 0,01 0,1 0,0002 0,0025 0,5 0,5 nm 2 1 Non Linear %FS Sens ±hPa accuracy °C 0,1 0,3 1 0,2 0,2 0,2 nm 0,25 nm nm nm nm nm nm nm nm nm 0,50 0,15 0g-Off ±mg TCOff ±mg/°C Non Linear %FS Sens ±mg Sens ±mg/T Dodatek D CD Na płycie znajduje się: • folder pdf, który zawiera elektroniczną wersję tego dokumentu • folder kod, który zawiera kod programu dla quadrotora • folder schematy, który zawiera schematy elektryczne quadrotora Bibliografia [1] Atan2. http://en.wikipedia.org/wiki/Atan2. [2] GCC, the GNU compiler collection. http://gcc.gnu.org/. [3] Getting started with the STM32F4 and GCC. http://jeremyherbert.net/get/ stm32f4_getting_started. [4] Open source software for ARM SoCs. http://www.linaro.org/. [5] Quaternions and spatial rotation. http://en.wikipedia.org/wiki/Quaternions_ and_spatial_rotation. [6] Taylor series. http://en.wikipedia.org/wiki/Taylor_series. [7] M. J. Baker. Conversion quaternion to Euler. http://www.euclideanspace.com/ maths/geometry/rotations/conversions/quaternionToEuler/. [8] M. J. Baker. Euler angles. http://www.euclideanspace.com/maths/geometry/ rotations/euler/index.htm. [9] M. Bednar. MEMS (Micro Electro-Mechanical Systems) technology. http://www. sensorland.com/HowPage023.html. [10] G. X. Jay Esfandyari, Roberto De Nuccio. Introduction to MEMS gyroscopes. http: //electroiq.com/blog/2010/11/introduction-to-mems-gyroscopes/. [11] S.-k. Kitashirakawa-Oiwake Cho. Data analysis center for geomagnetism and space magnetism. http://wdc.kugi.kyoto-u.ac.jp/igrf/. [12] M. Looney. Motionless bandwidth test for MEMS sensors. http://www.analog. com/library/analogdialogue/archives/41-04/mems_bandwidth.html. [13] S. O. Madgwick. An efficient orientation filter for inertial and inertial/magnetic sensor arrays. diploma thesis, University of Bristol, April 2010. [14] U. G. Survey. USGS geomagnetism program. http://geomag.usgs.gov/. Spis rysunków 1.1 1.2 1.3 1.4 Schemat funkcyjny. . . . . Schemat ideowy. . . . . . . Quadrotor. . . . . . . . . . Quadrotor przyrównany do 2.1 2.2 2.3 Kalibracja magnetometru . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 Rozmieszczenie czujników na górnej stronie PCB. . . . . . . . . . . . . . . 14 Rozmieszczenie czujników na dolnej stronie PCB. . . . . . . . . . . . . . . 14 3.1 Porównanie podstawowych parametrów wybranych mikrokontrolerów. . . . 16 4.1 4.2 4.3 4.4 Rozmieszczenie osi yaw, pitch i roll. . . . . . . . . . . . . . . . . . . . . . Interpretacja kwaternionu jako wektor i kąt obrotu. . . . . . . . . . . . . Orientacja względem wektora grawitacji w jednej z osi. . . . . . . . . . . Schemat blokowy filtru S. Madgwicka wykorzystujący żyroskop, akcelerometr i magnetometr. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.1 5.2 5.3 . . . . . . . . . . . . linijki. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 6 6 7 . 20 . 20 . 21 . 24 Struktura organizacyjna kodu źródłowego oprogramowania quadrotora. . . 31 Diagram sekwencji pobrania danych z akcelerometru . . . . . . . . . . . . 32 Diagram sekwencji systemu AHRS . . . . . . . . . . . . . . . . . . . . . . 33 6.1 Charakterystyka dynamiczna (początek) i statyczna (od 39 sekundy) akcelerometru. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.2 Charakterystyka dynamiczna (początek) i statyczna (od 39 sekundy) magnetometru. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.3 Charakterystyka dynamiczna żyroskopu. . . . . . . . . . . . . . . . . . . 6.4 Charakterystyka statyczna żyroskopu. . . . . . . . . . . . . . . . . . . . . 6.5 Wpływ pracy silnika na pomiar akcelerometru. Pod koniec pomiaru silniki zostały wyłączone. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.6 Wpływ pracy silnika na pomiar magnetometru. Pod koniec pomiaru silniki zostały wyłączone. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.7 Wpływ filtracji Kalmana na wynik pomiaru z akcelerometru przy włączonych silnikach. Pod koniec pomiarów silniki zostały wyłączone. . . . . . . 6.8 Wpływ filtracji Kalmana na wynik pomiaru z magnetometru przy włączonych silnikach. Pod koniec pomiarów silniki zostały wyłączone. . . . . . . 6.9 Wpływ filtracji Kalmana na charakterystykę statyczną akcelerometru. . . 6.10 Wpływ filtracji Kalmana na charakterystykę statyczną magnetometru. . . 6.11 Orientacja wyznaczona przez system AHRS podczas spoczynku quadrotora. Czas pomiaru: 10 s. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 . 36 . 37 . 37 . 38 . 38 . 39 . 40 . 40 . 41 . 41 66 SPIS RYSUNKÓW 6.12 Orientacja wyznaczona przez system AHRS podczas spoczynku quadrotora. Czas pomiaru: 15 min. . . . . . . . . . . . . . . . . . . . . . . . . . . 6.13 Orientacja wyznaczona przez system AHRS podczas spoczynku quadrotora. Czas pomiaru: 1 h. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6.14 Stanowisko do pomiaru dokładności wyznaczonej orientacji. . . . . . . . . 6.15 Orientacja wyznaczona przez system podczas testów z robotem przemysłowym, dla wychyłu w osi roll. . . . . . . . . . . . . . . . . . . . . . . . . . 6.16 Orientacja wyznaczona przez system podczas testów z robotem przemysłowym, dla wychyłu w osi pitch. . . . . . . . . . . . . . . . . . . . . . . . . 6.17 Orientacja wyznaczona przez system podczas testów z robotem przemysłowym, dla obrotu w osi yaw. . . . . . . . . . . . . . . . . . . . . . . . . . . 42 . 42 . 44 . 45 . 45 . 46 Spis tabel 6.1 Nastawy przegubów robota przemysłowego w czasie pomiarów dla poszczególnych kątów. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43