Od kilku dni pracuję nad AHRS, czyli podsystemem, który dostarcza informację o kątach orientacji w przestrzeni 3D: przechylenia (ang. roll), pochylenia (pitch) i odchylenia (yaw). Gdy samolot zna swoje kąty orientacji, może za pomocą regulatorów PID tak się sterować, aby utrzymać kąty wymagane w na danym etapie lotu. Tak więc jest to jeden z najważniejszych podsystemów dla wszystkiego, co aktywnie lata, od wielowirnikowca po rakietę.
Pierwsze rozwiązanie powstało przy okazji robienia jednostki inercyjnej. Trzeba było jakoś weryfikować czujniki, sprawdzić na ile stabilne są ich wskazania, itp. To jest rozwiązanie na filtrze komplementarnym, gdzie z jednej strony całkowane są prędkości kątowe z żyroskopów, budując kąty dla każdej osi a z drugiej, poszczególne składowe wektora przyspieszenia są wstawiane do równań trygonometrycznych i w ten sposób obliczana jest druga wersja kątów. Kąty uzyskane z żyroskopów są znacznie mniej wrażliwe na drgania mechaniczne, nie wpływają na nie zakłócenia wywołane zmianą prędkości w czasie manewrów, ale nie są stabilne długookresowo, czyli płyną w funkcji temperatury. W dużej mierze daje się to skompensować rozbudowaną kalibracją, ale nigdy nie jest idealnie. Kąty uzyskane z akcelerometru, oraz magnetometru opierają się na konkretnych wielkościach fizycznych i dzięki temu są stabilne długookresowo. Choćby pomiar był zaszumiony, to poza chwilami zmiany prędkości, średnia grawitacji będzie zawsze wskazywała na ziemię a średnia magnetyczna na północny biegun magnetyczny. Niestety takie pomiary są podatne na zakłócenia np. przyspieszeniami wywołanymi zmianą prędkości, lub lokalnymi zaburzeniami pola magnetycznego. Najgorszą cechą jest to, że zwykłe drgania mechaniczne wywołane pracą silnika, niewyważonym śmigłem, czy turbulencjami, przekładają się bezpośrednio na liczony kąt i taki pomiar jest mocno zaszumiony. Szum można odfiltrować, ale to opóźnia fazę, przez co trudno jest sterować obiektem z opóźnieniem w pętli sterowania. Filtr komplementarny bierze dużą część kąta (powiedzmy 98%) kąta z żyroskopów i dopełnia to kątem z akcelerometrów tyle, aby skompensować ewentualny dryft i żeby składowa z szumem nie była zbyt duża. Bardziej zaawansowane rozwiązania, np. filtr Kalmana, sam dobiera sobie proporcję danych, minimalizując błędy.
To rozwiązanie jest intuicyjne, da się na tym latać, ale ma kilka znanych wad. Jedną z nich jest to, że funkcje trygonometryczne są ułamkiem, gdzie po obu stronach dzielenia są długości kątów trójkąta, jaki tworzą składowe wektora przyspieszenia, lub wektora magnetycznego. W pewnych sytuacjach jeżeli długość składowej wektora znajdującej się w mianowniku jest zerowa, to następuje niedozwolona operacja dzielenia przez zero. Wynik takiej operacji wykonywanej w mikrokontrolerze jest nieokreślony (NaN - Not a Number). Ten wynik z AHRS idzie do dalszych podsystemów sterowania i rozchodzi się lawinowo po całym systemie, zwykle zatrzymując jego pracę a w praktyce nasz bezzałogowiec spada.
Takie punkty nieokreślone można obchodzić, stosując warunki badające czy mianownik jest zerem. Tak działa funkcja atan2(x, y), dając bezpieczny wynik, w odróżnieniu od klasycznej funkcji atan(y/x), która wysypie się dla x=0. W obu przypadkach chodzi oczywiście o arkus tangens. Funkcje trygonometryczne zajmują też dosyć dużo mocy obliczeniowej. Przy współczesnych mikrokontrolerach nie jest to krytyczne, ale są inne metody bezpieczniejsze matematycznie i prostsze obliczeniowo. To są kwaterniony. Jeszcze do niedawna, to była dla mnie czarna magia, o której nawet nie wspominano w szkole, ale gdy zacząłem szukać, to znalazłem kilka fajnych wytłumaczeń jak to działa. Jeżeli jeszcze nie wiesz co co chodzi z kwaternionami, to na początek polecam film dr. Tomasza Millera o historii i podstawach kwaternionów, a już po tym film Mateusza Kowalskiego o algorytmach obrotu w przestrzeni 3D przy użyciu kwaternionów. Jest tego więcej, ale po obejrzeniu tych dwu, już będzie wiadomo czego szukać dalej.
Już nie pamiętam co było wersją AHRS nr 2, ale wersje 3 i 4 działają na kwaternionach. Wersja 3, tworzy kwaterniony definiujące obroty na podstawie danych z żyroskopu i tymi kwaternionami obraca wektory: przyspieszenia ziemskiego, oraz magnetyczny. Odczyt kątów jest nadal robiony metodami trygonometrycznymi. Wersja 4 oprócz 3 kwaternionów definiujących obrót, ma jeszcze 2 kwaterniony przechowujące wspomniane wektory przyspieszenia i magnetyczny. Wszystkie obliczenia, oraz odczyt kątów są robione na kwaternionach. Wiele algorytmów AHRS robi fuzję czujników, łącząc dane z akcelerometru i magnetometru, ale moje obecne doświadczenie pokazuje, że taka fuzja bywa źródłem przykrych błędów, gdzie niewłaściwie skalibrowany magnetometr powoduje, że kąty nie trzymają zera i trudno zgadnąć o co chodzi i co trzeba poprawić aby było dobrze. Dlatego na tym etapie rozwoju nie robię fuzji tych dwu czujników. Kąty pochylenia i przechylenia będę odczytywał z danych akcelerometru a kąt odchylenia z danych magnetometru. Nadal zostaje filtr komplementarny. W wersji 3 z wektorami, implementacja filtra jest banalna i jest już zrobiona, w wersji 4 jeszcze nie ma filtra, ale wydaje się że nie powinno być z tym kłopotu.
Dosyć dużym problemem okazało się odczytanie kątów z kwaternionu. Niby są na to wzory, ale występują pewne efekty uboczne, gdzie zmiana jednego kąt implikuje zmianę innego. Zrobiłem symulację, gdzie w każdym kroku obracam wektory grawitacji i magnetyczny o niewielki kąt, w wyniku czego powinny obracać się płynnie w danej osi. Z odchyleniem nie ma problemów, ale dla pochylenia i przechylenia, obie wersje dają inne wyniki. Dla wersji 3, kąty wyglądają tak jak na obrazku niżej. Wykres czerwony to obrót wokół osi Y, czyli symulowane pochylenie. Widać że zmienia się w zakresie od -π do +π, czyli występuje pełen obrót o 360°, ale dodatkowo w wierzchołkach gdzie samolot jest skierowany pionowo w górę, oraz w zerze jest przeskok przechylenia między -π/2 a +π/2
Dla wersji 4 jest trochę inaczej, bo zmiana pochylenia jest w zakresie +/- π/2, czyli nie ma pełnego obrotu a dodatkowo zmiana przechylenia ma aż 3 stany.
Na pierwszy rzut oka wersja 3 wydaje się lepsza niż 4, ale trochę głębsza analiza pokazuje że jednak nie. Jestem człowiekiem o małej wyobraźni, dlatego muszę posługiwać się wizualizacjami, aby mogły dotrzeć do mnie pewne rzeczy. Jeszcze z czasów prac rozwojowych nad APL1 został mi taki model samolotu, przeznaczony do lotów w wyobraźni.
Jeżeli teraz weźmiemy nasz samolot i rozpoczniemy lot od punktu na wykresie, gdzie kąt pochylenia jest zerowy (samolot jest poziomo) i rośnie, to samolot będzie powoli zadzierał swój dziób. W wersji 3 będzie go zadzierał aż osiągnie +π, czyli zrobi fikołek przez plecy i będzie leciał poziomo do tyłu, do góry kołami. W tym czasie nastąpi zmiana kąta przechylenia z π/2 na -π/2, czyli o 180°. Koła wrócą w stronę ziemi, ale stanie się to dopiero gdy samolot wypłaszczy lot do góry kołami. Kolejna zmiana przechylenia będzie dla zerowego kąta. Zmiana przy zerowym kącie, czyli w typowej sytuacji, to przykra sprawa, bo występuje bardzo często. Dodatkowo kąt przechylenia zmienia się z 90° na -90°, czyli z lotu na żyletkę z jednym skrzydłem w górę, na lot na żyletkę z przeciwnym skrzydłem w górę. Trzeba było by przesunąć fazę obu kątów o 90° aby działo się to w sytuacjach gdy dziób jest pionowo w górę, lub w dół.
Dla wersji 4 gdy zaczniemy od kąta zerowego i zadzieramy dziób, to dla π/2 czyli pionowo w górę, nastąpi zmiana przechylenia o π czyli 180°, czyli to, co trzeba było by zrobić dla wersji 3. Po tym, gdy samolot zrobi obrót wokół osi X (podłużnej), kąt zaczyna maleć, aż do chwili gdy dziób znajdzie się pionowo w dół. Tam dzieje się analogiczny obrót wokół osi X w drugą stronę i nadal koła mamy od spodu. Ale co ze zmianą w zerze, którą widać dla czasu 27 i 53? Tak duża zmiana kąta w zerze to przykra sprawa, ale to jest przecież zmiana o 2π, czyli pełen obrót, czyli de fakto brak zmiany, bo gdyby wyciąć z papieru ten wykres i skleić go w rurkę, górny brzeg z dolnym, to wartość -π jest tym samym co +π, bo jest to funkcja okresowa. Może istnieją lepsze rozwiązania dające kąty, ale wygląda na to, że wersja 4 jest na tym etapie akceptowalna. Nie da się tym algorytmem zrobić figury zwanej beczką, ale na tym etapie mogę to przeboleć.
Jeżeli chodzi o symulację drugiego kąta, czyli przechylenia, to dla wersji 3 wygląda to tak jak niżej. Zmiana jest o pełen kąt 2π a w maksimum i w zerze, pozostałe 2 kąty obracają się o π, czyli 180°. Biorę gąbkę z samolotem i zaczynam od płaskiego lotu. Kąt rośnie, więc pochylam się na prawe skrzydło dotąd, aż zrobię 180°, czyli koła są do góry nogami. Wtedy następuje obrót o 180° w pochyleniu, czyli dziób obraca się do tyłu, ale jest też obrót o 180° w odchyleniu, czyli dziób wraca tam gdzie być powinien. Niestety zmiany dzieją się również w zerze, czyli w normalnym locie. Trzeba było by tak jak wcześniej ręcznie przesunąć fazę o 90°.
Natomiast wersja 4 ma zmianę tylko jednego kąta i to o wartość 2π czyli de fakto nie ma żadnych zmian. Jeszcze jedną różnicą jest to, że w wersji 3 kąt rośnie a w wersji 4 maleje, ale to wydaje się być niewielką zmianą, bo wystarczy odwrócić znak, co dla kwaternionów jest zmianą kolejności mnożenia. Analogicznie dla kąta odchylenia, obie wersje różnią się tylko znakiem, więc już ich nie pokazuję.
Tak wiec do dalszych prac przechodzi wersja 4, gdzie trzeba będzie zrobić filtr komplementarny na kwaternionach, żeby można było tak obracany kwaternion synchronizować z danymi akcelerometru i magnetometru. Te dane trzeba będzie zapakować w kolejny kwaternion, wrzucając składowe przyspieszeń w osiach X, Y i Z do jego części urojonej (i, j, k), następnie spróbować dodać w proporcji określonej filtrem komplementarnym do tego, co było tutaj obracane.
Porównanie metod
Poniżej przedstawiam wyniki porównania 4 metod liczenia kątów orientacji dla procedury testowej gdzie dla wszystkich rodzai kątów zmieniam kąt o około +-45°, potem jest zmiana o +-90° przez położenie na przeciwległych bokach i na koniec przekroczenie wartości +-180° aby zobaczyć czy nie ogranicza nas matematyka. We wszystkich poniższych testach porównywane są katy liczone w następujący sposób:
1) Żółty wykres to wynik filtra komplementarnego gdzie przeważająca większość jest całką z żyroskopu a reszta wynikiem funkcji trygonometrycznej. Dla pochylenia jest to tangens (Z/X) osi akcelerometru, przechylenia tangens (Z/Y) akcelerometru a dla odchylenia tangens (Y/X) osi magnetometru.
2) Wykres czerwony jest uzyskany powyższymi metodami z kwaternionu wektora przyspieszenia ziemskiego obracanego kwaternionami obrotu będącymi całką z żyroskopu. Tutaj jest analogiczny filtr komplementarny tylko na etapie kwaternionów.
3) Wykres niebieski to kąty z samej trygonometrii na bazie akcelerometru i magnetometru bez wpływu żyroskopów, czyli poprawna wartość a zła dynamika
4) Wykres zielony to kąty z będące całką z samego żyroskopu, czyli dobra dynamika ale wartość bezwzględna płynie w czasie, chociaż starałem się aby była poprawna.
Nie ma na wykresach tego na czym najbardziej mi zależało, czyli kąta uzyskanego z operacji na kwaternionach i odczytywanego z kwaternionów. Ta metoda ma ograniczenie do 180° i na razie brakuje mi umiejętności aby to pokonać. Dopóki metoda nie będzie zwracała poprawnego wyniku dla pełnego kąta, nie nadaje się do zastosowania w AHRS.
Przebieg zmian na kącie pochylenia. Jedyne co widać w pełnej skali to zauważalny szum niebieskiego wykresu czyli kąta z samych akcelerometrów.
Dla kąta przechylenia jest to samo.
Aby móc zobaczyć coś więcej, zarejestrowałem zmianę o kąt około 2°. W takim powiększeniu dopiero widać że zielony wykres całki z prędkości kątowej płynie. Wyraźnie też widać szum kąta z samych akcelerometrów. Przez to że skale dla wykresów są inne, jest to dosyć słabe narzędzie do porównywania wyników.
W przypadku kata odchylenia, gdzie kat jest czytany z magnetometru, jest wyraźnie gorzej. Jeżeli za wzorzec dynamiki kata przyjmiemy wykres z całki z prędkości kątowej żyroskopu, czyli zielony, to wszystkie inne katy mają jakieś problemy. Najprawdopodobniej jest to efekt zakłóceń pola magnetycznego w budynku. Wykres czerwony odstaje najwięcej, ale on jest poddany największej obróbce, ponieważ w obecnym etapie jest liczony z magnetometru skompensowanego o pochylenie i przechylenie (patrz artykuł o magnetometrach). Trzeba też mieć na uwadze to że skala wykresu czerwonego jest trochę mniejsza niż pozostałych, więc należało by go trochę rozciągnąć i podnieść do góry.
