Visual odometry i VIO w UAV

Visual odometry (VO) estymuje ruch kamery z kolejnych obrazów, a visual-inertial odometry (VIO) łączy ten tor z IMU. Wynikiem jest lokalna pozycja, orientacja i zwykle prędkość względem układu utworzonego przy inicjalizacji. Nie jest to automatycznie pozycja geograficzna ani mapa wolna od dryfu. System może wyglądać poprawnie w podglądzie trajektorii, a mimo to nie nadawać się do zamknięcia pętli sterowania z powodu opóźnienia, złej ekstrynsyki, skoków po relokalizacji albo nieuczciwie raportowanej kowariancji.

W UAV kamera i IMU tworzą przyrząd nawigacyjny, nie tylko zestaw urządzeń podłączonych do komputera pokładowego. Obiektyw, ekspozycja, rolling shutter, timestamp akwizycji, sztywność uchwytu, położenie względem środka obrotu, temperatura IMU oraz mechanizm resetów są częścią jednego kontraktu pomiarowego. Ten artykuł opisuje ten kontrakt od geometrii obrazu do komunikatu ODOMETRY i kryteriów dopuszczenia do lotu bez GNSS.

Spis treści#

VO, VIO i SLAM — różne produkty
Co jest obserwowalne
Architektura potoku
Model kamery
Kalibracja intrinsics
Ekstrynsyka camera–IMU
Czas, ekspozycja i synchronizacja
Global shutter i rolling shutter
Frontend wizyjny
Cechy i śledzenie
Odrzucanie błędnych korespondencji
Geometria dwóch widoków
Monocular, stereo i RGB-D
Inicjalizacja
Fuzja z IMU
Filtr MSCKF i optymalizacja sliding-window
Keyframes, marginalizacja i map points
VO a loop closure
Układy odom, map, body i camera
Integracja z autopilotem
Kowariancja, quality i health
Resety i ciągłość sterowania
Degeneracje
Wymagania sprzętowe
Logowanie i metryki
Walidacja offline
SITL, HIL i próby w locie
Kryteria odbioru
Typowe błędy
Powiązane tematy
Przypisy

VO, VIO i SLAM — różne produkty#

Nazwy są często używane zamiennie, choć opisują inne zadania:

System	Główne wejście	Typowy wynik	Własność istotna dla UAV
optical flow	kolejne obrazy, opcjonalnie gyro/range	ruch obrazu lub prędkość	mały koszt, ograniczona geometria
visual odometry	obrazy mono/stereo/depth	lokalny przyrost pozy i trajektoria	dryf rośnie z przebytą drogą
VIO	obrazy + IMU	lokalna poza, prędkość, biasy	większa częstotliwość i lepsza skala
visual SLAM	obrazy, często IMU	poza + mapa + relokalizacja	może korygować wcześniejszy dryf
place recognition	obraz/descriptors	identyfikacja znanego miejsca	nie musi dawać metrycznej pozy

VO odpowiada przede wszystkim na pytanie „jak kamera przesunęła się od poprzednich klatek?”. SLAM utrzymuje reprezentację środowiska i może rozpoznać wcześniej odwiedzone miejsce. Loop closure koryguje graf pozy, dlatego globalna trajektoria może skoczyć. To zachowanie jest pożądane dla mapy, ale bezpośrednie podanie skorygowanej pozycji do regulatora może wywołać gwałtowną komendę.

VIO nie oznacza automatycznie SLAM. OpenVINS jest przykładem filtra visual-inertial opartego na MSCKF, a VINS-Mono łączy optymalizację sliding-window z dodatkowymi mechanizmami, takimi jak detekcja pętli i globalny pose graph. ORB-SLAM3 obsługuje tryby visual oraz visual-inertial i wiele map. Nazwa biblioteki nie zwalnia integratora z ustalenia, który jej strumień jest ciągły, a który podlega korektom globalnym.

Co jest obserwowalne#

Kamera mierzy kierunek do punktów sceny. Dla punktu P w układzie kamery model pinhole daje po normalizacji:

x = X / Z
y = Y / Z
u = f_x x + c_x
v = f_y y + c_y

Pojedynczy obraz nie podaje głębokości Z. Monocular VO odzyskuje strukturę i translację tylko do wspólnego współczynnika skali. Obrót jest metrycznie niezależny od skali, ale translacja t i scena P mogą zostać jednocześnie pomnożone przez tę samą wartość bez zmiany projekcji.

Skala staje się obserwowalna przez:

znaną bazę stereo;
pomiar depth/range;
dynamikę IMU i znaną wartość grawitacji;
obiekt lub znacznik o znanych wymiarach;
inne źródło metryczne, np. GNSS lub lidar.

IMU pomaga, ale nie usuwa wszystkich nieobserwowalności. Globalne przesunięcie lokalnego układu pozostaje dowolne. Globalny yaw względem północy nie wynika z samej grawitacji. Przy niewystarczającym pobudzeniu skala, biasy, prędkość i ekstrynsyka mogą być słabo rozdzielalne. Dlatego inicjalizacja podczas prawie nieruchomego zawisu, czystej rotacji lub ruchu z niemal stałą prędkością może dać rozwiązanie numeryczne bez dobrej obserwowalności.

Wymaganie „pozycja 6DoF” powinno być rozwinięte do tabeli:

Składowa	Odniesienie	Ciągłość	Globalna poprawność
roll/pitch	grawitacja + vision/IMU	zwykle ciągła	lokalnie obserwowalna
yaw	początkowa oś lokalna	dryfuje	bez zewnętrznego odniesienia nie jest north
pozycja XYZ	origin lokalny	dryfuje	bez mapy/GNSS nie jest geograficzna
prędkość	układ lokalny lub body	zależy od estymatora	metryczna po poprawnej skali
covariance	model konkretnego estymatora	musi rosnąć przy degradacji	nie jest gwarancją prawdy

Architektura potoku#

Typowy system VIO ma dwie szybkości działania:

camera frames ─→ frontend ─→ feature tracks ─┐
                                             ├→ backend → pose/velocity/bias/covariance
IMU samples ─→ timestamp/buffer/preintegrate ┘                  │
                                                                ↓
                                  health/reset/frame adapter → autopilot

Frontend dekoduje obraz, koryguje model obiektywu, wykrywa lub śledzi cechy i odrzuca część outlierów. Backend propaguje stan IMU i wykorzystuje reprojection residuals do korekcji. Adapter systemowy przekształca osie, kompensuje lever arm, przypisuje timestamp, koduje covariance, wykrywa reset i pilnuje terminowości.

Podział na procesy jest ważny. Utrata pojedynczej ramki nie może blokować publikacji wszystkich innych danych, a chwilowe obciążenie loop closure nie może zagłodzić sterownika IMU. Proces nadzorujący powinien rozróżniać:

brak nowych obrazów;
brak nowych próbek IMU;
opóźnienie backendu;
utratę trackingu;
restart procesu;
relokalizację lub zmianę mapy;
wynik świeży obliczeniowo, ale dotyczący starego czasu akwizycji.

Model kamery#

Macierz kamery pinhole jest zwykle zapisywana:

K = [ f_x   0   c_x ]
    [  0   f_y  c_y ]
    [  0    0    1  ]

Rzeczywisty obiektyw wprowadza dystorsję. Model radialno-tangencjalny może używać k1, k2, k3, p1, p2, ale szerokokątna kamera fisheye często wymaga innego modelu. Dobór modelu jest częścią identyfikacji. Dopasowanie wielomianu o wysokim stopniu do niewystarczającego zbioru może zmniejszyć średni błąd reprojekcji, jednocześnie pogarsając ekstrapolację na brzegach obrazu.

Parametry intrinsics zależą od całego toru:

obiektywu i ustawienia ostrości;
rozdzielczości i obszaru odczytu matrycy;
binningu, cropu i resize;
stabilizacji cyfrowej;
temperatury i mechanicznego położenia soczewek;
tego, czy obraz jest już prostowany przez sterownik.

Kalibracja dla 1280×720 nie może być bezpośrednio użyta po zmianie na 640×480, jeśli pipeline zmienił proporcje, crop albo skalowanie. Jeżeli resize jest czystym skalowaniem przez s_x, s_y, odpowiednio skaluje się f_x, f_y, c_x, c_y; dystorsję stosuje się zgodnie z definicją modelu. Bez znajomości operacji obrazu bezpieczniej kalibrować dokładny strumień dostarczany estimatorowi.

Kalibracja intrinsics#

OpenCV udostępnia modele kalibracji i funkcje wyznaczania pozy planszy, lecz samo wywołanie calibrateCamera() nie kwalifikuje sensora. Zbiór powinien pokrywać:

środek i wszystkie narożniki obrazu;
różne odległości;
różne pochylenia planszy;
pełny zakres ostrości używany w locie;
ekspozycję bez rozmycia;
temperaturę reprezentatywną dla pracy.

Po kalibracji ocenia się nie tylko globalny RMS, ale residual w funkcji położenia piksela, kąta planszy i klatki. Systematyczny wzór na brzegach oznacza niepasujący model lub deformację obrazu. Jedna błędna klatka może podnieść RMS; z kolei setki niemal identycznych widoków mogą dać mały RMS, ale źle uwarunkować ogniskową i dystorsję.

Minimalny raport kalibracji powinien zawierać:

camera serial / lens / focus lock
resolution / crop / pixel format
camera model and coefficient order
K and distortion coefficients
mean, median and percentile reprojection error
coverage map of calibration observations
dataset checksum and tool version
temperature and date

Po uderzeniu, wymianie obiektywu, zmianie ostrości albo uchwytu kalibrację trzeba traktować jako podejrzaną. Parametry zapisane w pliku bez identyfikatora konkretnego egzemplarza łatwo przypisać do niewłaściwej kamery.

Ekstrynsyka camera–IMU#

Estimator potrzebuje transformacji między układem IMU I a kamery C:

T_IC = [ R_IC  p_I_in_C ]
       [  0       1     ]

Konwencja musi określać kierunek transformacji. T_IC bywa rozumiane jako „poza C w I” albo operacja przekształcająca współrzędne z I do C. Plik YAML bez dokumentacji konwencji może mieć liczby poprawne, lecz użyte odwrotnie.

Rotacja ekstrynsyczna wpływa bezpośrednio na porównanie ruchu obrazu z gyro. Błąd kilku stopni miesza osie i tworzy residual zależny od manewru. Translacja określa lever arm:

v_camera = v_imu + omega × r_IC

Podczas szybkiego yaw kamera oddalona od IMU ma rzeczywistą prędkość liniową. Estimator z zerową translacją może wyjaśnić ją ruchem całej platformy lub błędami cech.

OpenVINS zaleca wspólną kalibrację camera–IMU i zwraca uwagę na potrzebę pobudzenia wielu osi. Uchwyt musi być sztywny. Miękka izolacja kamery, autofocus przemieszczający optykę albo elastyczna belka zmieniają ekstrynsykę podczas lotu; nie jest to szum, który da się usunąć zwiększeniem covariance.

Kalibracja online może korygować małe błędy, ale zwiększa liczbę stanów i wymaga obserwowalnego ruchu. Produkcyjny system powinien mieć dobrą kalibrację offline oraz wąskie, uzasadnione priory online. Pozostawienie całkowicie swobodnej ekstrynsyki pozwala estimatorowi dopasować ją do błędów czasu lub modelu kamery.

Czas, ekspozycja i synchronizacja#

Najważniejszy timestamp obrazu dotyczy czasu, w którym fotony utworzyły pomiar, nie chwili odebrania bufora przez aplikację. Łańcuch może wyglądać tak:

exposure → sensor readout → CSI/USB → kernel buffer → userspace → decode → estimator

Timestamp przy wejściu do estymatora zawiera zmienne opóźnienie całego łańcucha. Przy stałym ruchu błąd czasu Δt daje w przybliżeniu błąd pozycji vΔt, a przy obrocie błąd orientacji ωΔt. W dynamicznym locie nawet mały offset może powodować residual skorelowany z body rate i pozorny błąd ekstrynsyki.

Potrzebne są cztery osobne wielkości:

czas początku i środek ekspozycji;
czas odczytu ramki lub wiersza;
offset zegara camera–IMU;
latency publikacji wyniku i jego jitter.

Sprzętowy trigger i wspólny licznik są najlepszym punktem wyjścia. Jeśli urządzenia mają osobne zegary, trzeba estymować offset i dryf, a nie tylko jednorazowo odjąć stałą. NTP może synchronizować komputery, ale nie naprawi nieznanego opóźnienia wewnętrznego kamery USB.

OpenVINS i VINS-Mono przewidują estymację offsetu czasowego. Jest to narzędzie diagnostyczne i estymacyjne, nie usprawiedliwienie dla timestampu czasu odbioru. Online offset może stać się słabo obserwowalny w spokojnym ruchu, a następnie absorbować inne błędy.

Global shutter i rolling shutter#

Global shutter przypisuje całej klatce prawie ten sam okres ekspozycji. Rolling shutter eksponuje lub odczytuje kolejne wiersze w innych chwilach:

t_row(y) = t_frame + y · t_line

Podczas obrotu każdy wiersz obserwuje inną orientację. Prosta krawędź może się wygiąć, a standardowy model jednej pozy kamery na klatkę staje się błędny. Skutek rośnie z czasem odczytu, prędkością kątową, ogniskową i położeniem wiersza.

Możliwe strategie:

global-shutter camera;
ograniczenie ekspozycji i readout time;
model rolling shutter wykorzystujący gyro/ciągłą trajektorię;
odrzucenie klatki przy przekroczeniu body rate lub blur;
zwiększenie noise model — tylko dla małych, losowych reszt.

VINS-Mono zawiera obsługę rolling shutter, lecz integrator nadal musi znać właściwy czas wiersza i konwencję. Sam checkbox bez identyfikacji sensora może pogorszyć estymację.

Frontend wizyjny#

Frontend zamienia obraz na zestaw korespondencji. Jego typowa kolejność to:

image → optional rectify/equalize → detect → describe/track
      → geometric outlier rejection → tracks with timestamps

Nie każda korekcja obrazu jest neutralna geometrycznie. Elektroniczna stabilizacja, automatyczne odszumianie temporalne, sharpening i zmienna korekcja dystorsji mogą przesuwać cechy lub łączyć kilka chwil w jedną ramkę. Dla nawigacji korzystniejszy jest przewidywalny surowy tor niż obraz „ładniejszy” dla operatora.

Frontend powinien publikować metryki jeszcze przed backendem:

liczbę wykrytych i śledzonych cech;
rozkład po siatce obrazu;
długości tracków;
medianę disparity/parallax;
procent inlierów po modelu geometrycznym;
histogram residuali;
ekspozycję, gain i wskaźnik blur;
czas każdej fazy.

Duża liczba cech skupionych w jednym narożniku nie daje takiej samej geometrii jak mniejsza liczba równomiernie rozłożona. Grid-based selection ogranicza dominację obszaru o bardzo bogatej teksturze.

Cechy i śledzenie#

Dwie podstawowe rodziny frontendu to:

tracking optyczny, np. pyramidal KLT, który prowadzi punkt między sąsiednimi klatkami;
detector + descriptor, np. ORB, który wykrywa punkty i dopasowuje deskryptory.

KLT jest wydajny i daje subpikselowe śledzenie przy małych zmianach wyglądu. Wymaga odpowiedniego displacement, tekstury i piramidy. Deskryptory lepiej wspierają ponowne dopasowanie po większej zmianie widoku i place recognition, ale kosztują pamięć oraz CPU.

Detektor narożników powinien odrzucać punkty słabo uwarunkowane. Jedna długa krawędź nie określa ruchu wzdłuż niej — aperture problem. Dobry punkt ma gradient w co najmniej dwóch kierunkach w lokalnym oknie.

Praktyczne reguły:

maskować śmigła, podwozie i stałe elementy własnej platformy;
nie śledzić timestampów OSD ani nakładek obrazu;
ograniczać minimalny dystans między cechami;
odnawiać cechy, zanim liczba tracków spadnie krytycznie;
zachować wieloklatkowy identyfikator tracku;
nie używać wyłącznie liczby cech jako quality.

Śmigło widoczne w kadrze tworzy szybkie, okresowe cechy związane z UAV. Backend może interpretować je jako bliski obiekt sceny lub ogromny outlier load. Maska musi uwzględniać ugięcie ramy i zmianę położenia łopat.

Odrzucanie błędnych korespondencji#

Descriptor match nie jest pomiarem geometrycznym dopóki nie przejdzie kontroli. Typowe testy obejmują:

ratio test odległości descriptorów;
wzajemność dopasowania;
RANSAC/USAC dla macierzy fundamentalnej, essential lub homografii;
reprojection gate zależny od modelu szumu;
spójność z predykcją IMU;
długość i ciągłość tracku;
zgodność stereo epipolar.

RANSAC zakłada, że istnieje dominujący model i wystarczający udział inlierów. W scenie z dużym poruszającym się obiektem dominujący model może opisywać ten obiekt, nie świat statyczny. Kamera skierowana na jadący pojazd, wodę, tłum albo kołyszącą się roślinność może raportować pewną, ale błędną egomotion.

Homografia dobrze opisuje płaską scenę lub czysty obrót. Essential matrix opisuje geometrię dwóch skalibrowanych kamer dla ogólnej sceny, ale przy małej paralaksie translacja jest słabo uwarunkowana. System powinien rozpoznawać model degeneracyjny, a nie zawsze wymuszać rozwiązanie 6DoF.

Geometria dwóch widoków#

Dla skalibrowanych współrzędnych punktów x1, x2 zachodzi constraint epipolarny:

x2^T E x1 = 0
E = [t]_x R

Z E można odzyskać względny obrót R oraz kierunek translacji t, lecz nie jej skalę. Z kilku możliwych dekompozycji wybiera się tę, w której triangulowane punkty leżą przed obiema kamerami.

Triangulacja jest dobrze uwarunkowana, gdy promienie obserwacji mają dostateczny kąt. Dla dalekich punktów lub małego przesunięcia baseline promienie są prawie równoległe, a mały błąd piksela daje duży błąd głębokości. Backend powinien opóźnić inicjalizację punktu, użyć inverse-depth albo zachować track bez natychmiastowego tworzenia niestabilnej map point.

Gdy istnieją punkty 3D z wcześniejszej mapy i obserwacje 2D, pose wyznacza problem PnP z robust estimation. OpenCV dokumentuje rodziny solvePnP, ale wybór solvera, konfiguracji punktów oraz refine jest częścią algorytmu. Punkty prawie współpłaszczyznowe lub skupione w małym obszarze zwiększają niepewność.

Monocular, stereo i RGB-D#

Monocular#

Najmniejsza masa i bandwidth, największe wymagania inicjalizacji. Skala pochodzi z IMU lub innego odniesienia. Nie ma natychmiastowej głębokości dla nowej cechy. Działa na większych odległościach niż część aktywnych depth cameras, o ile scena ma teksturę i paralaksę.

Stereo#

Znana baza b daje dla obrazu rectified:

Z = f · b / disparity

Błąd głębokości rośnie w przybliżeniu z Z², więc dalekie punkty nadal są słabe. Mała baza jest lekka, ale zmniejsza dokładność głębokości. Duża baza poprawia geometrię kosztem ramy, synchronizacji i wspólnego pola widzenia. Obie kamery muszą mieć wspólny czas albo znany offset.

RGB-D / active stereo / ToF#

Depth upraszcza metryczną inicjalizację w zakresie sensora, lecz aktywne systemy mają limity słońca, refleksów, minimalnej odległości i wielodrogowości. Nie należy zakładać, że każdy piksel RGB ma ważną, zsynchronizowaną głębokość.

Wybór nie sprowadza się do „stereo jest dokładniejsze”. Kryteria obejmują odległość sceny, światło, baseline, moc obliczeniową, masę, pole widzenia, motion blur, dostęp do triggera i wymagany zakres prędkości.

Inicjalizacja#

Estimator przed publikacją pełnego stanu musi ustalić co najmniej:

roll/pitch względem grawitacji;
początkową prędkość;
bias gyro i accelerometer;
skalę monocularną;
lokalny origin i yaw;
zgodność camera–IMU czasu i ekstrynsyki.

Inicjalizacja statyczna może dobrze oszacować grawitację i gyro bias, lecz nie daje paralaksy ani pobudzenia translacyjnego. Inicjalizacja dynamiczna wykorzystuje serię pozy i preintegrację IMU, ale wymaga ruchu o odpowiedniej jakości. OpenVINS ma procedury statyczne i dynamiczne; VINS-Mono opisuje automatyczną inicjalizację oraz recovery.

Stan initialized=true powinien znaczyć więcej niż „solver zwrócił wynik”. Warunki obejmują:

gravity norm plausible
scale positive and within prior bounds
bias within sensor qualification
enough parallax and feature distribution
condition number / covariance acceptable
camera–IMU time residual stable
reprojection and inertial residuals pass gates

Po uruchomieniu na drgającej platformie accelerometer nie mierzy samej grawitacji. Jeśli algorytm uzna przyspieszenie za tilt, błędna orientacja inicjalna przeniesie się na prędkość. Sekwencja startowa powinna mieć zdefiniowane warunki spoczynku albo kontrolowany ruch inicjalizacyjny.

Fuzja z IMU#

IMU dostarcza szybkie przyrosty kąta i prędkości, kamera wolniejsze constrainty geometryczne. Stan VIO często zawiera:

x = [R, p, v, b_g, b_a, camera/IMU calibration, feature-related states]

Między klatkami stan jest propagowany pomiarami:

omega = omega_m - b_g - n_g
a     = a_m     - b_a - n_a

Po obrocie siły właściwej do układu lokalnego i dodaniu grawitacji aktualizuje się v i p. Backend porównuje projekcję punktów z ich obserwacją. Rozbieżność koryguje trajectory, biasy i czasem kalibrację.

W optymalizacji często stosuje się preintegrację IMU: wiele próbek między dwiema klatkami jest składanych w constraint zależny od biasu. Nie jest to zwykła suma. Integracja na rozmaitości obrotów, covariance i Jacobiany względem biasu są częścią pomiaru.

Nieprawidłowa gęstość szumu lub random walk IMU daje backendowi zły podział zaufania między vision a inertial. OpenVINS zaleca pomiar charakterystyk IMU, m.in. przez analizę Allan variance, zamiast kopiowania liczb z innego egzemplarza. Jednostki w pliku konfiguracyjnym muszą odpowiadać definicji continuous/discrete noise użytej przez bibliotekę.

Filtr MSCKF i optymalizacja sliding-window#

W rodzinie MSCKF stan zawiera bieżący stan IMU oraz klony wcześniejszych pozy kamery. Track punktu obserwowany z wielu pozy tworzy constraint; stan samego punktu może zostać usunięty przez projekcję w nullspace. Pozwala to użyć informacji geometrycznej bez utrzymywania wszystkich feature states w filtrze. OpenVINS implementuje sliding-window MSCKF i szczegółowo dokumentuje propagację oraz update.

W backendzie optymalizacyjnym utrzymuje się okno keyframes, stanów IMU i landmarków. Minimalizowany koszt może zawierać:

J = Σ rho(||r_reprojection||²_W)
  + Σ ||r_IMU-preintegration||²_W
  + ||r_prior||²_W

rho jest robust loss ograniczającą wpływ outlierów. W wynika z inverse covariance. Po wyjściu starych stanów z okna ich informację zachowuje prior z marginalizacji.

Filtr i optymalizacja nie są prostym rankingiem „szybki kontra dokładny”. Różnią się reprezentacją niepewności, liniaryzacją, sposobem obsługi cech i kosztami. Dla integratora ważniejsze są mierzalne właściwości: latency, reset behavior, covariance consistency, odporność na dropout oraz deterministyczny worst-case time.

Keyframes, marginalizacja i map points#

Nie każda klatka musi być keyframe. Nowy keyframe dodaje się zwykle po odpowiednim przesunięciu, rotacji, spadku wspólnych tracków albo czasie. Zbyt częste keyframes zwiększają koszt backendu; zbyt rzadkie pogarszają geometrię i utrudniają śledzenie.

Marginalizacja ogranicza rozmiar okna. Jej prior zależy od punktu liniaryzacji. Gwałtowne przeliniaryzowanie, zły ordering lub numerycznie słaba macierz mogą tworzyć nadmierną pewność. Monitoruje się condition, czas solve, liczbę iteracji oraz to, czy covariance nie maleje podczas utraty informacji.

Landmark może być przechowywany jako XYZ, inverse depth zakotwiczony w keyframe albo inna reprezentacja. Inverse depth lepiej opisuje bardzo dalekie punkty i pozwala opóźnić pełną triangulację. Punkt powinien zostać usunięty po powtarzalnych dużych residualach, ale masowe usunięcie mapy podczas chwilowego blur może utrudnić recovery.

VO a loop closure#

Odometria ma zapewniać lokalną ciągłość; loop closure poprawia globalną zgodność. W architekturze zgodnej z ideą REP 105 warto utrzymywać dwa strumienie:

odom → body     continuous, locally smooth, allowed to drift
map  → odom     global correction, allowed to jump

Regulator korzysta z ciągłego odom→body. Planner i prezentacja mapy mogą korzystać z map→body. Po wykryciu pętli aktualizuje się map→odom, nie symuluje nagłego fizycznego skoku pojazdu.

Jeżeli biblioteka publikuje tylko jedną trajektorię po global pose-graph optimization, adapter powinien stworzyć warstwę ciągłości albo nie używać tego strumienia w bezpośrednim feedbacku. Reset counter musi wzrosnąć przy każdej zmianie niebędącej rzeczywistym ruchem.

Loop closure może również dać fałszywy positive w powtarzalnym środowisku: identyczne korytarze, rzędy regałów albo okna. Geometric verification i możliwość odrzucenia korekty są równie ważne jak szybkość rozpoznania miejsca.

Układy odom, map, body i camera#

W jednym systemie występują co najmniej:

camera: osie wynikające z modelu obrazu;
imu: osie fizycznego sensora;
body: umowny układ platformy;
odom: lokalny układ ciągłej odometrii;
map: układ globalnie korygowany;
NED/FRD autopilota;
ENU/FLU często spotykane w ROS.

Transformacja pozycji i orientacji musi obejmować zarówno parent frame, jak i child frame. Zamiana z na minus nie wystarcza do poprawnej konwersji kwaternionu. Należy składać jawne macierze obrotu lub kwaterniony zgodnie z jedną konwencją pasywną/aktywną.

Test stołowy dla adaptera:

Ruch fizyczny	Oczekiwany wynik PX4 FRD/local FRD
przód	`x` rośnie
prawo	`y` rośnie
w dół	`z` rośnie
roll w prawo	znak zgodny z right-hand rule wokół `x`
yaw zgodny z obrotem wokół `z` down	sprawdzony kwaternion, nie tylko Euler

PX4 dokumentuje wymagane ramy dla external vision i zaleca sprawdzenie transformacji przed lotem. Błąd osi może wyglądać poprawnie przy jednym kierunku ruchu, a stać się dodatnim sprzężeniem po obrocie yaw.

Integracja z autopilotem#

Komputer pokładowy może podawać external vision przez MAVLink ODOMETRY. Komunikat zawiera m.in.:

time_usec pomiaru;
frame_id pozy;
child_frame_id prędkości;
pozycję i kwaternion;
prędkość liniową i kątową;
pose/velocity covariance;
reset_counter;
estimator_type;
quality.

Timestamp powinien reprezentować czas stanu po uwzględnieniu opóźnionych pomiarów. Jeśli backend publikuje pose dla środka ostatniej ekspozycji, nie wolno oznaczać jej czasem wysłania pakietu. Autopilot może kompensować delay tylko wtedy, gdy zna czas pomiaru i zegary są zsynchronizowane.

PX4 mapuje external vision do własnego topicu odometrii i opisuje konfigurację fuzji pozycji, prędkości i yaw. Dokumentacja wskazuje także, że ODOMETRY pozwala przekazać prędkość. Wybór pól do fuzji powinien wynikać z jakości estymatora. Jeśli VIO ma dobrą prędkość, ale yaw ma dryf lub resety, można nie używać external yaw jako jedynego heading source.

Adapter powinien mieć kontrakt:

if stale || tracking_lost || covariance_invalid:
    do not publish measurement as healthy
else:
    transform frames
    apply timestamp and lever arm convention
    publish covariance + quality + reset_counter

„Nie publikuj jako healthy” nie zawsze oznacza zatrzymanie strumienia. Zależnie od autopilota lepiej przesłać status failed/dużą covariance lub przerwać pomiary, aby timeout zadziałał jednoznacznie. Zachowanie musi być przetestowane z konkretną wersją firmware.

Kowariancja, quality i health#

Mały reprojection error nie jest pełną miarą błędu pozycji. W scenie o małej paralaksie solver może dobrze odtwarzać piksele przy dużej niepewności translacji. Quality powinno łączyć geometrię i terminowość.

Przydatne składowe health:

liczba i rozkład inlier tracks;
median parallax i triangulation angle;
reprojection NIS/residual distribution;
inertial residual i bias plausibility;
covariance pozy, prędkości, attitude;
latency, jitter i frame age;
liczba kolejnych odrzuconych klatek;
solver convergence i condition;
czas od ostatniego resetu/relocalization;
disagreement z GNSS, flow lub innym estimator instance.

MAVLink definiuje quality=-1 jako failure, 0 jako unknown/unset, a 1…100 jako rosnącą jakość. Mapowanie wewnętrznych metryk na procent musi być opisane i wersjonowane. Stałe quality=100 usuwa informację potrzebną do zarządzania ryzykiem.

Kowariancja powinna być skończona, symetryczna, dodatnio półokreślona i uporządkowana zgodnie ze specyfikacją. Wstawienie zer, bo biblioteka nie udostępnia covariance, oznacza „idealny pomiar”, nie „brak danych”. Jeśli covariance jest nieznana, należy użyć reprezentacji przewidzianej przez interfejs albo konserwatywnego modelu zweryfikowanego empirycznie.

Consistency można ocenić przez NEES/NIS przy ground truth oraz coverage: czy błąd rzeczywiście mieści się w deklarowanych przedziałach z oczekiwaną częstością. Estimator dokładny, ale nadmiernie pewny, może być bardziej niebezpieczny dla fuzji niż nieco mniej dokładny i uczciwy.

Resety i ciągłość sterowania#

Reset może wystąpić po:

restarcie procesu;
utracie i odzyskaniu trackingu;
zmianie mapy;
loop closure;
ponownej inicjalizacji skali;
przełączeniu camera/IMU pipeline;
overflow lub corruption stanu.

Należy rozróżnić trzy klasy:

ciągła korekta — zwykły update estymatora;
jawny skok ramy — state valid, ale origin się zmienił;
utrata ważności — nie ma wiarygodnej pozy.

reset_counter w ODOMETRY służy do sygnalizowania resetów/skoków. Sam licznik nie kompensuje skoku; odbiorca musi posiadać politykę. Możliwe strategie to aktualizacja map→odom, przesunięcie setpointów o delta reset, wstrzymanie trybu pozycyjnego albo przełączenie źródła.

Nie wolno interpolować przez okres, w którym system nie znał pozy. Wygładzenie wizualne ukrywa utratę informacji. Jeżeli po relokalizacji odometry frame jest odbudowany, adapter powinien jawnie opublikować reset i nową transformację.

Degeneracje#

Mała tekstura#

Biała ściana, czysta posadzka, mgła lub niebo nie dają stabilnych punktów. Liczba cech i spatial coverage spadają. System powinien zwiększyć covariance i wyjść z trybu, zanim tracking całkowicie zniknie.

Powtarzalna tekstura#

Siatka, elewacja okienna i regały tworzą wiele podobnych descriptorów. Local tracking może pomylić sąsiadujące elementy, a place recognition — korytarze. Potrzebna jest weryfikacja geometryczna i kontrola skali korekty.

Czysty obrót#

Jest dużo ruchu obrazu, lecz brak baseline translacyjnego do triangulacji. Obrót jest obserwowalny, głębokość i kierunek translacji nie. Nie należy interpretować dużego optical flow jako dobrej paralaksy.

Ruch wzdłuż osi optycznej#

Punkty rozsuwają się promieniście, ale głębokość dalekiej sceny może być słaba. Przy wąskim FOV i małej teksturze w centrum conditioning spada.

Scena płaska#

Homografia może wyjaśnić większość obserwacji. Pełny model 3D ma słabsze constrainty, zwłaszcza przy małej paralaksie i niepewnej skali.

Dynamiczne obiekty#

Tłum, pojazd wypełniający kadr, fale, dym, obracający się wentylator i cień mogą dominować. Semantic mask pomaga, lecz sama sieć segmentacji ma latency i błędy. Kontrola z IMU oraz wielomodelowy RANSAC są nadal potrzebne.

Motion blur i ekspozycja#

Długi exposure niszczy gradienty i przesuwa położenie cech. Auto-exposure może zwiększyć czas właśnie po wejściu do ciemnego obszaru, gdy nawigacja jest najbardziej potrzebna. Limit ekspozycji powinien być powiązany z przewidywaną prędkością kątową.

Zmiana oświetlenia#

Wyjście z hali w słońce, migotanie LED, refleksy i cienie łopat naruszają constancy. HDR z kilkoma ekspozycjami może mieszać czasy akwizycji. Pipeline musi znać tryb kamery.

Wibracje i elastyczność#

Wibracja daje blur i zmienną ekstrynsykę. Rolling shutter zamienia drgania na falowanie obrazu. Analiza powinna korelować residual vision z RPM, gyro spectrum i exposure.

Niewystarczające pobudzenie IMU#

Stała prędkość, ruch w jednej osi lub długi zawis mogą słabo obserwować część biasów i calibration states. Online calibration nie powinna wtedy swobodnie dryfować.

Wymagania sprzętowe#

Dobór kamery obejmuje:

Parametr	Znaczenie nawigacyjne
shutter/readout	deformacja podczas ruchu
frame rate	displacement, latency i koszt CPU
exposure control	blur kontra szum
FOV	pokrycie sceny kontra rozdzielczość kątowa
resolution	liczba cech kontra bandwidth
trigger/timestamp	zgodność z IMU
lens/focus lock	stabilność intrinsics
interface	latency, jitter i utrata ramek
temperature	stabilność sensora i optyki
mass/power	integracja UAV

Komputer pokładowy musi być dobrany według najgorszego przypadku, nie średniego FPS na biurku. Obciążenie rośnie przy dużej liczbie cech, relokalizacji i loop closure. Telemetria CPU powinna obejmować per-thread deadline misses, temperaturę, throttling, pamięć, kolejki i drop frames.

GPU/NPU pomaga w feature extraction lub semantic masking, lecz kopiowanie obrazu między domenami pamięci może dominować latency. Zero-copy i sprzętowy timestamp są często ważniejsze niż szczytowe TOPS.

Zasilanie kamery i companion computer powinno przetrwać transienty ESC i restartować się w kontrolowany sposób. Ponowne uruchomienie VIO w locie jest resetem źródła pozycji; autopilot musi przejść do przygotowanego trybu, a nie czekać bezterminowo na nowy origin.

Logowanie i metryki#

Bez danych wejściowych nie da się odtworzyć awarii. Log powinien zawierać lub umożliwiać zsynchronizowane odtworzenie:

obrazów albo przynajmniej wybranych klatek diagnostycznych;
surowych IMU z timestampami;
intrinsics/extrinsics i ich hash;
feature tracks, inlier mask i residuale;
pose, velocity, bias, covariance;
status inicjalizacji i tracking state;
reset counter i transformacje frame;
latency poszczególnych etapów;
komunikaty wysłane do autopilota;
stan EKF autopilota i innovations external vision;
wersje kodu, konfiguracji i modelu kamery.

Podstawowe metryki trajektorii:

ATE — absolute trajectory error po jawnie opisanym alignment;
RPE — relative pose error na wybranym horyzoncie;
drift jako procent drogi lub błąd na minutę;
orientation error osobno od translation;
latency median/p95/p99/max;
dropout rate i longest outage;
recovery time i reset magnitude;
consistency covariance;
CPU/power/temperature.

Alignment może ukryć błędy. Dopasowanie pełnego Sim(3) usuwa skalę; jest użyteczne do analizy monocular VO, ale nie dowodzi poprawnej skali metrycznej. Dla VIO wymagającego metrów raport powinien używać SE(3) albo ograniczonego alignment zgodnego z wymaganiem.

Walidacja offline#

EuRoC MAV Dataset zawiera sekwencje visual-inertial z ground truth i jest wspierany przez wiele projektów, m.in. VINS-Mono i ORB-SLAM3. Dataset publiczny sprawdza poprawność instalacji i daje punkt porównania, lecz nie kwalifikuje własnej kamery, drgań, obiektywu i komputera.

Program badań powinien mieć warstwy:

dataset referencyjny — czy build reprodukuje oczekiwany rząd wyników;
synthetic/simulator — kontrolowany offset czasu, noise, rolling shutter, dropout;
własny bag z ground truth — dokładny sensor i mocowanie;
replay deterministyczny — identyczne wejście daje zgodne wyniki lub wyjaśniony nondeterminism;
fault injection — utrata klatek, skok zegara, bias, błędna ekstrynsyka;
regression suite — metryki z progami dla każdej wersji.

Sekwencje powinny obejmować: statykę, ruch sześciu stopni swobody, czysty obrót, szybki yaw, ciemność, wyjście indoor/outdoor, gładką ścianę, powtarzalny korytarz, dynamiczne osoby, długą pętlę, powrót do znanego miejsca i termiczny soak.

Test offsetu czasu może sztucznie przesuwać timestampy obrazu w zakresie dodatnim i ujemnym, a następnie wykreślać residual i ATE. Minimum krzywej pomaga wykryć stały offset; zmieniające się minimum w czasie sugeruje drift zegara lub zmienne latency.

SITL, HIL i próby w locie#

SITL powinien sprawdzić interfejs, stany i failsafe, nawet jeśli renderer obrazu nie odtwarza całej fotometrii. Wstrzykuje się:

stały delay i jitter;
stop publishing;
quality=-1;
rosnącą covariance;
reset counter i skok origin;
zamianę osi;
utratę yaw przy zachowaniu position;
ponowne uruchomienie źródła.

HIL/replay sprawdza rzeczywiste timestampy, obciążenie komputera i MAVLink. Przed swobodnym lotem wykonuje się test ruchu ręcznego z wyłączonymi aktuatorami, porównuje znaki osi, attitude i velocity oraz obserwuje innovations autopilota.

Kolejność prób:

statyczny bench i ruch ręczny;
platforma na prowadnicy/turntable z ground truth;
UAV unieruchomiony, napęd pracuje — wpływ wibracji i EMI;
lot z GNSS jako niezależnym odniesieniem, VIO tylko logowane;
VIO fused przy gotowym przejęciu manualnym;
kontrolowane krótkie odcinki bez GNSS;
stopniowe rozszerzanie światła, prędkości i scen;
test recovery i każdego failsafe.

Nie rozpoczyna się od lotu indoor bez alternatywnego pozycjonowania. Stabilny zawis przez minutę nie testuje loop closure, długiego dryfu, dynamicznego blur ani resetu procesu.

Kryteria odbioru#

Podsystem VIO jest gotowy do konkretnej obwiedni, gdy:

intrinsics odpowiadają dokładnemu trybowi obrazu i mają raport residuali;
ekstrynsyka camera–IMU jest zmierzona, wersjonowana i mechanicznie stabilna;
offset oraz drift czasu są zmierzone, a timestamp opisuje akwizycję;
układy camera/IMU/body/odom/map/autopilot przechodzą test znaków;
initialization ma jawne warunki obserwowalności i timeout;
scale error, RPE, ATE i orientation error mieszczą się w budżecie;
latency p99/max spełnia deadline, także przy relokalizacji;
covariance przechodzi test consistency i nie jest wypełniona zerami;
quality spada przed krytyczną utratą trackingu w znanych degeneracjach;
reset, loop closure i restart nie powodują skoku regulatora;
autopilot poprawnie reaguje na stale data, dropout i failure;
obwiednia obejmuje mierzone limity blur, light, texture, rate i temperature;
log umożliwia replay wejść i korelację z innovations autopilota;
regression suite blokuje wersję pogarszającą metryki;
procedura degradacji i lądowania została sprawdzona w SITL/HIL oraz kontrolowanym locie.

Każde kryterium powinno mieć liczbę, metodę pomiaru i warunki. „VIO działa indoor” nie jest wymaganiem odbiorowym.

Typowe błędy#

Traktowanie visual SLAM i ciągłej odometrii jako jednego strumienia.
Podanie pozy po loop closure bez reset counter do regulatora.
Timestamp czasu odbioru obrazu zamiast środka ekspozycji.
Kalibracja intrinsics w innej rozdzielczości lub po innym cropie.
Odwrócenie T_IC przez niejasną konwencję.
Założenie zerowego lever arm kamery względem IMU.
Online calibration użyta do maskowania elastycznego mocowania.
Stałe quality=100 i zerowa covariance.
Ocena jakości tylko liczbą feature points.
Brak spatial coverage i parallax metric.
Uznanie czystej rotacji za dobrą inicjalizację translacji.
Brak maski własnych śmigieł i podwozia.
Włączenie automatycznej stabilizacji obrazu w sterowniku kamery.
Ignorowanie rolling shutter przy szybkim yaw.
Brak limitu ekspozycji w ciemności.
Test tylko na EuRoC, bez własnego sensora i drgań.
Dopasowanie Sim(3) w raporcie VIO i pominięcie błędu skali.
Brak logu obrazów/IMU uniemożliwiający replay.
Fuzja external yaw bez oceny jego dryfu i resetów.
Restart companion computera bez przejścia autopilota do failsafe.

Powiązane tematy#

Optical flow w UAV — prędkość obrazu, wysokość i gyro compensation.
IMU w dronie — bias, szum i przygotowanie próbek inercyjnych.
Filtr Kalmana i EKF w UAV — covariance, innovation i gating.
Kwaterniony w kontrolerze lotu — składanie obrotów i konwencje.
Układy współrzędnych UAV — NED, ENU, FRD, FLU i transformacje.
INS w UAV — mechanizacja IMU oraz wzrost błędu.
Dead reckoning UAV — przejścia po utracie GNSS i time-to-limit.
SLAM dla UAV — mapa, loop closure i relokalizacja.
PX4 — architektura i workflow — integracja external vision z autopilotem.
MAVLink — architektura protokołu — transport komunikatu ODOMETRY.

Przypisy#

Bibliografia jest generowana z centralnego rejestru. Architektura filtra MSCKF, procedury inicjalizacji i wymagania kalibracji camera–IMU opierają się na dokumentacji OpenVINS. Sliding-window VIO, preintegracja, recovery, temporal calibration i rolling shutter są opisane w publikacji oraz repozytorium VINS-Mono. Rozdzielenie odometrii od globalnych korekt porównano z architekturą ORB-SLAM3 i semantyką układów REP 105. Format, ramy, kowariancja, jakość i reset counter pochodzą ze specyfikacji MAVLink ODOMETRY oraz dokumentacji external position estimation PX4. Model kamery i narzędzia kalibracji zweryfikowano w dokumentacji OpenCV. EuRoC jest zbiorem referencyjnym do testów, nie substytutem kwalifikacji konkretnej kamery, IMU, uchwytu i komputera pokładowego.

Źródła z centralnego rejestru

OpenVINS: documentation and estimator overview [dokumentacja projektu badawczego]
OpenVINS: Visual-Inertial Sensor Calibration [dokumentacja projektu badawczego]
HKUST Aerial Robotics Group: VINS-Mono [repozytorium open source]
Qin, Li, Shen: VINS-Mono — A Robust and Versatile Monocular Visual-Inertial State Estimator [publikacja naukowa]
UZ-SLAMLab: ORB-SLAM3 [repozytorium open source]
Campos et al.: ORB-SLAM3 — An Accurate Open-Source Library for Visual, Visual-Inertial, and Multimap SLAM [publikacja naukowa]
PX4: Using Vision or Motion Capture Systems for Position Estimation [dokumentacja projektu]
MAVLink Common Message Set: ODOMETRY [specyfikacja protokołu]
OpenCV: Camera calibration and 3D reconstruction [dokumentacja projektu]
ETH Zurich ASL: EuRoC Micro Aerial Vehicle Dataset [zbiór danych badawczych]
ROS REP 105: Coordinate Frames for Mobile Platforms [standard projektu open source]
PX4: Using PX4 Navigation Filter EKF2 [dokumentacja projektu]
ROS REP 145: Conventions for IMU Sensor Drivers [standard projektu open source]