Dziś pomówimy na temat systemu AHRS. Będzie on nam potrzebny w celu obliczenia naszego przemieszczenia względem ziemi. Dlaczego? Jak wspomniałem w poście na temat nawigacji bezwładnościowej do wyznaczenia przemieszczeń są nam potrzebne pomiary przyspieszeń w trzech osiach. W taki właśnie sposób dokonują pomiarów wszelkiego rodzaju czujniki wielkości fizycznych.

Jeśli chcemy obliczyć jak bardzo przemieściliśmy się na północ lub południe wykorzystamy przyspieszenia mierzone w osi y telefonu. Jeśli chcemy uzyskać informację o przemieszczeniach na wschód lub zachód wykorzystamy przyspieszenia mierzone w osi x. Analogicznie przyspieszenia mierzone w osi z wykorzystamy do obliczenia przemieszczeń góra dół. Takie ułożenie telefonu gwarantuje nam to, że jego układ współrzędnych jest zorientowany zgodnie z układem ziemskim.

Jest jedno ale

Powyższy tok rozumowania jest prawdziwy tylko w jednym przypadku. Mianowicie tylko wtedy, gdy oś y telefonu jest równoległa do ziemskich południków, oś x jest równoległa do ziemskich równoleżników a oś z jest prostopadła do powierzchni ziemi. Dobrze wiemy, że żaden użytkownik nawigacji nie będzie w stanie utrzymać telefonu właśnie w takiej pozycji przez cały czas jej użytkowania. To nierealne! Tutaj z pomocą przychodzi nam właśnie tytułowy AHRS.

Co to jest AHRS?

Jest to system obliczający swoją orientację przestrzenną na podstawie pomiarów pochodzących z akcelerometrów, żyroskopów (kąty pochylenia i przechylenia) i magnetometrów (kurs). Brzmi znajomo? Wszystkie te czujniki znajdują się w naszych smartfonach i tabletach. To właśnie dzięki temu systemowi urządzenie „wie” w jakiej orientacji aktualnie się znajduje. Poniższy film pokazuje prostą implementację sztucznego horyzontu w aplikacji Kompas na iOS, która korzysta właśnie z takiego systemu.

 Jak to się ma do INS?

Tak jak wspomniałem w pierwszym akapicie, czujniki w telefonie dokonują pomiarów w trzech osiach związanych z telefonem. AHRS posłuży nam do przeliczenia składowych mierzonych wielkości z układu związanego z telefonem na ten związany z ziemią. Poniższy film wyjaśni Ci czym są składowe wektorów i na czym polega ich rzutowanie.

Na dokładkę małe demo pokazujące jak wyglądają składowe tego samego wektora na obrócone względem siebie układy współrzędnych. Wyobraź sobie, że czerwone składowe są tymi mierzonymi przez czujniki telefonu a zielone są rzutami wektora na układ związany z ziemią. Niebieski wektor może przykładowo wyznaczać przyspieszenie działające na poruszającego się człowieka, który korzysta z naszej aplikacji.

Warto zwrócić uwagę na fakt, że ten sam wektor we wszystkich układach odniesienia posiada tę samą długość. Inne są jednak składowe, a o nie nam właśnie chodzi.

 Podsumowanie

Na początku chciałem stworzyć autorskie rozwiązanie oparte o prostą trygonometrię ale ma ono zasadniczą wadę. Istnieją takie szczególne przypadki, w których obliczanie kątów orientacji przestrzennej przy pomocy tej metody zwyczajnie nie działa. Powód jest prozaiczny – możliwość wystąpienia dzielenia przez zero. Kiedy? Otóż w momencie kiedy mierzone składowe na osi x lub y będą wynosiły zero.

$$
\theta = arctg \frac{a_z}{a_y}
\\
\phi = arctg \frac{a_z}{a_x}
$$

Z tego powodu zdecydowałem się na gotowe rozwiązanie. Jest nim implementacja algorytmu stworzonego przez Seba Madgwicka, która została udostępniona na GitHubie. Algorytm jest dość skomplikowany i chciałbym w przyszłości poświęcić mu więcej czasu aby dobrze go zrozumieć. Poniżej zamieszczam film prezentujący jego działanie.

Mam nadzieję, że dostatecznie dobrze wyjaśniłem istotę problemu. Tradycyjnie zapraszam do dyskusji.