3 Fuzja czujników w 3D: estymacja orientacji na kwaternionach

0 Prolog: lot bez widoczności

We wrześniu 1929 roku Jimmy Doolittle wystartował, okrążył lotnisko i wylądował z kabiną zakrytą płótnem, patrząc tylko na przyrządy. Kluczowym z nich był sztuczny horyzont firmy Sperry: żyroskop na przegubach, którego wirnik trzymał kierunek pionu, a mechanizm wahadłowy powoli korygował jego dryf. Bez takiego przyrządu pilot w chmurach traci orientację w ciągu minut, bo zmysł równowagi w zakręcie mówi to samo co akcelerometr: że samolot leci prosto.

Dzisiejsze samoloty lekkie mają w miejscu kilku wirujących przyrządów jeden moduł AHRS (ang. attitude and heading reference system) z czujnikami MEMS, a obliczenia, które w 1929 roku robił mechanizm, robi kilkanaście linijek kodu. Ten kod musi rozwiązać dokładnie ten sam problem co mechanizm Sperry’ego: żyroskop dryfuje, a jedyny czujnik pionu na pokładzie mierzy nie grawitację, tylko siłę.

Pytanie na start. Samolot leci w ustalonym zakręcie skoordynowanym z przechyleniem $30^\circ$ . Jakie przechylenie pokazuje akcelerometr, jeśli liczymy je tak jak na ziemi? Odpowiedź w części 5.

1 Trzy czujniki w trzech osiach

Wszystkie trzy czujniki są przykręcone do kadłuba, więc mierzą w osiach ciała. Orientację opisuje kwaternion $q$ albo macierz $R$ (ciało → NED) z lekcji 1 Orientacja w 3D: macierz obrotu, kąty Eulera, oś i kąt, kwaterniony.

Modele czujników AHRS

Żyroskopy: $\;\boldsymbol\omega_m = \boldsymbol\omega + \mathbf b + \mathbf w_g$ (prędkość kątowa plus bias plus szum).

Akcelerometry mierzą siłę właściwą, czyli przyspieszenie minus grawitację: $\;\mathbf f_m = R^{\mathsf T}(\mathbf a^n - \mathbf g^n) + \mathbf w_a$ , $\;\mathbf g^n = [0;\ 0;\ g]$ .

Magnetometr: $\;\mathbf m_m = R^{\mathsf T}\mathbf B^n + \mathbf w_m$ , gdzie $\mathbf B^n$ to pole Ziemi w osiach NED.

W locie niezakłóconym ( $\mathbf a^n = 0$ ) akcelerometr pokazuje $\mathbf f = -R^{\mathsf T}\mathbf g^n$ : wektor „do góry” zapisany w osiach ciała. Czyli $-\mathbf f$ wskazuje pion w dół, a jego składowe niosą przechylenie i pochylenie (zadanie 1.3 z lekcji 1 Orientacja w 3D: macierz obrotu, kąty Eulera, oś i kąt, kwaterniony). Obrót wokół pionu nie zmienia go wcale, więc kursu akcelerometr nie widzi. Do tego potrzebny jest drugi kierunek odniesienia, a jedyny łatwo dostępny to pole magnetyczne Ziemi. W Polsce jest nachylone pod kątem ok. $66$ – $70^\circ$ w dół (więcej na północy), więc jego składowa pozioma to tylko 35–40% całości, a każdy kawałek żelaza i każdy przewód z prądem w pobliżu czujnika je zniekształca.

Zadania · część 1

3 zadania · ok. 15 min

Samolot stoi poziomo na płycie lotniska. Co pokazują trzy akcelerometry (osie: $x$ do przodu, $y$ w prawo, $z$ w dół)?

Odpowiedź
$\mathbf f = -R^{\mathsf T}\mathbf g^n = [0;\ 0;\ -9{,}81]$ m/s². Akcelerometr osi $z$ pokazuje $-g$ : siła reakcji podłoża działa w górę, czyli w stronę $-z$ .
Co pokazują akcelerometry w swobodnym spadku (np. w locie po paraboli w samolocie do symulacji nieważkości)?

Odpowiedź
Zero: $\mathbf a^n = \mathbf g^n$ , więc $\mathbf f = 0$ . Z akcelerometru nie da się wtedy wyznaczyć pionu.
Samolot stoi z nosem uniesionym o $10^\circ$ (bez przechylenia). Policz $\mathbf f$ .

Odpowiedź
$\mathbf f = -g\,[-\sin\theta;\ \sin\varphi\cos\theta;\ \cos\varphi\cos\theta] = [g\sin 10^\circ;\ 0;\ -g\cos 10^\circ] = [1{,}70;\ 0;\ -9{,}66]$ m/s². Akcelerometr osi $x$ „czuje” składową grawitacji wzdłuż kadłuba: nie odróżni jej od przyspieszania samolotu.

2 Orientacja z dwóch wektorów

Gdy samolot nie przyspiesza, orientację można wyznaczyć bez żadnego filtru, z dwóch pomiarów naraz.

Przechylenie, pochylenie i kurs z czujników (postój lub lot niezakłócony)

\varphi = \operatorname{atan2}(-f_y,\ -f_z),\qquad \theta = \operatorname{atan2}\!\left(f_x,\ \sqrt{f_y^2 + f_z^2}\right).

Kompensacja przechylenia magnetometru: $\;\mathbf m_h = R_y(\theta)R_x(\varphi)\,\mathbf m_m$ (wektor $\mathbf m$ sprowadzony do poziomu), $\;\psi = \operatorname{atan2}(-m_{h,y},\ m_{h,x})$ plus deklinacja magnetyczna.

Skąd te wzory? Z trzeciego wiersza macierzy $R$ : $-\mathbf f/g = [-\sin\theta;\ \sin\varphi\cos\theta;\ \cos\varphi\cos\theta]$ . Kurs: $R_y(\theta)R_x(\varphi)\,R^{\mathsf T} = R_z(\psi)^{\mathsf T}$ , więc po „cofnięciu” przechylenia i pochylenia zostaje tylko obrót o kurs. Ten sam pomysł (dwa wektory odniesienia znane w obu układach wyznaczają orientację) w ogólnej postaci to algorytm TRIAD, używany w satelitach (kurs astronautyki, lekcja 12 Wyznaczanie orientacji: czujniki, TRIAD i filtr).

Bez kompensacji przechylenia kompas magnetyczny jest bardzo czuły na przechylenie, bo pole jest prawie pionowe: przy przechyleniu $20^\circ$ i pochyleniu $10^\circ$ kurs $60^\circ$ może wyjść jako $-6^\circ$ (przykład 6.2). To jest źródło znanych pilotom błędów kompasu w zakrętach.

Zadania · część 2

3 zadania · ok. 25 min

Akcelerometry pokazują $\mathbf f = [0;\ 4{,}905;\ -8{,}496]$ m/s². Jakie przechylenie i pochylenie wynika z tego, jeśli samolot nie przyspiesza?

Odpowiedź
$\varphi = \operatorname{atan2}(-4{,}905;\ 8{,}496) = -30^\circ$ (lewe skrzydło w dół), $\theta = \operatorname{atan2}(0;\ 9{,}81) = 0$ .
Szum akcelerometru $0{,}3$ m/s², szum magnetometru $0{,}02$ (w jednostkach $|\mathbf B|$ ), nachylenie pola $67^\circ$ . Oszacuj błędy przechylenia i kursu z pojedynczego pomiaru.

Odpowiedź
Przechylenie: $0{,}3/9{,}81 = 0{,}031$ rad $\approx 1{,}8^\circ$ . Kurs: liczy się tylko składowa pozioma pola, $\cos 67^\circ = 0{,}39$ , więc $0{,}02/0{,}39 = 0{,}051$ rad $\approx 2{,}9^\circ$ . Kurs z magnetometru jest mniej dokładny niż pion z akcelerometru, choć czujnik jest „lepszy” (2% wobec 3%).
Dlaczego z samego akcelerometru nie da się wyznaczyć kursu, a z samego magnetometru da się wyznaczyć tylko dwa kąty? Który obrót jest „niewidoczny” dla każdego z nich?

Odpowiedź
Pomiar jednego kierunku nie zmienia się przy obrocie wokół tego kierunku. Akcelerometr nie widzi obrotu wokół pionu (kursu), magnetometr nie widzi obrotu wokół linii pola. Dwa nierównoległe kierunki wyznaczają orientację jednoznacznie, im bliżej prostopadłości, tym dokładniej.

3 Filtr komplementarny na kwaternionie

Pomysł z jednej osi przenosi się wprost: całkujemy żyroskop (dokładny w krótkim okresie), a powolny dryf korygujemy pomiarami kierunków (dokładnymi w długim okresie). Różnica: w 3D błąd orientacji to obrót, a nie liczba. Jak zmierzyć, o ile i wokół jakiej osi estymata odbiega od prawdy? Porównujemy kierunek zmierzony $\mathbf v_m$ z kierunkiem przewidywanym przez estymatę $\hat{\mathbf v} = \hat R^{\mathsf T}\mathbf v^n$ . Iloczyn wektorowy $\mathbf v_m\times\hat{\mathbf v}$ ma długość $\sin$ kąta między nimi i wskazuje oś, wokół której trzeba obrócić estymatę.

Filtr komplementarny Mahony’ego

\mathbf e = \mathbf d_m\times\hat{\mathbf d} + \big((\mathbf m_m\times\hat{\mathbf m})\cdot\hat{\mathbf d}\big)\,\hat{\mathbf d},\qquad \dot{\hat{\mathbf b}} = -k_I\,\mathbf e,

\dot{\hat q} = \tfrac12\,\hat q\otimes\big(0,\ \boldsymbol\omega_m - \hat{\mathbf b} + k_P\,\mathbf e\big) .

$\mathbf d_m = -\mathbf f_m/|\mathbf f_m|$ (pion zmierzony), $\hat{\mathbf d} = \hat R^{\mathsf T}[0;0;1]$ (pion przewidywany), $\hat{\mathbf m} = \hat R^{\mathsf T}\mathbf B^n$ . Wektory magnetyczne znormalizowane. Wzmocnienia: $k_P$ (proporcjonalne) i $k_I$ (całkujące); w symulacji 2 $k_I = 0{,}05\,k_P$ .

To jest regulator PI w pętli korekty, dokładnie jak w filtrze z członem całkującym z lekcji 4.5 Rozszerzony filtr Kalmana i fuzja czujników w jednej osi: człon $k_P\mathbf e$ ciągnie estymatę do pomiaru z czasem odpowiedzi ok. $1/k_P$ , a człon całkujący uczy się biasu żyroskopów. Dwa szczegóły odróżniają porządną implementację:

błąd z magnetometru rzutujemy na kierunek pionu: magnetometr ma poprawiać tylko kurs, żeby zakłócenia magnetyczne nie psuły przechylenia i pochylenia;
kwaternion po każdym kroku normalizujemy, a krok całkowania robimy dokładnie (obrót o $|\boldsymbol\omega|T$ wokół $\boldsymbol\omega$ ), nie metodą Eulera.

Zadania · część 3

3 zadania · ok. 25 min

Pion zmierzony to $\mathbf d_m = [0;\ 0;\ 1]$ , a estymata przewiduje $\hat{\mathbf d} = [0;\ 0{,}1;\ 0{,}995]$ . Policz $\mathbf d_m\times\hat{\mathbf d}$ . Wokół której osi i o jaki kąt filtr obróci estymatę?

Odpowiedź
$[0\cdot 0{,}995 - 1\cdot 0{,}1;\ 1\cdot 0 - 0;\ 0] = [-0{,}1;\ 0;\ 0]$ : wokół osi $x$ (przechylenie), długość $0{,}1 = \sin 5{,}7^\circ$ . Estymata ma przechylenie błędne o ok. $5{,}7^\circ$ i filtr koryguje je z prędkością $k_P\cdot 0{,}1$ rad/s.
Filtr bez członu całkującego ( $k_I = 0$ ), bias żyroskopu $b = 0{,}5^\circ$ /s, $k_P = 1$ 1/s. Jaki jest ustalony błąd orientacji? Porównaj z błędem $b\tau$ z lekcji 4.5 automatyki.

Odpowiedź
W stanie ustalonym korekta znosi bias: $k_P\,\delta = b$ , więc $\delta = b/k_P = 0{,}5^\circ$ . To ten sam wynik co $b\tau$ z $\tau = 1/k_P$ . Człon całkujący usuwa ten błąd.
Dlaczego błąd z magnetometru rzutujemy na pion? Co by się stało bez rzutowania, gdy samolot przelatuje nad linią wysokiego napięcia?

Odpowiedź
Zakłócone pole odchyla zmierzony wektor $\mathbf m$ w dowolnym kierunku, także w pionie. Bez rzutowania filtr próbowałby wyjaśnić to obrotem wokół osi poziomej, czyli zmianą przechylenia lub pochylenia. Po rzutowaniu zakłócenie może zepsuć tylko kurs, a ten jest mniej krytyczny dla utrzymania samolotu w powietrzu.

4 Filtr Kalmana dla orientacji (MEKF)

Rozszerzony filtr Kalmana z lekcji 4.5 Rozszerzony filtr Kalmana i fuzja czujników w jednej osi szacuje stan w przestrzeni wektorowej: dodaje poprawki i liczy kowariancję. Kwaternion do tego nie pasuje. Ma cztery składowe i jeden więz $|q| = 1$ , więc jego macierz kowariancji $4\times 4$ jest osobliwa, a dodanie poprawki $\hat q + \delta q$ wyprowadza poza sferę jednostkową. Rozwiązanie, używane w satelitach od lat 70. (Lefferts, Markley i Shuster, 1982), nazywa się multiplikatywnym EKF (MEKF): filtr szacuje nie orientację, tylko mały błąd orientacji wokół bieżącej estymaty.

Multiplikatywny filtr Kalmana (MEKF)

Stan błędu: $\delta\mathbf x = [\delta\boldsymbol\theta;\ \delta\mathbf b]$ (6 liczb), gdzie $q = \hat q\otimes(1, \tfrac12\delta\boldsymbol\theta)$ , $\;\delta\mathbf b = \mathbf b - \hat{\mathbf b}$ .

Predykcja: $\hat q \leftarrow \hat q\otimes\exp\!\big(\tfrac12(\boldsymbol\omega_m - \hat{\mathbf b})T\big)$ , $\;P \leftarrow \Phi P\Phi^{\mathsf T} + Q$ , $\;\Phi = \begin{bmatrix} I - [\hat{\boldsymbol\omega}]_\times T & -IT\\ 0 & I\end{bmatrix}$ .

Korekta pomiarem kierunku $\mathbf y$ o wartości odniesienia $\mathbf r^n$ : $\;\hat{\mathbf y} = \hat R^{\mathsf T}\mathbf r^n$ , $\;H = \begin{bmatrix}[\hat{\mathbf y}]_\times & 0\end{bmatrix}$ , $\;K = PH^{\mathsf T}(HPH^{\mathsf T} + R_v)^{-1}$ , $\;\delta\hat{\mathbf x} = K(\mathbf y - \hat{\mathbf y})$ .

Wcielenie poprawki: $\hat q \leftarrow \hat q\otimes(1, \tfrac12\delta\hat{\boldsymbol\theta})$ , normalizacja, $\;\hat{\mathbf b}\leftarrow\hat{\mathbf b} + \delta\hat{\mathbf b}$ , $\;P \leftarrow (I - KH)P$ . Błąd znowu wynosi zero.

Skąd $H = [\hat{\mathbf y}]_\times$ ? Dla małego błędu $R = \hat R(I + [\delta\boldsymbol\theta]_\times)$ (zadanie 8.2 z lekcji 1 Orientacja w 3D: macierz obrotu, kąty Eulera, oś i kąt, kwaterniony), więc $\mathbf y = R^{\mathsf T}\mathbf r^n \approx (I - [\delta\boldsymbol\theta]_\times)\hat{\mathbf y} = \hat{\mathbf y} + \hat{\mathbf y}\times\delta\boldsymbol\theta$ . Pomiar jest liniowy w błędzie, czego potrzebuje filtr Kalmana. Macierz $\Phi$ wynika z kinematyki błędu: $\delta\dot{\boldsymbol\theta} = -\hat{\boldsymbol\omega}\times\delta\boldsymbol\theta - \delta\mathbf b$ .

Co MEKF daje w porównaniu z filtrem komplementarnym? Wzmocnienia dobiera sam z kowariancji i wie, które składowe błędu są aktualnie obserwowalne. Bias żyroskopu osi pionowej jest np. obserwowalny tylko dzięki magnetometrowi, a w zakrętach także dzięki akcelerometrowi. Cena: macierze $6\times 6$ i odwracanie $3\times 3$ w każdym kroku, co dla współczesnego mikrokontrolera nie jest problemem.

Zadania · część 4

3 zadania · ok. 30 min

Dlaczego kowariancja kwaternionu $4\times 4$ jest osobliwa?

Odpowiedź
Kwaternion jednostkowy leży na sferze $|q| = 1$ ; błędy wzdłuż $q$ (zmieniające normę) są zabronione, więc wariancja w tym kierunku jest zerowa. Macierz ma wartość własną 0. Orientacja ma 3 stopnie swobody i stan błędu powinien mieć 3 składowe.
Samolot leci poziomo, prosto, z jednym pomiarem kierunku: pionem $\hat{\mathbf y} = [0;0;1]$ . Policz $H$ dla części $\delta\boldsymbol\theta$ . Która składowa błędu orientacji jest nieobserwowalna?

Odpowiedź
$[\hat{\mathbf y}]_\times = \begin{bmatrix} 0 & -1 & 0\\ 1 & 0 & 0\\ 0 & 0 & 0\end{bmatrix}$ . Kolumna dla $\delta\theta_z$ jest zerowa: błąd kursu nie wpływa na pomiar pionu. Wiersz 3 też zerowy: składowa $z$ pomiaru nie niesie informacji w pierwszym rzędzie (długość wektora jest stała).
Dlaczego po każdej korekcie stan błędu „wraca do zera” i co by się stało, gdybyśmy zapomnieli wcielić poprawkę do $\hat q$ ?

Odpowiedź
Poprawka zostaje przeniesiona do $\hat q$ i $\hat{\mathbf b}$ , więc nowa estymata błędu to 0, a linearyzacja zostaje dokładna (błąd zawsze mały). Bez wcielenia filtr akumulowałby duży $\delta\boldsymbol\theta$ , model $R \approx \hat R(I + [\delta\boldsymbol\theta]_\times)$ przestałby obowiązywać i filtr by się rozjechał.

5 Zakręt, czyli akcelerometr kłamie

W ustalonym zakręcie skoordynowanym samolot nie ślizga się na boki, więc wypadkowa sił aerodynamicznych leży w płaszczyźnie symetrii. Akcelerometr mierzy właśnie siłę (bez grawitacji) podzieloną przez masę, więc pokazuje $\mathbf f = [0;\ 0;\ -ng]$ , gdzie $n = 1/\cos\varphi$ to przeciążenie. Wzór z części 2 daje przechylenie $0^\circ$ . Pilot czuje to samo: siedzi wciśnięty w fotel, ale „w pionie”. Odpowiedź na pytanie z prologu: zero.

Filtr, który ufa akcelerometrowi, w długim zakręcie powoli „prostuje” swoją estymatę do $0^\circ$ , i to tym szybciej, im większe ma wzmocnienie korekty. Zły pion psuje następnie kompensację magnetometru i kurs. Rozwiązania są trzy, wszystkie stosowane w praktyce:

Kompensacja przyspieszenia. Jeśli prędkość lotu $V$ jest znana (rurka Pitota, GPS) i samolot leci nosem w kierunku ruchu, przyspieszenie dośrodkowe w osiach ciała wynosi $\mathbf a^b = V\,\boldsymbol\omega\times\mathbf e_x = V\,[0;\ r;\ -q]$ . Odejmujemy je: $\mathbf f_c = \mathbf f_m - V[0;\ r;\ -q]$ i znowu mamy pion.
Bramkowanie. Gdy $|\mathbf f|$ wyraźnie różni się od $g$ albo prędkość kątowa jest duża, korektę akcelerometrem wyłączamy lub mocno osłabiamy.
Małe wzmocnienie. Korekta o stałej czasowej kilkudziesięciu sekund zdąży tylko trochę „wyprostować” estymatę w typowym zakręcie. Ceną jest wolniejsze usuwanie dryfu.

Wyniki symulacji (RMS błędów od 20. sekundy, te same szumy dla wszystkich filtrów):

filtr	kompensacja	przechylenie	pochylenie	kurs
tylko żyroskop	nie dotyczy	6,8°	6,5°	26°
Mahony, $k_P = 1$ 1/s	nie	28°	2,7°	64°
Mahony, $k_P = 0{,}1$ 1/s	nie	23°	14°	18°
Mahony, $k_P = 1$ 1/s	tak	0,36°	0,13°	1,6°
MEKF	tak	0,12°	0,13°	0,19°

Bez kompensacji żadne wzmocnienie nie jest dobre: duże ufa kłamiącemu akcelerometrowi, małe nie zdąży usunąć dryfu. Z kompensacją filtr komplementarny i MEKF dają przechylenie z dokładnością ułamka stopnia. MEKF jest jeszcze trzy razy dokładniejszy w przechyleniu i osiem razy w kursie, bo wzmocnienia dobiera z kowariancji, a bias osi pionowej szacuje i z magnetometru, i z akcelerometru w zakrętach. W MEKF trzeba przy tym pamiętać, że kompensacja używa żyroskopów: błąd biasu $\delta b_r$ przesuwa zmierzony pion o $V\delta b_r/g$ , co trzeba wpisać do macierzy $H$ (zadanie 5.3). Bez tego członu filtr w tej symulacji przez pierwsze kilkanaście sekund gubi orientację (błąd kursu do ok. $75^\circ$ ), zanim ustali się estymata biasu, a potem jest wyraźnie gorszy (RMS kursu $2{,}6^\circ$ zamiast $0{,}19^\circ$ ).

Zadania · część 5

3 zadania · ok. 30 min

Zakręt skoordynowany z przechyleniem $30^\circ$ przy 55 m/s. Policz prędkość zmiany kursu $\dot\psi = g\tan\varphi/V$ , promień zakrętu $V^2/(g\tan\varphi)$ i to, co pokazują akcelerometry.

Odpowiedź
$\dot\psi = 9{,}81\cdot 0{,}577/55 = 0{,}103$ rad/s ( $5{,}9^\circ$ /s, pełny okrąg w ok. 61 s); promień 534 m; $\mathbf f = [0;\ 0;\ -9{,}81/\cos 30^\circ] = [0;\ 0;\ -11{,}33]$ m/s², przeciążenie 1,155.
Wyprowadź kompensację: samolot leci z prędkością $V$ wzdłuż osi $x$ ciała, $V$ stałe. Pokaż, że przyspieszenie w osiach ciała to $V[0;\ r;\ -q]$ .

Odpowiedź
$\mathbf v^n = R\,[V;0;0]$ , więc $\mathbf a^n = \dot R\,[V;0;0] = R[\boldsymbol\omega]_\times[V;0;0] = R\,(\boldsymbol\omega\times V\mathbf e_x)$ . W osiach ciała: $V\,(p, q, r)\times(1, 0, 0) = V\,[0;\ r;\ -q]$ . Przy niezerowym kącie natarcia i ślizgu trzeba użyć pełnego wektora prędkości w osiach ciała.
Żyroskop odchylania ma bias $0{,}4^\circ$ /s. Jaki błąd przechylenia wprowadza kompensacja przy 55 m/s? Jak to wpisać do macierzy $H$ w MEKF?

Odpowiedź
Błąd kompensacji $V\delta b_r = 55\cdot 0{,}007 = 0{,}38$ m/s² w osi $y$ , czyli przechylenie błędne o $\arctan(0{,}38/9{,}81) = 2{,}2^\circ$ . Pomiar pionu $\mathbf y = -\mathbf f_c/g$ (bez normalizacji; po normalizacji trzeba by jeszcze odrzucić składową wzdłuż $\hat{\mathbf y}$ ) zależy więc od błędu biasu: $\partial\mathbf y/\partial\delta\mathbf b = \frac{V}{g}\begin{bmatrix}0 & 0 & 0\\ 0 & 0 & 1\\ 0 & -1 & 0\end{bmatrix}$ , i to jest prawy blok $H$ dla akcelerometru zamiast zer.

6 Metoda i przykłady rozwiązane

Metoda: projekt AHRS

Modele czujników w osiach ciała; skalibruj magnetometr (przesunięcie od żelaza twardego, macierz od miękkiego) i osie akcelerometrów.
Inicjalizacja na postoju: przechylenie i pochylenie z akcelerometrów, kurs z magnetometru z kompensacją.
Propagacja kwaternionu żyroskopami z odjętym biasem, dokładny krok obrotu, normalizacja.
Korekta: pion z akcelerometrów (z kompensacją przyspieszenia lub bramkowaniem), kurs z magnetometru (tylko obrót wokół pionu); Mahony albo MEKF.
Sprawdzenie: długi zakręt, pętla, przejście przez pion, zakłócenie magnetyczne; błędy porównaj z kowariancją.

Przykład 6.1 Przechylenie i pochylenie z akcelerometrów

Samolot stoi na płycie. Akcelerometry: $\mathbf f = [1{,}70;\ -0{,}85;\ -9{,}63]$ m/s². Wyznacz przechylenie i pochylenie.

$\theta = \operatorname{atan2}(1{,}70;\ \sqrt{0{,}85^2 + 9{,}63^2}) = \operatorname{atan2}(1{,}70;\ 9{,}667) = 9{,}97^\circ$ .
$\varphi = \operatorname{atan2}(0{,}85;\ 9{,}63) = 5{,}04^\circ$ .

Wynik: nos w górę o $10^\circ$ , prawe skrzydło niżej o $5^\circ$ .

Sprawdzenie: $|\mathbf f| = 9{,}816 \approx g$ : samolot rzeczywiście nie przyspiesza, więc wzory obowiązują ✓.

Przykład 6.2 Kurs z magnetometru przy przechyleniu

Samolot: kurs $60^\circ$ , pochylenie $10^\circ$ , przechylenie $20^\circ$ . Pole magnetyczne o nachyleniu $67^\circ$ (bez deklinacji), znormalizowane. Policz kurs z magnetometru bez kompensacji i z kompensacją.

$\mathbf B^n = [\cos 67^\circ;\ 0;\ \sin 67^\circ] = [0{,}391;\ 0;\ 0{,}921]$ , $\;\mathbf m = R^{\mathsf T}\mathbf B^n = [0{,}033;\ 0{,}004;\ 0{,}9995]$ . Pole w osiach ciała jest prawie dokładnie wzdłuż osi $z$ .
Bez kompensacji: $\psi = \operatorname{atan2}(-0{,}004;\ 0{,}033) = -6{,}4^\circ$ .
Z kompensacją: $\mathbf m_h = R_y(10^\circ)R_x(20^\circ)\mathbf m = [0{,}195;\ -0{,}338;\ 0{,}921]$ , $\;\psi = \operatorname{atan2}(0{,}338;\ 0{,}195) = 60{,}0^\circ$ .

Wynik: bez kompensacji błąd $66^\circ$ , z kompensacją zero.

Sprawdzenie: składowa pionowa $\mathbf m_h$ to $0{,}921 = \sin 67^\circ$ ✓, pozioma $\sqrt{0{,}195^2 + 0{,}338^2} = 0{,}391 = \cos 67^\circ$ ✓.

Przykład 6.3 Kompensacja w zakręcie

Zakręt skoordynowany z przechyleniem $30^\circ$ przy 55 m/s, lot poziomy. Pokaż, że po kompensacji akcelerometr daje poprawne przechylenie.

$\dot\psi = 0{,}103$ rad/s; prędkości kątowe w osiach ciała: $q = \dot\psi\sin\varphi = 0{,}0515$ , $r = \dot\psi\cos\varphi = 0{,}0892$ rad/s.
Akcelerometry: $\mathbf f = [0;\ 0;\ -11{,}33]$ ; wzór z części 2 daje $\varphi = 0$ .
Kompensacja: $V[0;\ r;\ -q] = [0;\ 4{,}905;\ -2{,}832]$ ; $\;\mathbf f_c = [0;\ -4{,}905;\ -8{,}496]$ .
$\varphi = \operatorname{atan2}(4{,}905;\ 8{,}496) = 30{,}0^\circ$ .

Wynik: po odjęciu przyspieszenia dośrodkowego zostaje sama grawitacja, $|\mathbf f_c| = 9{,}81$ , i poprawne przechylenie.

Sprawdzenie: to dokładnie składowe $-\mathbf g$ w osiach ciała przy $\varphi = 30^\circ$ : $-g\,[0;\ \sin 30^\circ;\ \cos 30^\circ]$ ✓.

7 Błędne intuicje, których trzeba się oduczyć

„Akcelerometr wskazuje pion.”: Wskazuje kierunek siły właściwej. Pion tylko wtedy, gdy samolot nie przyspiesza. W zakręcie skoordynowanym wskazuje „pion samolotu”, a nie pion Ziemi.
„Magnetometr to kompas, więc daje kurs.”: Daje wektor pola w osiach ciała, prawie pionowy na naszych szerokościach. Kurs wychodzi dopiero po sprowadzeniu wektora do poziomu, czyli po znajomości przechylenia i pochylenia.
„Filtr Kalmana dla kwaternionu to zwykły EKF z czterema stanami.”: Taki filtr ma osobliwą kowariancję i łamie więz $|q| = 1$ . Poprawny jest filtr błędu (MEKF) z trzema składowymi błędu orientacji.
„Wystarczy zmniejszyć zaufanie do akcelerometru i zakręty przestaną szkodzić.”: Mniejsze wzmocnienie zmniejsza błąd w zakręcie, ale wydłuża usuwanie dryfu i uczenie się biasu. Bez kompensacji przyspieszenia albo bramkowania nie ma dobrego kompromisu.
„MEKF jest zawsze lepszy od filtru komplementarnego.”: Przy dobrze dobranych wzmocnieniach różnica w przechyleniu i pochyleniu jest mała. MEKF wygrywa tam, gdzie obserwowalność zmienia się w czasie (bias osi pionowej), i daje kowariancję, czyli ocenę własnej wiarygodności.

8 Zadania do przemyślenia

Te zadania wymagają więcej myślenia niż liczenia.

Zadania do przemyślenia

5 zadań · ok. 1,5 godziny

Złudzenie w chmurach. Pilot w chmurach powoli wchodzi w zakręt z rosnącym przechyleniem. Błędnik działa jak akcelerometr z bardzo małym wzmocnieniem korekty plus „żyroskop” (kanały półkoliste), który nie czuje powolnych obrotów. Co pilot czuje i dlaczego to prowadzi do tzw. spirali śmierci?
Rozwiązanie
1. Powolne przechylanie jest poniżej progu kanałów półkolistych, a „akcelerometr” błędnika pokazuje pion samolotu. Pilot czuje lot prosto, mimo przechylenia.
2. Samolot w przechyleniu bez zwiększenia ciągu i kąta natarcia traci wysokość; pilot widzi spadek wysokości i ciągnie drążek, co przy przechyleniu zacieśnia zakręt. Spirala przyspiesza. Jedyną obroną jest zaufanie przyrządom, czyli sztucznemu horyzontowi z prawidłową fuzją.
Bramkowanie. Zaproponuj regułę: kiedy i jak bardzo osłabiać korektę akcelerometrem. Jakie dwie wielkości są łatwo dostępne i dlaczego żadna sama nie wystarczy?
Rozwiązanie
1. $\big||\mathbf f| - g\big|$ i $|\boldsymbol\omega|$ . Np. $k_a = k_0\exp(-(|\mathbf f| - g)^2/\sigma_f^2)$ , dodatkowo zero, gdy $|\boldsymbol\omega|$ przekracza próg.
2. Sam $|\mathbf f|$ zawodzi przy łagodnych zakrętach ( $n = 1{,}02$ przy $11^\circ$ ) i przy przyspieszaniu wzdłuż kadłuba, gdzie $|\mathbf f|$ prawie się nie zmienia, a kierunek tak. Sama $|\boldsymbol\omega|$ zawodzi w długim ustalonym zakręcie (mała prędkość kątowa, stały błąd). Dlatego najlepsza jest kompensacja z prędkością lotu.
Pętla. Czy MEKF albo filtr Mahony’ego ma kłopot z przejściem przez pion, jak kąty Eulera w lekcji 1 Orientacja w 3D: macierz obrotu, kąty Eulera, oś i kąt, kwaterniony? A kompensacja magnetometru z części 2?
Rozwiązanie
1. Filtry działają na kwaternionie i wektorach, nie na kątach, więc nie mają osobliwości. Kąty Eulera liczy się tylko do wyświetlenia.
2. Wzory z części 2 (atan2 i obroty $R_y(\theta)R_x(\varphi)$ ) są w kątach Eulera: przy $\theta = \pm 90^\circ$ przechylenie i kurs stają się nieokreślone. Inicjalizację na postoju robi się tymi wzorami, ale korekta w locie musi być wektorowa, jak w filtrze Mahony’ego.
Kalibracja magnetometru. Magnetometr w kokpicie pokazuje $\mathbf m_m = A\,R^{\mathsf T}\mathbf B^n + \mathbf c$ , gdzie $\mathbf c$ to stałe pole od namagnesowanych części (żelazo twarde), a $A$ to zniekształcenie od miękkich materiałów magnetycznych. Jak wyznaczyć $\mathbf c$ , obracając samolot (albo czujnik) we wszystkich kierunkach?
Rozwiązanie
1. Przy $A = I$ punkty $\mathbf m_m$ leżą na sferze o środku $\mathbf c$ i promieniu $|\mathbf B|$ . Środek sfery dopasowanej metodą najmniejszych kwadratów (lekcja o dopasowaniu w matematyce) to $\mathbf c$ .
2. Przy $A \ne I$ punkty leżą na elipsoidzie; dopasowanie elipsoidy daje i $\mathbf c$ , i $A$ (z dokładnością do obrotu). To robi procedura „kompensacji kompasu” w aplikacjach naziemnych dronów.
Inicjalizacja w locie. AHRS zrestartował się w zakręcie. Jakim kątem przechylenia się zainicjalizuje i jak szybko filtr Mahony’ego z kompensacją dojdzie do prawdy? A bez kompensacji?
Rozwiązanie
1. Wzory statyczne bez kompensacji dają przechylenie 0° (zakręt skoordynowany). Z kompensacją (prędkości kątowe są dostępne od razu z żyroskopów) da się zainicjalizować poprawnie.
2. Jeśli start był błędny, filtr z kompensacją zbiega w czasie kilku $1/k_P$ . Bez kompensacji pozostanie przy błędzie aż do końca zakrętu, a po wyprostowaniu skrzydeł wróci do prawdy z czasem $1/k_P$ . Po restarcie w chmurach to bardzo niebezpieczne kilkadziesiąt sekund.

9 Sprawdź się

Odpowiedz w myślach, zanim rozwiniesz odpowiedź.

1 Co mierzy akcelerometr?

Siłę właściwą $\mathbf f = R^{\mathsf T}(\mathbf a^n - \mathbf g^n)$ : przyspieszenie minus grawitacja, w osiach ciała.

2 Jak policzyć przechylenie i pochylenie z akcelerometru?

$\varphi = \operatorname{atan2}(-f_y, -f_z)$ , $\theta = \operatorname{atan2}(f_x, \sqrt{f_y^2 + f_z^2})$ , tylko gdy samolot nie przyspiesza.

3 Po co kompensacja przechylenia magnetometru?

Pole Ziemi jest prawie pionowe; kurs wynika z jego składowej poziomej, więc trzeba je najpierw sprowadzić do poziomu.

4 Jak filtr Mahony’ego mierzy błąd orientacji?

Iloczynem wektorowym kierunku zmierzonego i przewidywanego: oś i sinus kąta błędu. Korekta PI dodana do prędkości kątowej.

5 Czym MEKF różni się od zwykłego EKF?

Szacuje mały błąd orientacji (3 składowe) wokół estymaty, wciela go mnożeniem kwaternionów i zeruje; macierz pomiaru $H = [\hat{\mathbf y}]_\times$ .

6 Co pokazuje akcelerometr w zakręcie skoordynowanym i jak to naprawić?

Przechylenie 0° i przeciążenie $1/\cos\varphi$ . Naprawa: odjąć $V[0;\ r;\ -q]$ (kompensacja z prędkością lotu) albo bramkować korektę.

10 Karta wzorów i co dalej

Czujniki i statyka

$\boldsymbol\omega_m = \boldsymbol\omega + \mathbf b + \mathbf w$ , $\;\mathbf f = R^{\mathsf T}(\mathbf a^n - \mathbf g^n)$ , $\;\mathbf m = R^{\mathsf T}\mathbf B^n$

$\varphi = \operatorname{atan2}(-f_y, -f_z)$ , $\theta = \operatorname{atan2}(f_x, \sqrt{f_y^2 + f_z^2})$

$\mathbf m_h = R_y(\theta)R_x(\varphi)\mathbf m$ , $\psi = \operatorname{atan2}(-m_{h,y}, m_{h,x})$

zakręt: $\mathbf f = [0;0;-g/\cos\varphi]$ , kompensacja $V[0;\ r;\ -q]$

Filtry

Mahony: $\mathbf e = \sum k_i\,\mathbf v_{m,i}\times\hat{\mathbf v}_i$ , $\dot{\hat q} = \frac12\hat q\otimes(0, \boldsymbol\omega_m - \hat{\mathbf b} + k_P\mathbf e)$ , $\dot{\hat{\mathbf b}} = -k_I\mathbf e$

MEKF: $q = \hat q\otimes(1, \frac12\delta\boldsymbol\theta)$ , $\delta\dot{\boldsymbol\theta} = -\hat{\boldsymbol\omega}\times\delta\boldsymbol\theta - \delta\mathbf b$

$H = [\,[\hat{\mathbf y}]_\times\ \ 0\,]$ , $\hat{\mathbf y} = \hat R^{\mathsf T}\mathbf r^n$

Do przeczytania

Obowiązkowo: R. Mahony, T. Hamel, J.-M. Pflimlin, „Nonlinear complementary filters on the special orthogonal group”, IEEE Trans. Automatic Control 53 (2008), części o filtrze „pasywnym” i z biasem.
Dla chętnych: F. L. Markley, J. L. Crassidis, Fundamentals of Spacecraft Attitude Determination and Control, rozdz. 6.2 (MEKF w wersji satelitarnej, z gwiazdami zamiast akcelerometru); E. J. Lefferts, F. L. Markley, M. D. Shuster, „Kalman filtering for spacecraft attitude estimation” (1982).

Część I jest zamknięta: umiemy opisać orientację, policzyć jej zmiany z momentów sił i oszacować ją z czujników. Teraz potrzebujemy momentów. W części II wchodzi aerodynamika: dlaczego samolot w ogóle leci nosem do przodu, a nie koziołkuje, i co robi z nim ogon. Zaczniemy od statyki, jak u Palletta i Newmana: zanim zapytamy, jak samolot się porusza, zapytamy, czy po zaburzeniu w ogóle chce wrócić.