W poście „AHRS czyli punkt widzenia zależy od punktu siedzenia” pisałem o tym, że zdecydowałem się na użycie algorytmu Madgwicka do obliczania orientacji przestrzennej w celu rzutowania wektora przyspieszenia na układ związany z Ziemią. Wspomniany algorytm daje nam informację o rotacjach w postaci tytułowego kwaternionu. Nie zagłębiając się w szczegóły czym konkretnie są kwaterniony wyjaśnię na czym polega obracanie wektorów przy ich pomocy.

Macierz obrotu

Aby obrócić wektor w przestrzeni należy pomnożyć odpowiednio przygotowaną macierz przez obracany wektor. Wygląda to w ten sposób:

$$\overrightarrow{p}’=R \overrightarrow{p}$$

gdzie:
p’ to obrócony wektor,
p to obracany wektor,
R to macierz obrotu.

Jak wyglądają takie macierze?

Kąty Eulera

Aby obrócić wektor przy pomocy kątów Eulera musimy przeprowadzić trzy obroty względem każdej osi układu współrzędnych:

Poniżej znajdują się macierze obrotu umożliwiające obracanie wektora względem danej osi.

$$
\begin{alignat}{1}
R_x(\theta) &= \begin{bmatrix}
1 & 0 & 0 \\
0 & \cos \theta & -\sin \theta \\[3pt]
0 & \sin \theta & \cos \theta \\[3pt]
\end{bmatrix} \\[6pt]
R_y(\theta) &= \begin{bmatrix}
\cos \theta & 0 & \sin \theta \\[3pt]
0 & 1 & 0 \\[3pt]
-\sin \theta & 0 & \cos \theta \\
\end{bmatrix} \\[6pt]
R_z(\theta) &= \begin{bmatrix}
\cos \theta & -\sin \theta & 0 \\[3pt]
\sin \theta & \cos \theta & 0\\[3pt]
0 & 0 & 1\\
\end{bmatrix}
\end{alignat}
$$

gdzie θ to kąt obrotu.

W naszym przypadku wyliczanie kątów Eulera na podstawie przyspieszeń jest kłopotliwe ze względu na możliwość wystąpienia dzielenia przez zero:
$$
\theta = arctg \frac{a_z}{a_y}
\\
\phi = arctg \frac{a_z}{a_x}
$$

Kwaterniony

Tutaj z pomocą przychodzą nam tytułowe kwaterniony i algorytm Madgwicka, które są pozbawione powyższej wady. W dużym uproszczeniu rotacja wykonana przy pomocy kwaternionu jest zrealizowana przez jeden obrót wokół wyznaczonej osi. Poniższy film powinien Ci nieco rozjaśnić jak odbywa się ten proces:

Macierz rotacji powstała na podstawie kwaternionu będzie wyglądała w następujący sposób:

$$
Q = \begin{bmatrix}
1 – 2 y^2 – 2 z^2 & 2 x y – 2 z w & 2 x z + 2 y w \\
2 x y + 2 z w & 1 – 2 x^2 – 2 z^2 & 2 y z – 2 x w \\
2 x z – 2 y w & 2 y z + 2 x w & 1 – 2 x^2 – 2 y^2
\end{bmatrix}
$$

gdzie w, x, y, z to składowe kwaternionu.

Kod

Na potrzeby projektu stworzyłem klasę wektora, która ma kilka przydatnych właściwości:

• Klasa obsługuje kilka podstawowych operatorów. Przykładowo wektory można przez siebie mnożyć, dodawać je do siebie czy odejmować.
• Wektory można ze sobą porównywać przy pomocy operatorów ==, !=, >, <, >=, <=.
• Wektory można mnożyć z macierzami 3×3.
• I co najważniejsze. Wektor może być obracany przy pomocy zadanego kwaternionu.

Poniżej kod klasy:

Przy okazji powstały jeszcze dwie pomocnicze klasy opisujące kwaterniony i macierz 3×3.

Podsumowanie

Nie jestem do końca zadowolony z tego tekstu ale mam nadzieję, że chociaż odrobinę rozjaśniłem temat kwaternionów. Cieszy mnie jednak to, że mój kod zaczął poprawnie działać mimo wielu godzin mozolnej dłubaniny. Nie udałoby się to gdyby nie zastosowanie niezawodnych testów jednostkowych, które oszczędziły sporo mojego zdrowia.

Podczas pracy nad kodem natknąłem się na bardzo przyjemną wizualizację działania kwaternionów pod adresem http://quaternions.online/.

Mam nadzieję, że kolejnym etapem projektu będzie już złożenie poszczególnych elementów w całość i stworzenie algorytmu nawigacji inercyjnej.