Układy współrzędnych w UAV

Pozycja UAV nie jest po prostu trójką liczb. Te same wartości mogą oznaczać długość i szerokość geograficzną, metry względem punktu startu, współrzędne globalne względem środka Ziemi albo przesunięcie sensora względem kontrolera lotu. Bez jawnej informacji o układzie odniesienia, jednostkach, początku, kierunkach osi i czasie pomiaru liczby są niejednoznaczne. Błąd w tej warstwie potrafi wyglądać jak awaria GNSS, zły znak regulatora lub dryf estymatora, choć każdy z podsystemów osobno działa poprawnie.

W typowym stosie autonomii dane przechodzą przez kilka reprezentacji. Odbiornik GNSS raportuje położenie geodezyjne w stopniach i wysokość w metrach. Estymator zamienia je na lokalny układ kartezjański, zwykle NED albo ENU. Sterownik operuje na prędkościach i przyspieszeniach w układzie lokalnym oraz na momentach w układzie kadłuba. Kamera może stosować jeszcze inny układ optyczny. Warstwa mapowa na stacji naziemnej używa projekcji płaskiej. Każde przejście wymaga zdefiniowanej transformacji.

Spis treści#

Cztery pojęcia, których nie należy mieszać
Dane pozycyjne jako typ, a nie tablica liczb
WGS 84 i współrzędne geodezyjne
Wysokość elipsoidalna, ortometryczna i AGL
ECEF: globalny układ kartezjański
Przeliczenie geodezyjne na ECEF
Lokalne układy ENU i NED
Transformacja ECEF do ENU
Układ kadłuba FRD
Układy sensorów i kamer
Pozycja, wektor, orientacja i pełna poza
Macierze obrotu, kąty Eulera i kwaterniony
Transformacja kowariancji
Ramki w PX4, MAVLink i ROS
Początek lokalny, home i przesuwanie origin
Projekcje mapowe i UTM
Czas, epoka i ruch układu odniesienia
Precyzja numeryczna
Model danych odporny na pomyłki
Walidacja transformacji
Typowe awarie integracyjne
Procedura integracji nowego sensora
Powiązane tematy
Przypisy

Cztery pojęcia, których nie należy mieszać#

Układ współrzędnych określa osie i sposób zapisu punktu lub wektora. Przykładami są ECEF, ENU i NED. Sam zapis „X, Y, Z” nie wskazuje jeszcze, czy osie są związane z Ziemią, kadłubem, kamerą czy stanowiskiem pomiarowym.

Układ odniesienia nadaje układowi współrzędnych fizyczną realizację: definiuje początek, orientację oraz często epokę. Lokalny NED bez zapisanej szerokości, długości i wysokości jego początku nie pozwala odtworzyć pozycji globalnej.

Datum geodezyjne wiąże model matematyczny Ziemi z jej fizyczną powierzchnią i siecią pomiarową. W lotniczej nawigacji satelitarnej najczęściej spotyka się WGS 84. Nazwa datum nie jest synonimem sposobu zapisu: w WGS 84 można wyrazić położenie zarówno jako szerokość, długość i wysokość, jak i jako współrzędne ECEF.

Projekcja kartograficzna odwzorowuje zakrzywioną powierzchnię odniesienia na płaszczyznę. UTM jest rodziną takich odwzorowań. Projekcja wprowadza kontrolowane zniekształcenia i nie powinna być traktowana jako globalny, jednorodny układ metryczny.

W dokumentacji systemowej te cztery warstwy należy rozdzielać. Zdanie „pozycja jest w WGS84” nadal nie mówi, czy wysokość jest elipsoidalna, czy nad średnim poziomem morza, w jakich jednostkach zapisano kąty i z jakiej epoki pochodzi realizacja układu.

Dane pozycyjne jako typ, a nie tablica liczb#

Bezpieczny interfejs nie przekazuje anonimowego float position[3]. Minimalna definicja pozycji powinna zawierać:

identyfikator układu i konwencji osi;
jednostkę każdej składowej;
identyfikator początku lokalnego albo datum globalnego;
rodzaj wysokości;
znacznik czasu pomiaru;
informację, którego punktu statku dotyczy pozycja;
kowariancję lub przynajmniej oszacowanie jakości;
status poprawności i źródło danych.

Warto traktować GeodeticPosition, EcefPosition, NedVector i BodyVector jako odrębne typy nawet wtedy, gdy język programowania reprezentuje je identycznie. Zapobiega to przypadkowemu dodaniu metrów ECEF do metrów lokalnych albo użyciu prędkości z układu kadłuba jako prędkości północ–wschód–dół.

Szczególnie niebezpieczne jest oznaczanie jedynie komentarzem, że „Z jest wysokością”. W NED dodatnia oś Z prowadzi w dół, w ENU dodatnia oś Z prowadzi w górę, a w ECEF oś Z przechodzi przez umowny biegun północny. Wszystkie trzy zapisy są poprawne, ale nie są zamienne.

WGS 84 i współrzędne geodezyjne#

WGS 84 jest globalnym systemem odniesienia utrzymywanym przez amerykańską National Geospatial-Intelligence Agency. Jego częścią jest elipsoida obrotowa aproksymująca kształt Ziemi. Podstawowe parametry elipsoidy to półoś wielka:

a = 6 378 137,0 m

oraz odwrotność spłaszczenia:

1/f = 298,257223563

Z nich wyznacza się spłaszczenie f i pierwszy mimośród do kwadratu:

e² = f(2 - f)

Pozycja geodezyjna składa się z szerokości geodezyjnej φ, długości geodezyjnej λ i wysokości elipsoidalnej h. Szerokość geodezyjna jest kątem między płaszczyzną równika a normalną do elipsoidy. Nie jest tym samym co szerokość geocentryczna, mierzona od środka Ziemi. Poza równikiem i biegunami różnica jest niezerowa, dlatego w dokładnych obliczeniach nie wolno zastępować elipsoidy kulą bez świadomego oszacowania błędu.

Długość geograficzna opisuje obrót wokół osi Z. W oprogramowaniu trzeba ustalić zakres, na przykład [-180°, 180°) lub [0°, 360°). Różnica dwóch długości w pobliżu południka ±180° wymaga normalizacji. Proste odejmowanie pozycji 179,999° i -179,999° daje niemal 360°, chociaż punkty leżą blisko siebie.

Kąty w komunikatach zewnętrznych często są w stopniach, natomiast funkcje trygonometryczne bibliotek programistycznych przyjmują radiany. MAVLink w części komunikatów zapisuje szerokość i długość jako liczby całkowite w stopniach pomnożonych przez 10^7. Taki zapis ogranicza niejednoznaczność binarnej reprezentacji stopni, ale wymaga jawnego skalowania przed obliczeniami.

Wysokość elipsoidalna, ortometryczna i AGL#

Jedna liczba altitude może oznaczać kilka fizycznie różnych wielkości:

wysokość elipsoidalną h względem elipsoidy odniesienia;
wysokość ortometryczną H względem geoidy, potocznie wiązaną z wysokością nad średnim poziomem morza;
AGL — wysokość nad lokalnym terenem lub przeszkodą;
wysokość względem punktu startu albo pozycji home;
lokalną składową pionową estymatora;
wysokość ciśnieniową z barometru.

Geoidę można rozumieć jako powierzchnię ekwipotencjalną pola grawitacyjnego zbliżoną do średniego poziomu mórz. Jej separacja od elipsoidy, oznaczana zwykle N, zależy od miejsca. W stosowanej często konwencji:

h = H + N

Odbiornik GNSS może raportować zarówno wysokość elipsoidalną, jak i wysokość skorygowaną modelem geoidy. Trzeba sprawdzić definicję konkretnego pola w dokumentacji odbiornika oraz komunikatu. Brak tej kontroli może dać przesunięcie pionowe rzędu wielu metrów, które pozostaje niemal stałe i dlatego bywa błędnie przypisywane kalibracji barometru.

AGL wymaga modelu terenu, pomiaru dalmierzem albo innego źródła odległości od podłoża. Nie wynika bezpośrednio z wysokości GNSS. Różnica wysokości ortometrycznej UAV i wysokości numerycznego modelu terenu daje przybliżenie AGL, lecz dokładność zależy od rozdzielczości, daty i układu wysokościowego mapy. Nad budynkiem lub koronami drzew model gruntu nie opisuje najbliższej przeszkody.

W systemie warto przechowywać nazwę odniesienia pionowego jako osobne pole, na przykład ELLIPSOID, GEOID_MODEL_X, HOME, TERRAIN albo RANGE_SENSOR. Sam sufiks _m określa jednostkę, lecz nie rozwiązuje problemu odniesienia.

ECEF: globalny układ kartezjański#

ECEF — Earth-Centered, Earth-Fixed — jest prawoskrętnym układem kartezjańskim obracającym się razem z Ziemią. Jego początek znajduje się w środku masy Ziemi. Oś Z jest zgodna z umowną osią obrotu, oś X przechodzi przez przecięcie równika i południka zerowego, a oś Y uzupełnia układ prawoskrętny i przecina równik na długości 90°E.

Współrzędne ECEF mają wartości rzędu milionów metrów. Ich zaletą jest brak osobliwości długości geograficznej na biegunach i możliwość wykonywania części operacji wektorowych w jednym globalnym układzie. Wadą jest słaba intuicyjność: różnica X nie oznacza przesunięcia na wschód, a Z nie jest lokalną wysokością.

ECEF nadaje się do przechowywania globalnego punktu bazowego, obliczeń satelitarnych, transformacji pomiędzy odległymi lokalnymi układami oraz fuzji danych wymagającej globalnej geometrii. Do regulatora lotu zwykle wygodniejszy jest lokalny układ styczny, ponieważ wartości są niewielkie i odpowiadają kierunkom związanym z powierzchnią Ziemi.

Przeliczenie geodezyjne na ECEF#

Dla szerokości geodezyjnej φ, długości λ, wysokości elipsoidalnej h, półosi wielkiej a i mimośrodu e promień krzywizny pierwszego wertykału wynosi:

N(φ) = a / sqrt(1 - e² sin²φ)

Współrzędne ECEF oblicza się jako:

X = (N + h) cosφ cosλ
Y = (N + h) cosφ sinλ
Z = (N(1 - e²) + h) sinφ

Kąty w tych wzorach muszą być w radianach. Wysokość h musi być elipsoidalna i wyrażona w tej samej jednostce co a. Podstawienie wysokości nad geoidą bez korekty nie psuje kierunku poziomego, ale przesuwa punkt promieniowo i wprowadza błąd pionowy.

Transformacja odwrotna ECEF → współrzędne geodezyjne jest mniej trywialna. Istnieją algorytmy iteracyjne i zamknięte aproksymacje o różnych własnościach numerycznych. W kodzie produkcyjnym lepiej użyć sprawdzonej biblioteki geodezyjnej, na przykład GeographicLib, niż utrzymywać własną implementację opartą na przypadkowo znalezionym fragmencie kodu. Własna implementacja wymaga testów na biegunach, równiku, dużych i ujemnych wysokościach oraz blisko środka elipsoidy.

Lokalne układy ENU i NED#

Lokalny układ styczny ma początek w wybranym punkcie geodezyjnym. Jego osie są związane z lokalną normalną elipsoidy oraz kierunkami południka i równoleżnika.

W ENU osie prowadzą:

X — East, na wschód;
Y — North, na północ;
Z — Up, w górę.

W NED osie prowadzą:

X — North, na północ;
Y — East, na wschód;
Z — Down, w dół.

Oba układy są prawoskrętne. NED jest powszechny w awionice i autopilotach, ponieważ dodatnia oś Z odpowiada konwencji „w dół” używanej także w układzie kadłuba FRD. ENU często występuje w robotyce, GIS i ekosystemie ROS. Zamiana nie polega jedynie na zmianie znaku Z; trzeba także zamienić pierwsze dwie osie:

north = enu_y
east  = enu_x
down  = -enu_z

Lokalny układ jest dokładną ramką styczną w swoim początku, lecz nie stanowi płaskiego odwzorowania całej Ziemi. W miarę oddalania się od origin kierunek lokalnego „up” zmienia się względem osi początkowych, a płaszczyzna styczna oddala się od elipsoidy. Dla małego obszaru lotu efekt jest pomijalny, ale dla tras dalekiego zasięgu należy ponownie oszacować dopuszczalny błąd albo korzystać z reprezentacji globalnej.

Transformacja ECEF do ENU#

Najpierw punkt oraz origin zamienia się na ECEF. Następnie oblicza się różnicę:

Δr = [X - X0, Y - Y0, Z - Z0]ᵀ

Dla szerokości φ0 i długości λ0 początku lokalnego transformacja do ENU ma postać:

[E]   [-sinλ0             cosλ0            0     ] [ΔX]
[N] = [-sinφ0 cosλ0  -sinφ0 sinλ0     cosφ0    ] [ΔY]
[U]   [ cosφ0 cosλ0   cosφ0 sinλ0     sinφ0    ] [ΔZ]

Wiersze macierzy są wektorami jednostkowymi osi East, North i Up zapisanymi w ECEF. Ponieważ macierz jest ortonormalna, transformację odwrotną realizuje jej transpozycja, po czym trzeba ponownie dodać wektor ECEF origin.

Dla wektora prędkości translacja origin nie występuje — stosuje się wyłącznie obrót. Dla punktu translacja jest konieczna. To rozróżnienie jest ważne w bibliotekach transformacji: punkt, wektor swobodny, kierunek i normalna nie zawsze podlegają tej samej operacji.

Przejście ENU → NED można zapisać macierzą:

[N]   [0  1   0] [E]
[E] = [1  0   0] [N]
[D]   [0  0  -1] [U]

Ręczne wpisywanie tej zamiany w wielu miejscach programu prowadzi do rozbieżności. Lepiej zapewnić jedną przetestowaną funkcję graniczną i wewnętrznie utrzymywać jedną konwencję.

Układ kadłuba FRD#

Układ kadłuba opisuje wielkości względem statku powietrznego. W popularnej konwencji FRD:

X prowadzi do przodu — Forward;
Y prowadzi w prawo — Right;
Z prowadzi w dół — Down.

Układ porusza się i obraca razem z UAV. Prędkość [10, 0, 0] m/s w FRD oznacza ruch dziesięć metrów na sekundę wzdłuż nosa, niezależnie od kursu geograficznego. Ten sam wektor zapisany w NED zależy od orientacji statku.

Żyroskop mierzy prędkość kątową naturalnie w osiach sensora, zwykle sztywno związanych z kadłubem po uwzględnieniu orientacji montażu. Akcelerometr mierzy siłę właściwą, również w swojej ramce. Regulator prędkości kątowej zwykle operuje w osiach kadłuba, ponieważ momenty sterujące roll, pitch i yaw są generowane względem tych osi.

Punkt odniesienia układu kadłuba powinien być określony. Może to być środek ciężkości, geometryczny środek kontrolera lotu albo inny punkt konstrukcyjny. Dla samych obrotów translacja nie ma znaczenia, lecz przy dużych ramionach sensora przyspieszenie mierzone poza środkiem obrotu zawiera składniki styczne i dośrodkowe. W precyzyjnej nawigacji kompensuje się lever arm między anteną GNSS, IMU, kamerą i punktem referencyjnym pojazdu.

Układy sensorów i kamer#

Strzałka nadrukowana na płytce flight controllera nie zawsze odpowiada osi X samego układu IMU. Producent płytki może zamontować sensor obrócony o 90° lub odwrócony, a firmware zawiera transformację z ramki chipa do ramki płytki. Następna transformacja opisuje montaż płytki w pojeździe. Łączenie obu etapów w jedną „magiczną” zamianę znaków utrudnia diagnostykę.

Dla każdego sensora warto zdefiniować:

sensor frame -> board frame -> body frame -> local/world frame

Kamera często stosuje konwencję optyczną, w której oś Z patrzy do przodu wzdłuż osi optycznej, X prowadzi w prawo obrazu, a Y w dół obrazu. Jest to inny układ niż FRD kadłuba. Punkt wykryty w obrazie wymaga użycia parametrów wewnętrznych kamery, transformacji camera-to-body oraz aktualnej orientacji body-to-world. Pomylenie kierunku transformacji może tworzyć poprawnie wyglądający wynik tylko dla zerowych kątów.

Gimbal dodaje ruchomą transformację zależną od enkoderów lub estymowanych kątów. Łańcuch może wyglądać następująco:

camera_optical -> gimbal_payload -> body_FRD -> local_NED -> ECEF

Każda krawędź ma kierunek, czas obowiązywania i niepewność. Transformacja kadłub–kamera skalibrowana na stole nie zastępuje pomiaru chwilowych kątów gimbala.

Pozycja, wektor, orientacja i pełna poza#

Pozycja wskazuje punkt względem origin i podlega obrotowi oraz translacji. Wektor swobodny, na przykład prędkość względna, podlega obrotowi, ale nie translacji. Orientacja opisuje obrót jednej ramki względem drugiej. Poza łączy pozycję i orientację.

W zapisie macierzowym pełną transformację sztywną można przedstawić macierzą jednorodną:

T_AB = [ R_AB  t_AB ]
       [ 0 0 0   1  ]

Indeksy muszą mieć jednoznaczną semantykę. Jedna użyteczna konwencja mówi, że T_AB zamienia współrzędne punktu z ramki B na ramkę A:

p_A = T_AB p_B

Wtedy składanie transformacji jest naturalne:

T_AC = T_AB T_BC

Nie istnieje uniwersalna konwencja nazw używana przez wszystkie biblioteki. Dlatego trzeba ją zapisać w dokumentacji projektu i sprawdzić eksperymentem z prostym obrotem 90°. Nazwa zmiennej body_to_world bywa rozumiana jako obrót wektora albo jako opis orientacji ramki; te interpretacje mogą prowadzić do macierzy wzajemnie transponowanych.

Transformacja odwrotna obrotu to transpozycja macierzy R. Dla pełnej pozy:

T_BA = [ R_ABᵀ  -R_ABᵀ t_AB ]
       [ 0 0 0          1     ]

Samo zanegowanie translacji bez jej obrócenia jest poprawne tylko w szczególnym przypadku.

Macierze obrotu, kąty Eulera i kwaterniony#

Macierz obrotu R jest ortonormalna i ma wyznacznik +1. Zachowuje długości i kąty. Numeryczna integracja może stopniowo naruszać ortogonalność, dlatego estymatory stosują odpowiednią parametryzację i normalizację.

Kąty roll, pitch i yaw są intuicyjne dla operatora, lecz wymagają ustalonej kolejności obrotów. Obroty nie są przemienne: roll po yaw daje inny wynik niż yaw po roll. Dodatkowo reprezentacja Eulera ma osobliwość przy określonych orientacjach. Nadaje się do prezentacji i części lokalnych regulatorów, ale nie jest najlepszym formatem do ogólnego składania orientacji.

Kwaternion jednostkowy zapisuje orientację czterema liczbami i pozwala unikać osobliwości kątów Eulera. Nadal nie rozwiązuje problemu semantyki kierunku. Trzeba wiedzieć, czy q_AB obraca współrzędne z B do A, czy opisuje obrót ramki A względem B, oraz czy biblioteka zapisuje część skalarną jako pierwszą, czy ostatnią. Kwaterniony q i -q reprezentują tę samą orientację, co ma znaczenie przy interpolacji i porównywaniu ciągłości próbek.

Transformacja aktywna obraca fizyczny wektor w tej samej ramce. Transformacja pasywna zmienia współrzędne tego samego wektora między ramkami. Dla tej samej geometrii macierze są odwrotne. W integracji UAV najczęściej interesuje nas zmiana reprezentacji, ale dokumentacje bibliotek mogą opisywać operację językiem obrotu aktywnego.

Transformacja kowariancji#

Zmiana układu dotyczy nie tylko wartości średniej. Jeżeli wektor losowy ma kowariancję P w jednej ramce, po obrocie R jego kowariancja wynosi:

P' = R P Rᵀ

Przepisanie macierzy bez obrotu jest błędne, zwłaszcza gdy dokładność osi różni się znacząco. Przykładem jest GNSS o lepszej dokładności poziomej niż pionowej albo lidar mierzący bardzo dokładnie wzdłuż wiązki, ale słabiej poprzecznie.

Dla nieliniowej transformacji geodezyjnej kowariancję można lokalnie przenieść przez jakobian:

P_y = J P_x Jᵀ

Przy dużych niepewnościach liniowe przybliżenie może być niewystarczające. W typowym UAV obszar niepewności GNSS jest jednak mały względem promienia Ziemi, więc linearyzacja wokół bieżącej pozycji jest zwykle użyteczna.

Pełna poza ma niepewność translacji, orientacji i ich korelacje. Proste przesłanie osobnych trzech wariancji pozycji i trzech wariancji kątów gubi korelacje. To może być wystarczające dla wizualizacji, ale nie dla ścisłej fuzji wielosensorowej.

Ramki w PX4, MAVLink i ROS#

PX4 wewnętrznie używa między innymi lokalnej konwencji NED oraz układu kadłuba FRD. ROS historycznie preferuje konwencje ENU dla świata oraz FLU — Forward, Left, Up — dla kadłuba. Most PX4–ROS musi więc zmienić kolejność osi i znaki zarówno dla pozycji, jak i orientacji, prędkości, prędkości kątowej i kowariancji. Poprawna konwersja samego wektora pozycji nie wystarcza.

MAVLink definiuje wiele wartości MAV_FRAME, między innymi ramki globalne, lokalne NED, body i warianty z różnymi odniesieniami wysokości. Niektóre historyczne warianty są oznaczone jako przestarzałe. Znaczenie pola trzeba odczytać z dokumentacji konkretnego komunikatu, ponieważ identycznie nazwane składowe x, y, z mogą być pozycją, przesunięciem albo prędkością zależnie od maski typów.

Przy obsłudze komend pozycyjnych trzeba osobno sprawdzić:

czy wartości są absolutne, czy względne;
czy szerokość i długość są float, czy skalowanym int32;
czy Z jest wysokością dodatnią w górę, czy lokalnym down;
jakie jest odniesienie wysokości;
czy nieużywane pola należy wyzerować, ustawić na NaN, czy oznaczyć maską;
do jakiego czasu odnosi się stan użyty przez regulator.

Nie należy projektować wewnętrznego modelu danych jako kopii pojedynczego komunikatu MAVLink. Protokół transportowy optymalizuje kompatybilność i przepustowość; model domenowy aplikacji powinien zachowywać pełną semantykę i dopiero na granicy mapować ją na ramkę protokołu.

Początek lokalny, home i przesuwanie origin#

Lokalny origin estymatora, pozycja startu i home nie muszą być tym samym punktem. Origin może zostać ustanowiony po uzyskaniu dobrej pozycji GNSS, home może być później przestawiony przez operatora, a punkt startu jest obserwacją zdarzenia fizycznego. Łączenie ich w jedną zmienną prowadzi do nieoczekiwanych skoków misji powrotnej lub mapy.

Definicja origin musi obejmować pozycję geodezyjną i wysokość właściwego rodzaju. Jeżeli lokalne (0,0,0) jest związane z wysokością elipsoidalną, nie można później odtwarzać globalnego Z przy użyciu wysokości MSL bez modelu geoidy.

Przesunięcie origin może być potrzebne podczas długiej misji, przy przejściu z GNSS do lokalizacji wizyjnej albo w celu zachowania precyzji liczb zmiennoprzecinkowych. Taka zmiana nie powinna powodować fizycznego skoku estymowanej pozycji. Wszystkie aktywne trajektorie, mapy lokalne, bufory pomiarów i punkty misji muszą zostać przetransformowane albo jawnie przypisane do starej ramki.

SLAM może skorygować wcześniejszą mapę po zamknięciu pętli. Ramka mapy wtedy przesuwa się względem ramki odometrycznej, mimo że krótkoterminowa odometria pozostaje ciągła. Architektura znana z robotyki rozdziela stabilną lokalnie ramkę odom od globalnie korygowanej map. Bez takiego rozdzielenia korekta mapy może pojawić się regulatorowi jako nagły błąd pozycji.

Projekcje mapowe i UTM#

Operator oczekuje mapy płaskiej, a planista może potrzebować współrzędnych metrycznych. Projekcja UTM dzieli świat na strefy i wykorzystuje poprzeczne odwzorowanie Mercatora. W obrębie odpowiedniej strefy zapewnia wygodne wartości easting i northing, lecz nie jest jednym globalnym układem.

Sam zestaw dwóch liczb UTM jest niepełny. Potrzebny jest numer strefy, półkula, datum oraz jednostka. Trasa przekraczająca granicę strefy wymaga szczególnej obsługi. Odejmowanie współrzędnych z dwóch różnych stref nie daje fizycznego wektora przesunięcia.

Do krótkiej misji można zastosować lokalną ramkę styczną. Do wizualizacji regionalnej używa się odpowiedniej projekcji. Do globalnej wymiany punktów bezpieczniejsze są współrzędne geodezyjne lub ECEF. Wybór powinien wynikać ze skali problemu, a nie z wygody pierwszej biblioteki mapowej.

Odległość dwóch punktów na elipsoidzie nie jest ogólnie długością cięciwy ECEF ani odległością euklidesową w stopniach. Dla małych odległości lokalna aproksymacja ENU działa dobrze. Dla odległych punktów należy użyć rozwiązania geodezyjnego na elipsoidzie. GeographicLib udostępnia stabilne algorytmy również dla trudnych par punktów bliskich antypodom.

Czas, epoka i ruch układu odniesienia#

Transformacja przestrzenna bez czasu może być niewystarczająca. Sensory próbkują w różnych chwilach, pojazd porusza się, a gimbal obraca. Obraz z kamery trzeba powiązać z orientacją odpowiadającą ekspozycji, nie z najnowszą orientacją dostępną po zakończeniu przetwarzania. Kilkadziesiąt milisekund błędu czasu przy szybkim obrocie może dominować nad błędem kalibracji geometrycznej.

Znacznik czasu powinien określać moment pomiaru, a nie tylko moment odbioru komunikatu. Należy również znać skalę czasu i domenę zegara: czas monotoniczny autopilota, UTC, GPS Time i czas systemowy komputera nie są automatycznie zamienne. Synchronizacja zegarów oraz estymacja opóźnień są częścią geometrii systemu pomiarowego.

Globalne układy odniesienia mają realizacje związane z epoką. Ruch płyt tektonicznych powoduje, że współrzędne precyzyjnego punktu geodezyjnego mogą zmieniać się w czasie. Dla zwykłego UAV i dokładności metrowej efekt jest zwykle pomijalny. Dla wieloletniej bazy RTK, pomiarów geodezyjnych i dokładności centymetrowej należy przechowywać realizację układu oraz epokę zamiast zakładać, że napis „WGS84” wyczerpuje definicję.

Precyzja numeryczna#

Wartości ECEF mają moduł około 6,4 · 10^6 m. Liczba float32 ma około siedmiu cyfr znaczących, więc przechowywanie bezwzględnego ECEF w pojedynczej precyzji nie pozwala zachować centymetrowych różnic. Kwantyzacja może osiągać dziesiąte części metra lub więcej zależnie od wartości. Dlatego globalne obliczenia geodezyjne zwykle wykonuje się w double, a do sterownika przekazuje małe różnice w lokalnej ramce.

Nie wystarczy obliczyć dwa duże punkty ECEF w float, a następnie je odjąć. Informacja o małej różnicy mogła już zostać utracona. Oba punkty i odejmowanie muszą zachować odpowiednią precyzję. Alternatywnie można użyć algorytmu wyznaczającego lokalne przesunięcie bez degradacji pośredniej.

Stopnie geograficzne także nie są metrami. Jeden stopień szerokości odpowiada w przybliżeniu podobnej odległości na całym globie, ale jeden stopień długości skraca się z cosinusem szerokości i zanika na biegunie. Stały przelicznik „111 km na stopień” jest jedynie lokalną aproksymacją i nie nadaje się do dokładnego planowania ruchu w obu osiach.

Przy normalizacji kwaternionu trzeba obsłużyć normę bliską zeru, NaN oraz przerwy danych. Przy macierzach obrotu warto monitorować R Rᵀ ≈ I i det(R) ≈ 1. Macierz z wyznacznikiem -1 opisuje odbicie, nie czysty obrót; często zdradza zamianę pojedynczego znaku bez poprawnej zmiany pozostałych osi.

Model danych odporny na pomyłki#

Praktyczny rekord pozycji może wyglądać koncepcyjnie tak:

position:
  representation: geodetic
  datum: WGS84
  latitude_deg: 52.0000000
  longitude_deg: 21.0000000
  height_m: 121.4
  height_reference: ellipsoid
  point_of_reference: gnss_antenna_phase_center
  timestamp_ns: 1844674400000
  clock: vehicle_monotonic
  covariance_frame: local_enu
  covariance_m2: [0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.16]
  source: gnss_primary

Dla pozycji lokalnej potrzebne jest dodatkowo odwołanie do definicji origin:

position:
  representation: cartesian
  frame: local_ned
  origin_id: estimator_origin_17
  x_north_m: 14.2
  y_east_m: -3.7
  z_down_m: -22.1
  timestamp_ns: 1844674400000

origin_id jest ważniejszy niż sama nazwa local_ned. Dwa układy NED ustanowione w różnych punktach nie są tą samą ramką. Rejestr origin powinien być niezmienny i zawierać pozycję globalną, czas ustanowienia, źródło oraz odniesienie wysokości.

W kodzie wbudowanym pełne struktury mogą być kosztowne. Nadal warto utrzymywać semantykę na poziomie nazw typów, tematów komunikacji i dokumentacji interfejsów. Optymalizacja pakietu transmisyjnego nie powinna przenosić niejednoznaczności do logiki sterowania.

Walidacja transformacji#

Transformację należy testować na punktach o znanym znaczeniu, nie tylko na przypadkowych danych. Minimalny zestaw obejmuje:

Punkt na równiku i południku zerowym przy h=0, dla którego ECEF powinien być bliski [a, 0, 0].
Punkt na równiku i długości 90°E, dla którego dominującą składową jest Y.
Biegun północny, gdzie X i Y są bliskie zeru.
Punkt origin lokalnego, który po transformacji daje [0,0,0].
Małe przesunięcie na północ, wschód i w górę sprawdzające znaki wszystkich osi.
Przejście przez południk ±180°.
Pozycję o ujemnej wysokości.
Test round-trip geodetic → ECEF → geodetic z tolerancją zależną od zastosowanego algorytmu.

Dla orientacji stosuje się proste wektory bazowe. Jeżeli pojazd stoi poziomo i wskazuje północ, oś body X powinna pokrywać się z osią N lokalnego NED. Po obrocie yaw o +90° zgodnie z konwencją systemu należy jawnie sprawdzić, czy nos wskazuje wschód. Ten test ujawnia jednocześnie znak kąta, kolejność mnożenia i kierunek kwaternionu.

Testy własności są równie użyteczne:

R⁻¹ ≈ Rᵀ
R Rᵀ ≈ I
det(R) ≈ 1
T_AB T_BA ≈ I
norm(Rv) ≈ norm(v)

W testach integracyjnych trzeba odtwarzać rzeczywiste komunikaty zarejestrowane w locie. Jednostkowo poprawna transformacja może być używana z niewłaściwą ramką wejściową, błędnym timestampem albo starym origin.

Typowe awarie integracyjne#

NED potraktowane jako ENU#

Objawem może być ruch mapy na wschód przy locie na północ, odwrócona wysokość albo reakcja regulatora w niewłaściwej osi. Konwersja wymaga zamiany X/Y i negacji Z. Dla orientacji nie wolno przestawić jedynie trzech wartości kąta; trzeba przekształcić pełną reprezentację obrotu.

Wysokość GNSS pomieszana z MSL#

Tor poziomy wygląda poprawnie, a pion ma stały offset zależny od regionu. Należy sprawdzić dokumentację odbiornika, komunikatu i zastosowany model geoidy. Odejmowanie „stałej znalezionej na lotnisku” maskuje błąd i przestaje działać po zmianie obszaru.

Stopnie przekazane do funkcji oczekującej radianów#

Wynik może nadal być skończony i pozornie wiarygodny, dlatego potrzebne są jawne typy albo nazwy z sufiksami _deg i _rad. Konwersję wykonuje się raz na granicy modułu.

Wysokość dodatnia w górę użyta jako NED Z#

Autopilot otrzymuje cel po przeciwnej stronie punktu odniesienia. Znak osi pionowej należy sprawdzać razem z rodzajem wysokości; poprawna negacja nie naprawi pomieszania ellipsoid/MSL/home.

Transformacja w złym kierunku#

System działa przy zerowej orientacji, lecz błąd rośnie podczas obrotu. Pomaga zapis równania p_A = T_AB p_B obok definicji funkcji i test z pojedynczym obrotem 90°.

Pominięty lever arm#

Podczas obrotu w miejscu antena GNSS lub kamera zatacza łuk, chociaż środek ciężkości pozostaje prawie nieruchomy. Dla małego multirotora efekt bywa niewielki, lecz przy długich skrzydłach, wielu antenach lub dokładnym RTK staje się mierzalny.

Przestarzała transformacja gimbala#

Geolokalizacja celu przesuwa się wraz z prędkością ruchu gimbala. Należy interpolować pozę dla czasu ekspozycji i uwzględniać opóźnienie toru obrazu.

Utrata precyzji ECEF#

Pozycja lokalna skacze stopniami kwantyzacji mimo dokładnego źródła. Trzeba sprawdzić, czy bezwzględne współrzędne globalne nie zostały wcześniej zapisane jako float32.

Niejawna zmiana origin#

Ślad lotu i waypointy przesuwają się skokowo po restarcie estymatora lub odzyskaniu GNSS. Każdy origin powinien mieć identyfikator, a odbiorca powinien odrzucać lub przeliczać dane odnoszące się do innej wersji.

Zła kolejność osi kowariancji#

Estymator ufa niewłaściwej osi i może odrzucać dobre pomiary. Kowariancja musi przejść tę samą transformację co wektor, a format tablicy powinien jasno określać układ wierszy i kolumn.

Procedura integracji nowego sensora#

Zidentyfikuj fizyczny punkt pomiaru. Dla GNSS jest to centrum fazowe anteny, dla kamery centrum projekcji, a dla IMU umowny punkt układu MEMS.
Odczytaj definicję osi producenta. Nie wyprowadzaj jej wyłącznie z wyglądu obudowy.
Zapisz jednostki, skalowanie i znaki. Uwzględnij stopnie/radiany, g/m/s², stopnie/s/rad/s oraz endianowość.
Wyznacz statyczną transformację sensor → body. Zachowaj osobno obrót i lever arm.
Określ domenę czasu i opóźnienie. Sprawdź, czy timestamp oznacza początek ekspozycji, koniec konwersji czy odbiór pakietu.
Przenieś kowariancję. Jeśli producent podaje tylko wskaźnik jakości, udokumentuj sposób mapowania na niepewność.
Przetestuj osie pojedynczo. Obróć lub przesuń stanowisko w jednym znanym kierunku.
Sprawdź zachowanie dynamiczne. Statyczny test nie ujawni błędów czasu i lever arm.
Zapisz wersję kalibracji. Zmiana mocowania sensora tworzy nową transformację.
Rejestruj dane surowe i po transformacji. Umożliwia to rozdzielenie błędu sensora od błędu geometrii.

Najważniejszą zasadą jest wykonywanie konwersji na dobrze zdefiniowanych granicach. Jeśli każdy moduł dowolnie zamienia ENU, NED i FRD, liczba ścieżek rośnie, a przetestowanie wszystkich kombinacji staje się nierealne. Ustal jeden układ wewnętrzny dla każdej warstwy i zachowaj pełną informację o ramce w logach.

Powiązane tematy#

Osie roll, pitch i yaw — orientacja i ruch obrotowy kadłuba.
GNSS w UAV — źródła pozycji globalnej, jakość rozwiązania i błędy pomiaru.
IMU — pomiary inercyjne w układzie sensora i kadłuba.
MAVLink — ramki współrzędnych w protokole telemetrii i sterowania.
PX4 — konwencje NED/FRD w stosie autopilota.
Nawigacja inercyjna — propagacja stanu między pomiarami absolutnymi.
Filtr Kalmana i EKF — stan, obserwacje i transformacja niepewności.
Masa i środek ciężkości UAV — fizyczne punkty odniesienia kadłuba.

Przypisy#

National Geospatial-Intelligence Agency, World Geodetic System 1984 — oficjalne definicje i dokumenty WGS 84.
ESA Navipedia, Transformations between ECEF and ENU coordinates — definicje osi i macierze transformacji lokalnej.
Charles F. F. Karney, GeographicLib — biblioteka oraz dokumentacja dokładnych obliczeń geodezyjnych.
MAVLink, Coordinate Frames — definicje ramek używanych w komunikatach protokołu.
PX4, Reference Frames and ROS — konwencje NED/FRD i zasady integracji z ROS 2.
u-blox, ZED-F9P Integration Manual — przykład definicji danych GNSS i zagadnień integracji odbiornika precyzyjnego.

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

Źródła z centralnego rejestru

NGA Geomatics: World Geodetic System 1984 [standard geodezyjny]
ESA Navipedia: Transformations between ECEF and ENU coordinates [materiał techniczny ESA]
GeographicLib documentation [dokumentacja projektu open source]
MAVLink Common Message Set: coordinate frames and LOCAL_POSITION_NED [specyfikacja protokołu]
PX4: Reference Frames and ROS [dokumentacja projektu]
u-blox NEO-F9P Integration Manual [dokumentacja]