Dawno temu, gdy dopiero co zacząłem studia, przytrafiła się okazja, aby na weekend wrócić do domu na wieś z dalekim krewnym, który właśnie kończył karierę studencką i na październik została mu obrona pracy magisterskiej. Tego dnia wracał własnym samochodem, więc umówiliśmy się, że zabierze mnie bezpośrednio z Politechniki. Pochwalił się, że zrobił symulację komputerową czegoś związanego z motoryzacją. To był początek lat 90. Komputerów PC praktycznie jeszcze nie było w domach, co najwyżej w instytucjach typu PW. Pojęcie symulacji komputerowej znałem z programów publicystyczno-naukowych typu „Sonda” i kojarzyło mi się z efektowną animacją. Jakież było moje rozczarowanie, kiedy kuzyn przyniósł składankę wydruków z drukarki igłowej, która nie miała nawet obrazków, tylko blade, niezrozumiałe kolumny cyferek, wydrukowane na różowym papierze.
Mam nadzieję, że po takim wstępie będziecie bardziej wyrozumiali dla owoców mojej pracy. Niedawno zacząłem robić filtr Kalmana kątów orientacji, mający zebrać dane z kilku kompletów czujników i wyznaczyć w możliwie najlepszy sposób kąty Eulera. Powstało coś, co działa, ale nie do końca tak, jak bym tego oczekiwał. Czy przyczyną są błędy algorytmu, kalibracji czujników, czy coś innego? Czujniki, a w szczególności magnetometry, na pewno nie działają idealnie w żelbetonowym pomieszczeniu.
Potrzebne jest coś pewnego, co mogłoby posłużyć za punkt odniesienia. Wymyśliłem więc, że utworzę wzorcowe wektory grawitacji i pola magnetycznego. Wektor grawitacji o wartości 9,81 m/s2 skierowany będzie pionowo w dół, a wektor magnetyczny o wartości 50,58 μT skierowany na północ z obrotem w osi Z o deklinację i w osi X o inklinację. Następnie będę nimi obracał, a składowe XYZ wektorów będą odpowiadały idealnym czujnikom. To dosyć szybko ruszyło, ale coś było nie tak jak trzeba. Wtedy zrozumiałem, że obracam światem wokół czujnika, a nie czujnikiem wokół świata. Trzeba było zmienić znak oraz kolejność obrotów z ZYX na XYZ i wreszcie zaczęło się zgadzać. Można więc powiedzieć, że z rozwojem autopilota jestem już na etapie kopernikańskiego, późnego średniowiecza.
Na obrazku są pokazane 4 grupy wykresów. Pierwsze trzy to kąty φ (Phi), θ (Theta) i ψ (Psi) liczone różnymi metodami. Kolor czerwony mający w legendzie przedrostek „Trygono” to filtr komplementarny, łączący w stałej proporcji 0,98 do 0,02 całkę prędkości kątowej żyroskopów z kątami obliczonymi metodą trygonometryczną z wektorów akcelerometru i magnetometru. Kolor zielony z przedrostkiem „Kwatern” to też filtr komplementarny, ale liczony przy użyciu algebry kwaternionowej moim autorskim pomysłem. Proporcja składowych jest ustalana dynamicznie za pomocą filtra adaptacyjnego, w zależności od wielkości zakłóceń typu przeciążenie od siły odśrodkowej. Im większe zakłócenia, tym mniejszy udział korekcji akcelerometrem i magnetometrem. W symulacji nie ma zakłóceń, więc tutaj jest pełna korekta. Na małym obrazku tego nie widać, ale na pełnym ekranie widać różnicę w zaokrągleniu ostrego przejścia przy ±π. Finalnie kwaternion przeliczany jest na kąty Eulera. Kolor granatowy opisany jako „Kąt BSP” to kąty obliczone filtrem Kalmana, również przy użyciu kwaternionów. W ogólności struktura „BSP” zbiera finalne parametry uzyskane na podstawie wielu czujników, głównie filtrami Kalmana. Kolor cyjan to wzorzec kąta pochodzący z symulatora, który jest punktem odniesienia dla innych metod.
Ostatnia grupa to składowe symulowanych czujników. Składowe XYZ magnetometru narysowane są w kolorach RGB, a akcelerometru w kolorach CMY. Symulator obecnie ma możliwość obracania wektorów wokół wybranej osi. Ponieważ dane przesuwają się na wykresie z lewej do prawej, więc symulacja z prawej strony to pierwsza rotacja wokół osi X. Kąt przechylenia Phi cały czas maleje, a gdy dojdzie do wartości -π, to przeskakuje na +π, generując charakterystyczny piłokształtny przebieg. W tym czasie kąty pochylenia i odchylenia wzorcowo leżą w zerze dla wszystkich metod obliczeniowych.
Środkowa część wykresu to rotacja wokół osi Y. Tutaj nie jest tak różowo. Tylko kąt z symulatora zmienia się w pełnym zakresie, a wszystkie inne kąty operują w zakresie ±π/2. Jest kilka przyczyn tego zachowania. Dla kwaternionów problem leży w metodzie przeliczania na kąty Eulera. We wzorze jest arkus sinus, a on daje wyniki w takim właśnie zakresie. Trygonometria i kwaterniony dają piłę na połowie kąta, natomiast Kalman zachowuje się jeszcze inaczej. Krytyczna jest chwila, gdy BSP znajduje się pionowo dziobem w dół lub w górę. Po dalszym pochyleniu się od tej wartości pojawia się niejednoznaczność, czyli równie możliwe są dwa położenia: (φ, θ, ψ) oraz (π+φ, π-θ, π+ψ). Próbowałem dwóch metod obliczenia tego, który zestaw kątów jest tym właściwym, polegających na obliczeniu długości drogi między kolejnymi iteracjami. Na mój mały rozumek to ma szansę działać, ale na razie jest kicha, bo zmieniają się jednocześnie dwa kąty. Można wybrać właściwy zestaw danych na podstawie innego kąta. Tak robię z przechyleniem, gdyż ono jest ważniejsze, ale nie da się uzależnić jednocześnie przechylenia od pochylenia i pochylenia od przechylenia, bo proces się zapętla. Podczas obrotu dla części algorytmów: trygonometrii i kwaternionów przechylenie skacze od 0 do π, dla Kalmana przeskakuje z -π na +π, czyli praktycznie nie ma zmiany, chociaż sam kąt jest niewłaściwy. W odchyleniu też dzieją się dziwne rzeczy.
Obroty wokół osi Z są najtrudniejsze, bo składowe pochylenia i przechylenia są zerowe, więc jeżeli tam są problemy, to iloczyn z zerem je maskuje. Natomiast widać, że trygonometria i kwaterniony, które nie mają uwzględnionej deklinacji i inklinacji, idą jedną ścieżką, a Kalman i symulacja inną, przesuniętą o wartość deklinacji. Jeżeli pochylenie i przechylenie obejrzę w dużym powiększeniu, to tam też są widoczne sinusoidy o amplitudzie 0,002 radiana. Jeszcze nie rozumiem, skąd się one biorą, ale to pokazuje piękno symulacji, pozwalającej badać nawet takie mikroskopijne szczegóły, niezauważalne na rzeczywistych czujnikach. Widać to na obrazku poniżej:
To, co pokazałem, to dopiero początek. Teraz trzeba zweryfikować, jak algorytmy radzą sobie z kompensacją przechylenia i pochylenia magnetometru, czyli trzeba zadać stały niewielki kąt w jednej osi, a obracać wokół innej osi. Potem trzeba sprawdzić złożenie wielu obrotów jednocześnie. Przydałoby się też dopracować metodę liczenia kątów z kwaterniona. Oj, nie będę się nudził w tej ciemniejszej połowie roku. Gdybym uparł się latać, to korzystając z metody Kalmana mogę już wykonać beczkę i lot płaski z pochyleniami nie przekraczającymi ±90°. Obecnie nie poradzę sobie z lotem plecowym i pętlą.
Na koniec jeszcze spostrzeżenie, że jeżeli komuś przed przeczytaniem wstępniaka symulacja zaczęła kojarzyć się z symulatorem lotu, to dobrze, bo połączenie z symulatorem jest robione w ten sam sposób. Wystarczy w ten sposób podłączyć trochę bardziej efektowne narzędzie oraz odesłać mu odpowiedź autopilota i takie połączenie powinno działać w układzie znanym jako HIL (ang. Hardware In the Loop).