Algorytm estymatora kwaternionów

Algorytm estymatora kwaternionów ( QUEST ) to algorytm zaprojektowany do rozwiązania problemu Wahby , który polega na znalezieniu macierzy rotacji pomiędzy dwoma układami współrzędnych na podstawie dwóch zbiorów obserwacji wybranych odpowiednio w każdym układzie. Kluczową ideą algorytmu jest znalezienie wyrażenia funkcji straty dla problemu Wahby w postaci kwadratowej przy użyciu twierdzenia Cayleya – Hamiltona i metody Newtona – Raphsona w celu skutecznego rozwiązania problemu wartości własnej i skonstruowania numerycznie stabilna reprezentacja rozwiązania.

Algorytm został wprowadzony przez Malcolma D. Shustera w 1981 roku podczas pracy w Computer Sciences Corporation . Chociaż algorytm jest w zasadzie mniej niezawodny niż inne metody, takie jak metoda q Davenporta lub rozkład wartości osobliwych , algorytm jest znacznie szybszy i niezawodny w zastosowaniach praktycznych i jest stosowany do rozwiązywania problemów związanych z określaniem położenia w dziedzinach takich jak robotyka i awionika .

Sformułowanie problemu

Problem Wahby polega na znalezieniu $macierzy$ rotacji , która minimalizuje straty

${\ Displaystyle l \ lewo (\ mathbf {A} \ prawo) = {\ Frac {1} {2}} \ suma _{i=1}^{n}a_{i}\left\|\mathbf {w} _{i}-\mathbf {A} \mathbf {v} _{i}\right\|^{2 }}$

gdzie ${\ displaystyle \ mathbf {w} _ {i}}$ to obserwacje wektorów w układzie odniesienia, ${\ displaystyle \ mathbf {v} _ {i}}$ to obserwacje wektorów w układzie ciała, $jest macierzą$$rotacji$ między dwiema klatkami i $textstyle \ suma _ {i}a_{i}=1}$ zbiorem wag takich, jak . Można to przepisać jako problem maksymalizacji funkcji wzmocnienia ${\ displaystyle g}$

${\ Displaystyle g \ lewo (\ mathbf {A} \ prawo) = 1-l \ lewo (\ mathbf {A} \ prawo)=\sum _{i}a_{i}\mathbf {w} _{i}^{\top }\mathbf {A} \mathbf {v} _{i}}$

$maksymalizowana$ w taki sposób, że strata $jest$ minimum, gdy . Zysk $jako$ z kolei przepisać

${\ Displaystyle g \ lewo (\ mathbf {A} \ prawo) = \ operatorname {tr} \ lewo (\ mathbf {A} \ mathbf {B} ^ {\ top }\Prawidłowy)}$

gdzie ${\ Displaystyle \ mathbf {B} = \ Textstyle \ suma _ {i} a_ {i} \ mathbf {w} _ {i} \ mathbf {v} _ {i }^{\top }}$ jest znana jako macierz profilu postawy.

Aby zmniejszyć liczbę zmiennych, problem można przeformułować, parametryzując rotację jako kwaternion jednostkowy q $\ displaystyle \ mathbf {q} = \ lewo (v_ { 1}, v_ {2}, v_ {3}, q \ prawo)}$ z częścią wektorową ${\ Displaystyle \ mathbf {v} = \ lewo (v_ {1}, v_{2},v_{3}\right)}$ i część skalarna ${\ displaystyle q}$ , reprezentujący obrót kąta ${\ displaystyle \ theta = 2 \ cos ^ {-1} q}$ wokół osi, której kierunek jest opisany przez wektor $\ frac {\ theta} {2}}}} \ mathbf {v$$grzech$ ograniczenia jedności, . Można teraz wyrazić w kategoriach parametryzacji kwaternionów jako: ${\ displaystyle \ mathbf {A}}$

${\ Displaystyle \ mathbf {A} = \ lewo (q ^ {2} - \ mathbf {v} \ cdot \ mathbf {v } \right)\mathbf {I} +2\mathbf {v} \mathbf {v} ^{\top }+2q\mathbf {V} _{\times }}$

gdzie jest macierzą skośno-symetryczną ${\ displaystyle \ mathbf {V} _ {\ razy}}$

${\ Displaystyle \ mathbf {V} _ {\ razy} = {\ początek {pmatrix} 0 i v_ {3} i - v_ {2} \\-v_{3}&0&v_{1}\\v_{2}&-v_{1}&0\\\end{pmatrix}}}$ .

Zastępując $kwaternionów$ i upraszczając otrzymane wyrażenie, funkcję wzmocnienia można zapisać w postaci kwadratowej w ${\ displaystyle \ mathbf {A}}$

${\ Displaystyle g (\ mathbf {q}) = \ mathbf {q} ^ {\ top} \ mathbf {K} \ mathbf {q}}$

gdzie $_$ _

${\ Displaystyle \ mathbf {K} = {\ początek {pmatrix} \ mathbf {S} - \ sigma \ mathbf {I} & \ mathbf {z} \\\ mathbf {z} ^{\top }&\sigma \end{pmatrix}}}$

definiuje się na podstawie ilości

${\ Displaystyle {\ początek {wyrównane} \ mathbf {S} & = \ mathbf {B} + \ mathbf {B} ^ {\ top} \\\ mathbf {z} & = \ suma _ {i} a_ {i }\left(\mathbf {w} _{i}\times \mathbf {v} _{i}\right)\\\sigma &=\operatorname {tr} \mathbf {B} .\end{aligned}} }$

Tę postać kwadratową można zoptymalizować w ramach ograniczenia jedności, $Lagrange'a$ uzyskując funkcję

${\ Displaystyle {\ kapelusz {g}} \ lewo (\ mathbf {q} \ prawo) = \ mathbf {q} ^ {\ top} \ mathbf { K} \mathbf {q} -\lambda \mathbf {q} ^{\top }\mathbf {q} }$

który osiąga maksimum, gdy

${\ displaystyle \ mathbf {K} \ mathbf {q} = \ lambda \ mathbf {q}$ }

Oznacza to, że optymalny obrót jest sparametryzowany przez kwaternion, $} }$ wektor własny powiązany z największą wartością własną. $\ displaystyle \ mathbf {q} ^ {*}$ z ${\ displaystyle \ mathbf {K}$ }

Rozwiązanie równania charakterystycznego

Optymalny kwaternion można wyznaczyć rozwiązując równanie charakterystyczne i $.$ wektor własny dla największej wartości własnej Z definicji można przepisać ${\ displaystyle \ mathbf {K}}$

${\ Displaystyle \ mathbf {K} \ mathbf {q} = \ lambda \ mathbf {q}}$

jako układ dwóch równań

${\ Displaystyle {\ początek {wyrównane} \ mathbf {y} & = \ lewo ({\ lambda + \ sigma) \ mathbf { I} -\mathbf {S} \right)^{-1}\mathbf {z} \\\lambda &=\sigma +\mathbf {z} \mathbf {y} \end{aligned}}}$

gdzie jest wektorem Rodriguesa ${\ displaystyle \ mathbf {y} =\textstyle {\ Frac {1} {q}} \ mathbf {v}$ } Zastępując drugie równanie pierwszym, można wyprowadzić wyrażenie równania charakterystycznego ${\ displaystyle \ mathbf {y}}$

${\ Displaystyle \ lambda = \ sigma + \ mathbf {z} ^ {\ góra} \ lewo ({\ lambda + \ sigma) \ mathbf {I} -\mathbf {S} \right)^{-1}\mathbf {z} }$ .

Ponieważ , wynika z tego , że ${\ displaystyle \ lambda _ {\ tekst {max}} = \ max g \ lewo (\ mathbf {A} \ prawo)}$ wynika, że ${\ Displaystyle \ lambda _ {\ tekst {max}} = 1 - \ min l \ lewo (\ mathbf {A} \ prawo)} i dlatego λ max ≈ 1 {\$ Displaystyle $\ tekst {max }} \ około 1}$ dla optymalnego rozwiązania (kiedy strata ${\ displaystyle l}$ jest mały). Pozwala to $na$ skonstruowanie optymalnego kwaternionu , zastępując wektor Rodriguesa ${y} }$$q} ^ {*}$$}$

${\ Displaystyle \ mathbf {q} ^ {*} = {\ Frac {1} {\ sqrt {1+ \ lewo | \ mathbf {y} _ {\ lambda _ {\ tekst {max }}}\right|^{2}}}}(\mathbf {y} ,1)^{\top }}$ .

Wektor $_$$liczbą$ dla _ Alternatywne wyrażenie rozwiązania, które nie uwzględnia wektora Rodriguesa, można skonstruować za pomocą twierdzenia Cayleya – Hamiltona . Charakterystyczne równanie macierzy to: za ${\ displaystyle 3 \ razy$$}$

${\ Displaystyle \ det \ lewo [\ mathbf {S} - \ xi \ mathbf {I} \ prawo] = - \ xi ^{3}+2\sigma \xi ^{2}-k\xi +\Delta =0}$

Gdzie

${\ Displaystyle {\ początek {wyrównane} \ sigma & = {\ Frac {1} {2}} \ nazwa operatora {tr} {\ mathbf {S} }\\k&=\operatorname {tr} \left(\operatorname {adj} \mathbf {S} \right)\\\Delta &=\det \mathbf {S} \end{aligned}}}$

Twierdzenie Cayleya – Hamiltona stwierdza, że ​​każda macierz kwadratowa na pierścieniu przemiennym spełnia swoje własne równanie charakterystyczne, dlatego

${\ Displaystyle - \ mathbf {S} ^ {3} + 2 \ sigma \ mathbf {S} ^ {2} -k \ mathbf {S} + \ Delta =0}$

pozwalając pisać

${\ Displaystyle \ lewo ({\ omega + \ sigma) \ mathbf {I} - \ mathbf {S} \ prawo) ^ {-1}={\frac {\alfa \mathbf {I} +\beta \mathbf {S} +\mathbf {S} ^{2}}{\gamma }}}$

Gdzie

${\ Displaystyle {\ początek {wyrównane} \ alfa i = \ omega ^ {2} - \ sigma ^ {2} +k\\\beta &=\omega -\sigma \\\gamma &=(\omega +\sigma )\alpha -\Delta \end{aligned}}}$

i dla $zapewnia$ optymalnego

${\ Displaystyle {\ początek {wyrównane} \ mathbf {y} ^ {*} i = \ lewo ( (\lambda +\sigma )\mathbf {I} -\mathbf {S} \right)^{-1}\mathbf {z} \\&={\frac {\alpha \mathbf {I} +\beta \ mathbf {S} +\mathbf {S} ^{2}}{\gamma }}\mathbf {z} \end{aligned}}}$

co daje kwaternionową reprezentację optymalnej rotacji jako

${\ Displaystyle \ mathbf {q} ^ {*} = {\ Frac {1} {\ sqrt {\ gamma ^ {2} + \ lewo | \ mathbf {x} \ prawo | ^ { 2}}}}(\mathbf {x},\gamma)^{\top }}$

Gdzie

${\ Displaystyle \ mathbf {x} = \ lewo (\ alfa \ mathbf {I} + \ beta \ mathbf {S} + \ mathbf {S} ^ {2} \right)\mathbf {z} }$ .

Wartość $.$ wyznaczyć jako numeryczne rozwiązanie równania Zastępowanie ${\ Displaystyle \ lewo ({\ omega + \ sigma) \ mathbf {I} - \ mathbf {S} \ prawo) ^ {- 1}}$ wewnątrz wcześniej uzyskanego równanie charakterystyczne

${\ Displaystyle \ lambda = \ sigma + \ mathbf {z} ^ {\ góra} \ lewo ({\ lambda + \ sigma) \ mathbf {I} -\mathbf {S} \right)^{-1}\mathbf {z} }$ .

daje

${\ Displaystyle \ lambda ^ {4} - (a + b) \ lambda ^ {2} -c \ lambda + (ab+c\sigma -d)=0}$

Gdzie

${\ Displaystyle {\ początek {wyrównane} a i = \ sigma ^ {2} -k \\ b&=\sigma ^{2}+\mathbf {z} ^{\top }\mathbf {z} \\c&=\Delta +\mathbf {z} ^{\top }\mathbf {S} \mathbf {z } \\d&=\mathbf {z} ^{\top }\mathbf {S} ^{2}\mathbf {z} \end{aligned}}}$

którego pierwiastek można efektywnie przybliżyć metodą Newtona – Raphsona , przyjmując 1 jako początkowe przypuszczenie rozwiązania w celu zbieżności do najwyższej wartości własnej (wykorzystując fakt pokazany powyżej, że ${\ displaystyle \ lambda _ {\ tekst{max}}\około 1}$ , gdy kwaternion jest bliski rozwiązania optymalnego).

Zobacz też

• Metoda triady
• Problem Wahby

Źródła

• Crassidis, John L; Markley, F. Landis; Cheng, Yang (2007). „Badanie nieliniowych metod szacowania postawy”. Journal of Guidance, Control i Dynamics . 30 (1): 12–28. Kod Biblioteki : 2007JGCD...30...12C . doi : 10,2514/1,22452 .
• Markley, F. Landis; Mortari, Daniele (2000). „Oszacowanie położenia kwaternionów za pomocą obserwacji wektorowych”. Journal of the Astronautical Sciences . Skoczek. 48 (2): 359–380. Kod Bib : 2000JAnSc..48..359M . doi : 10.1007/BF03546284 .
• Psiaki, Mark L. (2000). „Filtrowanie określające postawę poprzez rozszerzoną estymację kwaternionów”. Journal of Guidance, Control i Dynamics . 23 (2): 206–214. Kod Bib : 2000JGCD...23..206P . doi : 10,2514/2,4540 .
• Shuster, lekarz medycyny; Och, SD (1981). „Trzyosiowe wyznaczanie położenia na podstawie obserwacji wektorowych”. Dziennik wytycznych i kontroli . 4 (1): 70–77. Kod Biblijny : 1981JGCD....4...70S . doi : 10.2514/3.19717 .
•   Wu, Jin; Zhou, Zebo; Gao, Bin; Li, Rui; Cheng, Yuhua; Fourati, Hassen (2017). „Szybki liniowy estymator położenia kwaternionów wykorzystujący obserwacje wektorowe” (PDF) . Transakcje IEEE dotyczące nauki i inżynierii automatyki . IEEE. 15 (1): 307–319. doi : 10.1109/TASE.2017.2699221 . S2CID 3455346 .
•   Yun, Xiaoping; Bachmann, Eric R.; McGhee, Robert B. (2008). „Uproszczony algorytm oparty na kwaternionach do szacowania orientacji na podstawie pomiarów grawitacji Ziemi i pola magnetycznego”. Transakcje IEEE dotyczące oprzyrządowania i pomiarów . IEEE. 57 (3): 638–650. doi : 10.1109/TIM.2007.911646 . S2CID 15571138 .

Linki zewnętrzne

• „QUEST — dokumentacja AHRS” .
• Boulder na Uniwersytecie Kolorado. „PYTANIE” . Kinematyka: opisywanie ruchów statku kosmicznego . Kursra.