Kwaterniony w kontrolerze lotu

Kwaternion jednostkowy jest zwartą reprezentacją orientacji 3D używaną w estymatorach i regulatorach UAV. Nie cierpi na osobliwość kątów Eulera i pozwala sprawnie składać obroty, ale nie usuwa potrzeby zdefiniowania ramek, znaków i kierunku transformacji. Większość błędów implementacji nie wynika z trudnej algebry, lecz z mieszania konwencji: kolejności wxyz/xyzw, obrotu aktywnego i pasywnego, mnożenia lewo-/prawostronnego oraz znaczenia q_AB.

Spis treści#

Obrót 3D i SO(3)
Definicja kwaternionu
Oś i kąt
Kwaterniony jednostkowe
Iloczyn Hamiltona
Odwrotność i sprzężenie
Obracanie wektora
Macierz obrotu
Kąty Eulera
Podwójne pokrycie
Kierunek transformacji
Integracja żyroskopu
Krok przez mapę wykładniczą
Błąd orientacji
Sterowanie attitude
Interpolacja SLERP
Średnia orientacji
Stabilność numeryczna
Interfejs programistyczny
Testy jednostkowe
Najczęstsze pułapki
Powiązane tematy
Przypisy

Obrót 3D i SO(3)#

Macierze czystego obrotu tworzą specjalną grupę ortogonalną:

SO(3) = { R ∈ R³ˣ³ : RᵀR = I, det(R)=+1 }

Warunek ortogonalności zachowuje długości i kąty, a wyznacznik +1 odróżnia obrót od odbicia. Orientacja ma trzy stopnie swobody, lecz globalnie nie da się jej opisać trzema gładkimi parametrami bez osobliwości.

Kąty Eulera są lokalnymi współrzędnymi orientacji. Macierz ma dziewięć liczb i sześć więzów. Kwaternion ma cztery liczby i jeden więz normy. Nie jest „czterowymiarową orientacją”; jest parametryzacją trójwymiarowego obrotu na sferze jednostkowej S³, przy czym dwa przeciwne punkty opisują ten sam obrót.

W regulatorze stan powinien ewoluować zgodnie z geometrią SO(3). Dodawanie kątów Eulera jak niezależnych współrzędnych jest dopuszczalne dla małych lokalnych błędów, nie dla ogólnego składania orientacji.

Definicja kwaternionu#

Kwaternion zapisuje się jako część skalarną i wektorową:

q = [w, v] = [w, x, y, z]

Niektóre biblioteki stosują kolejność [x,y,z,w]. Dane binarne bez metadanych są zatem niebezpieczne. Nazwa typu i serializacja powinny jawnie określać kolejność.

Jednostki urojone spełniają:

i² = j² = k² = ijk = -1
ij = k, jk = i, ki = j
ji = -k, kj = -i, ik = -j

Nieprzemienność jest kluczowa. q1 ⊗ q2 na ogół różni się od q2 ⊗ q1, tak jak dwa obroty wokół różnych osi wykonane w odwrotnej kolejności.

Oś i kąt#

Obrót o kąt θ wokół znormalizowanej osi u reprezentuje:

q = [cos(θ/2), u sin(θ/2)]

Półkąt wynika z geometrii reprezentacji. Dla θ=0 otrzymujemy kwaternion jednostkowy [1,0,0,0]. Dla obrotu 180° część skalarna jest zero, a część wektorowa wskazuje oś.

Odwrotnie, dla znormalizowanego q:

θ = 2 atan2(||v||, w)
u = v / ||v||

Gdy ||v|| jest bliskie zeru, oś jest nieokreślona, bo obrót jest zerowy. Kod musi obsłużyć ten przypadek bez dzielenia przez zero.

Do reprezentacji najkrótszego błędu często wymusza się w ≥ 0, zmieniając znak całego kwaternionu. Nie zmienia to orientacji, ale wybiera kąt z zakresu do π.

Kwaterniony jednostkowe#

Norma:

||q|| = sqrt(w² + x² + y² + z²)

Kwaternion obrotu musi mieć normę 1. Mnożenie kwaternionów jednostkowych zachowuje normę w arytmetyce dokładnej. W float pojawia się dryf, dlatego wynik się normalizuje.

Normalizacja nie może być bezwarunkowa. Dla normy bliskiej zero albo NaN należy zgłosić błąd i użyć kontrolowanej strategii odzyskania. Zastąpienie dowolnego złego stanu kwaternionem jednostkowym powoduje nagły reset attitude i musi być widoczne dla regulatora.

W funkcji fast inverse sqrt trzeba znać jej dokładność i zakres. Oszczędność kilku cykli nie uzasadnia ukrycia niestabilności w krytycznej orientacji. Na współczesnym MCU z FPU zwykłe sqrtf może być wystarczające.

Iloczyn Hamiltona#

Dla q1=[w1,v1] i q2=[w2,v2]:

q1 ⊗ q2 = [w1w2 - v1·v2,
           w1v2 + w2v1 + v1×v2]

W składowych:

w = w1w2 - x1x2 - y1y2 - z1z2
x = w1x2 + x1w2 + y1z2 - z1y2
y = w1y2 - x1z2 + y1w2 + z1x2
z = w1z2 + x1y2 - y1x2 + z1w2

Znaczenie kolejności zależy od umowy, czy kwaternion działa aktywnie na wektor i jak reprezentowane są macierze. Algebra Hamiltona jest stała; interpretacja q1⊗q2 jako „najpierw q2, potem q1” wynika z przyjętego działania na kolumny.

Biblioteka powinna zawierać test dwóch obrotów 90° wokół różnych osi. Test samych obrotów wokół tej samej osi nie wykryje zamienionej kolejności.

Odwrotność i sprzężenie#

Sprzężenie:

q* = [w, -x, -y, -z]

Odwrotność ogólnego kwaternionu:

q⁻¹ = q* / ||q||²

Dla kwaternionu jednostkowego odwrotność równa się sprzężeniu. Reprezentuje obrót przeciwny. Jeżeli q_AB przenosi współrzędne z B do A, q_BA = q_AB⁻¹.

Założenie jednostkowej normy w funkcji inverse() powinno być zawarte w typie lub sprawdzone. Użycie samego sprzężenia dla nienormalnego stanu nie daje poprawnej odwrotności i może skalować obracany wektor.

Obracanie wektora#

Wektor p zapisuje się jako czysty kwaternion [0,p]. Dla aktywnego obrotu jedną z konwencji jest:

p' = q ⊗ [0,p] ⊗ q*

Dla zmiany współrzędnych między ramkami może być potrzebna odwrotna kolejność. Nazwy API powinny mówić rotate_body_to_ned() zamiast ogólnego rotate().

Wydajna postać bez dwóch pełnych mnożeń:

t = 2(v × p)
p' = p + w t + v × t

Zakłada kwaternion jednostkowy i tę samą konwencję. Optymalizację wprowadza się po testach wersji referencyjnej.

Obrót musi zachowywać normę wektora. Jest to dobry test własności dla losowych kwaternionów i wektorów.

Macierz obrotu#

Dla aktywnej konwencji zgodnej z q p q* jedna z postaci macierzy wynosi:

R = [1-2(y²+z²)  2(xy-wz)    2(xz+wy)
     2(xy+wz)    1-2(x²+z²)  2(yz-wx)
     2(xz-wy)    2(yz+wx)    1-2(x²+y²)]

Transpozycja opisuje transformację odwrotną. W źródłach spotyka się obie wersje, bo różnią się znaczeniem q i tym, czy wektory są kolumnami czy wierszami. Nie wolno wybierać wzoru tylko po identycznych symbolach.

Walidacja:

RᵀR ≈ I
det(R) ≈ 1
R(q*) ≈ R(q)ᵀ
R(q1⊗q2) ≈ R(q1)R(q2)

NASA publikuje relacje pomiędzy kątami Eulera, kwaternionami i macierzami, ale nawet materiał lotniczy musi zostać dopasowany do konwencji konkretnego oprogramowania.

Kąty Eulera#

Konwersja do roll/pitch/yaw wymaga sekwencji, zwykle 3-2-1 yaw–pitch–roll, ale nazwy bywają różnie interpretowane. Dla określonej macierzy wzory wykorzystują atan2 i asin. Argument asin należy ograniczyć do [-1,1] z powodu błędu zmiennoprzecinkowego.

Przy pitch bliskim ±90° roll i yaw nie są osobno jednoznaczne. Kwaternion nadal reprezentuje orientację poprawnie; osobliwość pojawia się przy prezentacji w kątach Eulera.

Regulator nie powinien różniczkować wyświetlanych kątów, ponieważ wrap yaw z +π do -π tworzy sztuczny skok. Błąd orientacji liczy się kwaternionem względnym lub macierzą.

Kąty są przydatne dla interfejsu operatora, limitów mechanicznych i małych lokalnych poleceń. Powinny być wynikiem stanu, nie głównym formatem jego propagacji.

Podwójne pokrycie#

q i -q opisują tę samą macierz obrotu. Surowa odległość euklidesowa między nimi wynosi 2 mimo zerowego błędu orientacji. Porównanie powinno używać:

similarity = |dot(q1, q2)|

Kąt względny można wyznaczyć z wartości bezwzględnej iloczynu skalarnego po clampie. Dla ciągłego logu często wybiera się znak nowej próbki tak, aby dot(q_prev,q_new) ≥ 0.

Interpolacja liniowa bez korekty znaku może przejść przez zero pomiędzy q i -q, mimo że orientacje są identyczne. SLERP powinien odwrócić jeden kwaternion dla najkrótszej drogi.

W EKF znak stanu nominalnego można zmienić, ale trzeba zachować spójność wszystkich operacji. Fizyczna orientacja się nie zmienia.

Kierunek transformacji#

Przyjmijmy jednoznaczną definicję:

q_AB: p_A = q_AB ⊗ p_B ⊗ q_AB*

Wtedy q_AB zamienia współrzędne z B do A. Składanie:

q_AC = q_AB ⊗ q_BC

Jeśli orientacja pojazdu jest opisana jako body→NED, wektor grawitacji z NED do body wymaga odwrotności. Nazwa attitude bez indeksów nie wystarcza.

PX4, ROS, MAVLink i biblioteki graficzne mogą stosować różne układy oraz kolejności składowych. Granica protokołu powinna wykonywać jawne mapowanie. Nie należy reinterpretować pamięci jednej struktury jako drugiej.

Dokumentacja funkcji powinna zawierać równanie wejścia i wyjścia, a test — wektor bazowy. To jest skuteczniejsze niż opis „obraca z world do body” bez definicji world.

Integracja żyroskopu#

Żyroskop podaje ω w osiach sensora/body. Po odjęciu biasu równanie kwaternionu dla jednej konwencji brzmi:

q_dot = 1/2 q ⊗ [0,ω]

Dla odwrotnej definicji attitude może być [0,ω]⊗q ze znakiem zależnym od zmiany ramki. Jedyny bezpieczny sposób to wyprowadzenie z definicji oraz test stałego obrotu.

Prosty krok Eulera:

q[k+1] = normalize(q[k] + 0.5(q[k]⊗ωq)Δt)

ma błąd rzędu zależny od Δt i prędkości. Przy 1 kHz i typowych rate może być wystarczający, ale zgubiona próbka tworzy duży krok. Lepsza integracja wykorzystuje przyrost kąta z sensora lub mapę wykładniczą.

ω musi być w radianach na sekundę. Stopnie na sekundę są częstą przyczyną gwałtownej rotacji wyniku.

Krok przez mapę wykładniczą#

Dla wektora rotacji:

δθ = ω Δt
φ = ||δθ||

kwaternion przyrostu:

δq = [cos(φ/2), (δθ/φ) sin(φ/2)]

Dla małego φ używa się stabilnej aproksymacji sin(φ/2)/φ ≈ 1/2 - φ²/48, unikając dzielenia przez zero. Następnie:

q_next = q ⊗ δq

lub odwrotna kolejność zgodnie z ramką rate. Metoda dokładnie reprezentuje stałą prędkość w przedziale. Rzeczywiste ω zmienia się, więc dokładność zależy też od sposobu próbkowania i korekcji coning.

Jeżeli IMU raportuje delta angle już zintegrowane wewnątrz przedziału, nie należy ponownie mnożyć przez Δt.

Błąd orientacji#

Dla zadanej q_d i aktualnej q kwaternion błędu może być:

q_e = q_d ⊗ q⁻¹

albo q⁻¹⊗q_d, zależnie od ramki, w której ma być wyrażony błąd. To nie są zamienne formuły. Regulator momentu zwykle potrzebuje błędu w body.

Po wyborze znaku q_e.w ≥ 0 mały błąd można przybliżyć:

e_R ≈ 2 q_e.vector

Dla dużych kątów dokładniejsze jest log map SO(3). Przy obrocie 180° oś ma niejednoznaczność znaku; regulator musi zachować stabilne zachowanie numeryczne.

Odejmowanie składowych kwaternionu nie daje fizycznego błędu orientacji. Cztery składowe podlegają więzowi i podwójnemu pokryciu.

Sterowanie attitude#

Regulator attitude wyznacza żądaną prędkość kątową z błędu orientacji, a wewnętrzny regulator rate steruje momentem. Schemat:

q_d, q → q_error → desired body rate → rate PID → moments

W multirotorze yaw może mieć mniejszy priorytet niż kierunek wektora ciągu. Sterowanie „reduced attitude” najpierw wyrównuje oś ciągu, a pozostały obrót yaw realizuje w dostępnym zakresie. Kwaterniony ułatwiają rozdzielenie tych obrotów.

Limity prędkości i przyspieszenia kątowego powinny działać na trajektorię orientacji, nie przez niezależne clampowanie roll/pitch/yaw, które może zmienić geometrię ruchu.

Błąd znaku w q_error tworzy dodatnie sprzężenie. Test bez śmigieł powinien potwierdzić zwrot zadanej korekcji dla każdej osi.

Interpolacja SLERP#

Spherical linear interpolation pomiędzy jednostkowymi q0 i q1:

slerp(q0,q1,t)

porusza się ze stałą prędkością kątową po wielkim kole S³. Najpierw oblicza się dot. Jeśli jest ujemny, zmienia znak q1, aby wybrać krótszą drogę. Dla bardzo bliskich orientacji używa się znormalizowanej interpolacji liniowej, unikając dzielenia przez mały sinus.

SLERP jest przydatny do synchronizacji pozy kamery z orientacją IMU, generowania trajectory setpoint i wizualizacji. Nie zastępuje integracji dynamiki; interpoluje między znanymi końcami.

Przy extrapolacji poza [0,1] błąd rośnie i potrzebny jest model prędkości kątowej.

Średnia orientacji#

Średnia arytmetyczna czterech składowych zależy od znaków i po normalizacji nie zawsze jest właściwą średnią na SO(3). Dla bliskich próbek można najpierw wyrównać znaki względem referencji, zsumować i znormalizować. Dla szerszych rozkładów używa się metod opartych na własnym wektorze lub iteracyjnej średniej w przestrzeni stycznej.

Filtracja komponentów kwaternionu osobnymi filtrami dolnoprzepustowymi narusza geometrię i zmienia fazę w sposób zależny od orientacji. Lepiej filtrować prędkość kątową albo błąd na SO(3).

Kowariancja orientacji w error-state jest 3×3 dla małego wektora kąta, nie 4×4 dla dowolnych składowych kwaternionu. Ułatwia to spójny EKF.

Stabilność numeryczna#

Zasady praktyczne:

normalizuj po operacjach akumulujących błąd;
sprawdzaj isfinite wszystkich składowych;
clampuj argumenty funkcji odwrotnych;
stosuj serie dla małych kątów;
wyrównuj znak przed interpolacją i porównaniem;
nie wyciągaj osi z zerowego obrotu;
monitoruj normę przed korekcją;
nie ukrywaj resetu.

W float32 orientacja ma zwykle wystarczającą precyzję dla kontrolera, jeśli liczby pozostają blisko jedności. Akumulacja przez miliony kroków nadal wymaga normalizacji. Dla obliczeń geodezyjnych problem precyzji jest inny i nie powinien być mieszany z kwaternionem.

Macierz utworzona z niejednostkowego kwaternionu może nie być ortogonalna. Funkcja konwersji może normalizować wejście, ale koszt i zachowanie błędu muszą być jawne.

Interfejs programistyczny#

Dobry typ rozróżnia kwaternion ogólny i jednostkową orientację. Przykładowe API:

struct AttitudeBodyToNed {
    float w, x, y, z;
};

Vec3Ned rotateBodyToNed(AttitudeBodyToNed q, Vec3Body v);
AttitudeNedToBody inverse(AttitudeBodyToNed q);
BodyRotationError errorInBody(AttitudeBodyToNed current,
                              AttitudeBodyToNed desired);

Silne typy osi i ramek ograniczają przypadkowe mieszanie. Jeśli C nie daje typów jednostek, nazwy funkcji i komentarze równań są obowiązkowe.

Serializacja powinna podać kolejność, ramki, timestamp oraz normę/valid flag. MAVLink i ROS mają własne konwencje; adapter wykonuje kopiowanie pól, nie memcpy między niezgodnymi strukturami.

Testy jednostkowe#

Minimalny zestaw:

identity nie zmienia wektora;
obrót 90° wokół X/Y/Z daje znane osie;
inverse cofa obrót;
q⊗q⁻¹ daje identity;
q i -q obracają identycznie;
mnożenie odpowiada składaniu macierzy;
norma wektora jest zachowana;
map exp i log są wzajemne dla małych i dużych kątów;
integracja stałego rate daje oczekiwany obrót;
SLERP ma właściwe końce i najkrótszą drogę;
konwersja Euler→q→Euler działa poza osobliwością;
NaN i norma zero są odrzucane.

Testy property-based losują osie i kąty, sprawdzając inwarianty. Tolerancja zależy od float/double i liczby operacji; nie powinna być tak szeroka, by ukryć błąd znaku.

Najczęstsze pułapki#

Zamiana wxyz z xyzw#

Identity [1,0,0,0] zostaje zinterpretowane jako obrót 180° wokół osi. Typowe przy integracji bibliotek.

Stopnie w funkcji oczekującej radianów#

Półkąt jest wielokrotnie za duży. Nazwy parametrów powinny mieć _rad.

Odwrotna transformacja#

Wynik bywa poprawny dla identity i niektórych osi, lecz błędny w ruchu złożonym. Potrzebne są testy dwóch obrotów.

Odejmowanie kwaternionów#

Nie daje błędu rotacji i nie respektuje q≡-q.

Brak korekty znaku w SLERP#

Interpolacja wybiera długą drogę albo przechodzi przez zero.

Niezmieniona kolejność integracji#

Rate w body wymaga innej strony przyrostu niż rate w world. Wynik rozbiega się podczas ruchu wieloosiowego.

Normalizacja maskująca awarię#

Jeśli norma przed korekcją nagle wynosi 100 albo 0, samo sprowadzenie do 1 ukrywa błąd. Trzeba ustawić flagę.

Kwaternion jako cztery niezależne stany EKF#

Ignoruje więz normy. Error-state z trójwektorem małego kąta jest zwykle spójniejszy.

Powiązane tematy#

Przypisy#

NASA, Euler angles, quaternions, and transformation matrices — working relationships — relacje reprezentacji orientacji.
R. Mahony, T. Hamel, J.-M. Pflimlin, Nonlinear Complementary Filters on the Special Orthogonal Group — obserwacja orientacji bezpośrednio na SO(3).
S. O. H. Madgwick, An efficient orientation filter — równania kwaternionowe dla IMU i MARG.
PX4, Reference Frames and ROS — praktyczne konwencje NED/FRD i transformacje wejść.

Utworzono: 15 sierpnia 2026. Ostatnia aktualizacja: 15 sierpnia 2026. Źródła zweryfikowano: 15 sierpnia 2026.

Źródła z centralnego rejestru

NASA: Euler angles, quaternions, and transformation matrices — working relationships [memorandum techniczne]
Mahony, Hamel, Pflimlin: Nonlinear Complementary Filters on the Special Orthogonal Group [publikacja naukowa]
Sebastian Madgwick: An efficient orientation filter for inertial and inertial/magnetic sensor arrays [raport badawczy]
PX4: Reference Frames and ROS [dokumentacja projektu]