Projektuję urządzenie do mechanicznej stabilizacji obrazu kamery, czyli tak zwany 3 osiowy gimbal. Do ustalenia orientacji kamery w przestrzeni wykorzystuję sensory: akcelerometr, żyroskop i magnetometr. Dane, które z nich otrzymuję trafiają do filtru
Kod: Zaznacz cały
https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/Od takich obliczeń wyszedłem i na początek metoda ta się sprawdza. Minus tej metody pojawia się wtedy, kiedy oprócz samej stabilizacji do określonego układu, chciałbym jeszcze móc zmieniać ten określony układ, czyli zmieniać miejsce, w które patrzy kamera. Chciałbym mieć możliwość obrotu w osiach o więcej niż ±90°, więc tu pojawia się tzw. gimbal lock, kiedy operujemy na kątach Eulera i przepełnianie się funkcji podczas przeliczeń kwaternionu na postać kątów pitch, roll, yaw.
Wymyśliłem takie rozwiązanie, które nie wiem czy z matematycznego punktu widzenia jest poprawne. Mam kwaternion "p", który jest wyjściem filtru Madgwicka, który odpowiada za określenia pozycji kamery w przestrzeni. Mam też kwaternion "q", który jest pozycją, do której cały układ będzie dążył i początkowo ma wartość [1, 0, 0, 0]. Ten kwaternion "q" będę mógł zmieniać, będzie to ta zmiana pozycji, do której teraz gimbal będzie starał się dążyć, czyli zmiana miejsca, w które aktualnie patrzy kamera. Teraz chciałbym liczyć różnicę pomiędzy kwaternionem "p", a kwaternionem "q" i ten wypadkowy kwaternion "r" dopiero przeliczałbym na kąty Eulera: pitch, roll i yaw. Kiedy te wyliczone kąty będą małe, czyli różnica między kwaternionami będzie mała (a tak będzie, bo cały układ będzie natychmiastowo kompensował tę różnicę) to nigdy nie zbliżę się do wartości bliskich ±90° i nie powinien wystąpić problem, który pojawiał się kiedy od razu przeliczałem kwaternion na kąty Eulera. Czy ten tok myślenia jest poprawny, czy ma to prawo działać, jeśli nie to na co powinienem zwrócić uwagę lub może jakoś inaczej podejść do tematu? Starałem się opisać to w przejrzysty sposób, ale nie wiem czy mi się to udało, więc z chęcią doprecyzuję, jeżeli coś jest niejasne.