Projekt układu AHRS dla małego quadrotora - KoNaR

Transkrypt

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
ĝ = 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
â = 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 ĝ, L â) = EL q̂ E ĝ EL q̂ ∗ − L â
(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 ĝ, L â)
, dla k = 0,1,2,3...n
−µ
k 5 f (EL qˆk , E ĝ, L â)k
5f (EL qˆk , E ĝ, L â) = J T (EL qˆk , E ĝ)f (EL qˆk , E ĝ, L â)
(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 ĝ, L â, b̂, L m̂) =
E
Jg,b (EL q̂, b̂)
4.2.2
"
=
fg (EL q̂, E ĝ, L â)
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.
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
Czas pomiaru: 15 min.
Czas pomiaru: 1 h.
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.
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

Projekt układu AHRS dla małego quadrotora - KoNaR

Transkrypt

Podobne dokumenty