SLAM dla UAV — Drony

SLAM (simultaneous localization and mapping) estymuje ruch platformy, jednocześnie budując lub aktualizując reprezentację otoczenia. W UAV nie jest to pojedynczy algorytm „zwracający pozycję”, lecz zespół procesów o różnych wymaganiach czasowych: szybka odometria dla sterowania, wolniejsza optymalizacja mapy, rozpoznawanie miejsc, integracja wolnej i zajętej przestrzeni oraz mapa odległościowa dla planera. Pomylenie tych produktów prowadzi do skoków pozycji, planowania przez nieobserwowany obszar albo przeciążenia komputera w chwili loop closure.

Najważniejsza zasada architektoniczna brzmi: regulator potrzebuje ciągłego układu lokalnego, a mapa może zostać globalnie skorygowana. Jeżeli zamknięcie pętli przesuwa wcześniejsze keyframes, nie oznacza to, że UAV fizycznie przeskoczył. Korekta powinna zmienić transformację map→odom, natomiast odom→body pozostaje ciągła i może dryfować.

Spis treści#

Co rozwiązuje SLAM
Produkty i częstotliwości
Frontend i backend
Visual SLAM
Lidar SLAM
Lidar inertial SLAM
Scan matching
Deskew chmury punktów
Keyframes i submapy
Pose graph i factor graph
Loop closure
Relokalizacja
Układy map, odom i body
Rodzaje map
Occupancy i OctoMap
TSDF
ESDF dla planera
Unknown, free i occupied
Integracja mapy z planowaniem
Dynamika sceny
Czas, kalibracja i ekstrynsyka
Obserwowalność i degeneracje
Zarządzanie pamięcią
Architektura czasu rzeczywistego
Health i integralność
Logowanie i metryki
Testy i regresja
Kryteria odbioru
Typowe błędy
Powiązane tematy
Przypisy

Co rozwiązuje SLAM#

Problem ma dwa wzajemnie zależne elementy. Aby poprawnie wstawić obserwację do mapy, trzeba znać pozę sensora. Aby poprawić pozę, można dopasować obserwację do już zbudowanej mapy. Stan może obejmować:

x = [poses, velocity, IMU biases, landmarks/submaps, calibration states]

Pomiary tworzą constrainty między stanami. Kamera wiąże pozę i landmark przez reprojection. Lidar wiąże skan z powierzchniami lub submapą. IMU wiąże kolejne stany przez preintegrację. GNSS może dodać luźne odniesienie globalne, a loop closure — constraint między odległymi w czasie miejscami.

SLAM nie gwarantuje:

globalnej pozycji geograficznej;
braku dryfu przed zamknięciem pętli;
poprawnego rozpoznania każdego miejsca;
kompletnej mapy przestrzeni zasłoniętej;
aktualności w dynamicznym środowisku;
bezpiecznej trajektorii — to zadanie planera i warstwy safety.

Wymaganie powinno określać sensor, środowisko, rozmiar mapy, dokładność lokalną i globalną, deadline, dozwolone skoki, zachowanie po utracie trackingu oraz semantykę unknown space.

Produkty i częstotliwości#

Jeden system może publikować kilka różnych produktów:

Produkt	Typowa rola	Wymagana własność
local odometry	regulator i lokalny planner	ciągłość, małe opóźnienie
local submap	obstacle avoidance	świeżość i bounded memory
global pose graph	korekcja dryfu	spójność globalna, może skakać
occupancy map	free/occupied/unknown	probabilistyczna aktualizacja
TSDF/mesh	rekonstrukcja powierzchni	gładka geometria
ESDF	optymalizacja trajektorii	odległość do najbliższej przeszkody
place database	relokalizacja	odporność na false positive

Szybka odometria może działać z częstotliwością IMU lub klatek, local map kilkanaście razy na sekundę, a global optimization sporadycznie. Wykonanie wszystkiego w jednym wątku oznacza, że kosztowny loop closure może zatrzymać publikację pozy. Rozdzielenie procesów i kolejek jest elementem bezpieczeństwa czasowego.

Frontend i backend#

Frontend przekształca dane sensora w constrainty:

camera → features/tracks/descriptors → visual constraints
lidar  → filtered/deskeved cloud     → scan constraints
IMU    → preintegration              → inertial constraints

Backend utrzymuje stan, optymalizuje aktywne okno i/lub graf, marginalizuje stare zmienne oraz publikuje transformacje. Frontend odpowiada za outliery pomiarowe; backend za spójność wielu constraintów. Nie ma ostrej granicy — np. scan matching może być częścią lokalnego back-endu.

Kontrakt frontendu powinien zawierać:

timestamp akwizycji;
frame sensora;
covariance lub model szumu;
metrykę jakości i liczbę inlierów;
identyfikator kalibracji;
informację o odrzuceniu lub saturacji;
czas przetwarzania.

Backend nie powinien „naprawiać” błędnych osi albo czasu przez dowolne zwiększanie tolerancji. Residual może zmaleć, gdy optimizer zdeformuje mapę lub biasy, ale stan przestaje mieć fizyczne znaczenie.

Visual SLAM#

Visual SLAM wykorzystuje cechy punktowe, linie, direct photometric residuals albo reprezentacje uczone. Monocular camera ma nieobserwowalną skalę bez IMU lub innego odniesienia. Stereo i RGB-D dają metryczną głębokość w swojej obwiedni. Visual-inertial SLAM dodaje szybki tor inercyjny i obserwację skali.

ORB-SLAM3 jest przykładem systemu obsługującego monocular, stereo, RGB-D oraz tryby visual-inertial, z lokalnym mapowaniem, place recognition i wieloma mapami. Jest dobrym punktem odniesienia architektonicznego, ale implementacja lotna wymaga osobnego nadzoru deadline, ramek, resetów i interfejsu do autopilota.

Visual SLAM jest wrażliwy na:

motion blur i rolling shutter;
małą lub powtarzalną teksturę;
zmianę oświetlenia i ekspozycji;
dynamiczne obiekty;
zasłonięcie kamery;
błędne intrinsics, ekstrynsykę i offset czasu;
aliasing miejsca w podobnych korytarzach.

Zaletą jest bogata informacja kątowa i możliwość place recognition z wyglądu. Wadą — zależność od fotometrii. Szczegóły toru VIO opisuje Visual odometry i VIO w UAV.

Lidar SLAM#

Lidar zwraca odległości w wielu kierunkach. Po przekształceniu do 3D daje chmurę punktów, którą można dopasować do poprzedniego skanu, lokalnej mapy lub geometrycznych cech. Nie zależy od światła widzialnego, ale ma własne problemy:

mało geometrii w długim tunelu lub nad płaskim gruntem;
szkło, woda, deszcz, kurz i multipath;
różny czas każdego punktu w skanie;
duża masa, moc i bandwidth;
sparse returns na cienkich przewodach;
ograniczony vertical FOV;
ruchome obiekty.

LOAM rozdzielił szybki tor odometrii od wolniejszego mapowania. To ważna idea systemowa: lokalny ruch nie musi czekać na pełne dopasowanie globalne. Współczesne systemy często dodają IMU, factor graph i loop closure.

Lidar nie „widzi przestrzeni” bez modelu promieni. Punkt końcowy może oznaczać powierzchnię, a odcinek przed nim — wolną przestrzeń, jeśli return jest wiarygodny. Brak returnu nie zawsze oznacza free: może wynikać z maksymalnego zasięgu, absorpcji albo kąta padania.

Lidar inertial SLAM#

Lidar-inertial odometry (LIO) używa IMU do propagacji, deskew i initial guess scan matching. LIO-SAM formułuje problem w factor graph, integruje IMU oraz lidar odometry, może dodawać GNSS i loop closure. Repozytorium projektu podkreśla wymagania dotyczące czasu punktów, numeru pierścienia, ekstrynsyki i wysokiej częstotliwości IMU.

Przepływ może wyglądać tak:

IMU propagation → pose prediction
raw scan + per-point time → deskew
deskewed scan → feature extraction / registration
registration residual → factor graph update
optimized lidar pose → IMU bias correction

Tight coupling wykorzystuje surowsze constrainty we wspólnym estimatorze; loose coupling łączy gotową odometrię lidarową z innymi źródłami. Tight coupling potencjalnie lepiej wykorzystuje korelacje, ale zwiększa złożoność konfiguracji i recovery.

Scan matching#

Scan matching szuka transformacji T minimalizującej niezgodność chmury z celem. Typowe residuale:

point-to-point:  ||T p_i - q_i||²
point-to-plane:  (n_i^T (T p_i - q_i))²

Point-to-plane szybciej wykorzystuje lokalną geometrię powierzchni, ale wymaga normalnych i poprawnych korespondencji. ICP jest rodziną iteracyjnych metod: dobór nearest neighbors, odrzucanie outlierów i aktualizacja transformacji. NDT reprezentuje obszary rozkładami i dopasowuje punkty do komórek.

Optimizer potrzebuje dobrego initial guess. Bez IMU lub poprzedniej odometrii może zbiec do lokalnego minimum. Fitness score nie jest sam w sobie gwarancją poprawności; symetryczny korytarz może dać niski koszt dla przesunięcia o jeden powtarzalny segment.

Ocena conditioning powinna analizować Hessian/information matrix. Jeżeli scena ma jedną płaszczyznę, ruch wzdłuż niej i obrót wokół normalnej mogą być słabo ograniczone. System powinien zwiększyć covariance w tych kierunkach, a nie zwracać izotropową pewność.

Deskew chmury punktów#

Skan mechanicznego lidaru nie jest zdjęciem w jednej chwili. Platforma porusza się podczas obrotu głowicy. Każdy punkt ma czas t_i, więc jest mierzony z innej pozy:

p_ref = T_ref^-1 · T(t_i) · p_i

Deskew przelicza punkty do wspólnego czasu, wykorzystując trajectory z IMU/odometrii. Bez tego ściana wygina się, a scan matching próbuje wyjaśnić deformację ruchem lub geometrią mapy.

LIO-SAM wymaga względnego czasu punktu w skanie i wykorzystuje IMU do deskew. Nadanie wszystkim punktom czasu początku pakietu usuwa informację i nie jest poprawnym timestampingiem. Dla solid-state lidar wzór skanowania jest inny; algorytm przygotowany dla pierścieni mechanicznego lidaru może wymagać adaptacji.

Keyframes i submapy#

Przechowywanie każdego skanu i pozy bez ograniczeń szybko przekracza pamięć. Keyframe reprezentuje wybrany stan o wystarczającej zmianie pozy, rotacji, czasu lub geometrii. Submapa integruje wiele lokalnych obserwacji w ograniczonym regionie.

Zalety submap:

scan matching względem lokalnego, gęstszego celu;
stały koszt lokalny niezależny od całej trasy;
węzły grafu reprezentują większe fragmenty;
global optimization przesuwa submapy zamiast każdego punktu;
łatwiejsze archiwizowanie i stronicowanie.

Cartographer tworzy lokalne submapy i dodaje constrainty scan-to-submap. Repozytorium Cartographer podaje obecnie, że projekt nie jest aktywnie rozwijany; przy wyborze zależności trzeba uwzględnić status utrzymania, nie tylko publikowane wyniki historyczne.

Próg keyframe nie powinien być tylko stałą odległością. W ubogiej geometrii potrzebna może być wcześniejsza aktualizacja, a w zawisie seria identycznych keyframes jedynie zwiększa koszt.

Pose graph i factor graph#

Pose graph ma węzły reprezentujące pozy/keyframes/submapy oraz krawędzie reprezentujące względne constrainty:

node_i ── odometry factor ── node_i+1
   │                              │
   └──── loop closure factor ─────┘

Koszt typowo minimalizuje błąd transformacji ważony information matrix. Factor graph może dodatkowo zawierać biasy IMU, landmarki, GNSS i priory. Robust loss lub switchable constraints ograniczają wpływ fałszywych loop closures, ale nie usuwają obowiązku ich weryfikacji.

Gauge freedom oznacza, że cały graf można przesunąć i obrócić bez zmiany względnych pomiarów. Jeden węzeł lub prior ustala gauge. Monocular graph może dodatkowo mieć niejednoznaczność skali. Globalne GNSS anchor powinno uwzględniać niepewność, a nie sztywno deformować mapę do pojedynczego błędnego fixa.

Optymalizacja grafu może zmienić wiele historycznych pozy. Mapa punktowa zintegrowana „na sztywno” w starej ramie nie poprawi się automatycznie. Architektura musi przechowywać możliwość reintegracji, deformacji submap lub transformacji bloków.

Loop closure#

Loop closure wykrywa, że bieżąca obserwacja odpowiada wcześniej odwiedzonemu miejscu. Potok ma dwie fazy:

szybkie wyszukanie kandydatów przez descriptor/place database;
geometryczna weryfikacja i estymacja transformacji.

Visual place recognition może używać bag-of-words; lidar — descriptorów skanu lub submap matching. Kandydat bez weryfikacji geometrycznej nie powinien wejść do grafu.

False positive może złożyć mapę w błędny sposób. False negative pozostawia dryf, ale zwykle jest mniej gwałtowny. Polityka powinna określać minimalną liczbę inlierów, geometryczne coverage, zgodność skali, maksymalny correction rate i test z innymi sensorami.

Artykuł Cartographer opisuje real-time loop closure przez scan-to-submap matching i branch-and-bound dla 2D lidaru. To dowód konkretnej architektury, nie uniwersalna gwarancja dla 3D UAV. W 3D koszt wyszukiwania i liczba symetrii są większe.

Po akceptacji pętli system powinien raportować:

identyfikatory połączonych węzłów;
transformację i covariance/information;
score i liczbę inlierów;
wielkość globalnej korekty;
zmianę map→odom;
czas optymalizacji;
czy mapa planera została zaktualizowana.

Relokalizacja#

Relokalizacja przywraca pozę względem istniejącej mapy po utracie trackingu lub starcie w znanym miejscu. Jest innym zdarzeniem niż zwykły update. Pozycja może przeskoczyć, a wynik może być wieloznaczny.

Bezpieczna procedura:

tracking lost
→ local controller enters degraded mode
→ candidates generated
→ geometric verification
→ pose covariance checked
→ map/odom transform reset announced
→ controlled re-entry to position mode

Nie należy płynnie interpolować przez okres nieznanej pozycji. Interpolacja wizualna tworzy pozorny pomiar. Jeśli UAV nadal leci, dead reckoning może zapewnić krótką ciągłość, ale jego covariance musi rosnąć.

Układy map, odom i body#

Semantyka zgodna z REP 105 rozdziela:

odom→body: ciągłe lokalne położenie, może dryfować;
map→odom: globalna korekta, może skakać;
map→body: złożenie obu, globalnie poprawiane.

Planner lokalny może pracować w odom, jeśli mapa przeszkód jest świeża i również w tym układzie. Planner globalny używa map. Setpoint aktywny podczas zmiany map→odom trzeba przeliczyć zgodnie z jego semantyką: czy oznacza stały punkt fizyczny, czy punkt względem lokalnego układu.

Przy wielu sesjach mapa może mieć własny origin, wersję i scale. Załadowanie mapy bez zgodnego calibration hash lub frame convention powinno zostać odrzucone. Nazwa map nie gwarantuje zgodności dwóch procesów.

Rodzaje map#

Jedna reprezentacja rzadko spełnia wszystkie zadania:

Reprezentacja	Zawartość	Typowe użycie
sparse landmarks	punkty/cechy	lokalizacja i loop closure
point cloud	próbki powierzchni	rejestracja i wizualizacja
surfel map	lokalne elementy powierzchni	gęste mapowanie
occupancy grid/octree	prawdopodobieństwo zajęcia	free-space planning
TSDF	signed distance blisko powierzchni	rekonstrukcja i mesh
ESDF	odległość euklidesowa do przeszkody	trajectory optimization
elevation map	wysokość/niepewność komórki	terrain i landing
semantic map	klasy/instancje	zachowanie zadaniowe

Point cloud nie koduje jednoznacznie free ani unknown. Mesh może być świetny dla operatora, ale niewygodny do szybkich zapytań o clearance. ESDF może być dobry dla gradientowego planera, ale jego jakość zależy od poprawnej occupancy/TSDF i aktualności.

Occupancy i OctoMap#

Occupancy grid przechowuje prawdopodobieństwo zajęcia. Aktualizacja jest często wykonywana w log-odds:

L_t = L_{t-1} + inverse_sensor_model(z_t) - L_0

Promień sensora aktualizuje komórki przed trafieniem jako free, a końcową jako occupied. Clamping zapobiega nieskończonej pewności. Parametry hit/miss zależą od sensora i środowiska.

OctoMap implementuje probabilistyczną mapę 3D w octree, rozróżnia occupied, free i unknown oraz pozwala aktualizować mapę. Octree oszczędza pamięć w jednorodnych regionach, ale rozdzielczość nadal wpływa na cienkie przeszkody i koszt.

Pułapki:

ray clearing przez szkło może usuwać prawdziwą przeszkodę;
maksymalny zasięg nie oznacza automatycznie free do nieskończoności;
kilka trafień w liście nie powinno wiecznie blokować trasy;
cienki przewód mniejszy od voxela może zniknąć;
inflacja musi uwzględniać promień UAV, błąd pozycji i latency.

TSDF#

TSDF (truncated signed distance field) przechowuje przy powierzchni podpisaną odległość, obciętą do zakresu truncation. Wiele obserwacji jest integrowanych z wagami, co wygładza szum i umożliwia rekonstrukcję mesh.

Voxblox buduje TSDF blokowo i oferuje różne integratory o kompromisie dokładność–czas. Parametry obejmują voxel size, voxels per side, truncation distance, max ray length, weight i time budget.

Mały voxel zwiększa szczegółowość, lecz koszt pamięci i ray integration rośnie sześciennie dla objętości. Duży voxel może usunąć cienkie przeszkody. Truncation distance zbyt mała przy szumie daje przerwy; zbyt duża rozmywa bliskie powierzchnie.

TSDF nie jest bezpośrednio globalnie spójna po dużym loop closure. Jeżeli historyczne chmury zintegrowano w błędnych pozach, trzeba transformować submapy, reintegrować albo stosować deformation. Sam pose graph nie „prostuje” voxel values.

ESDF dla planera#

ESDF przechowuje euklidesową odległość do najbliższej przeszkody oraz zwykle znak inside/outside. Planner może szybko sprawdzić clearance i gradient:

cost_clearance(p) = phi(distance_ESDF(p))

Voxblox został zaprojektowany do inkrementalnego generowania 3D ESDF dla on-board MAV planning. Pipeline może wyglądać:

depth/lidar + pose → TSDF integration → ESDF update → local planner

ESDF musi być świeża względem ruchu UAV. Wolna, dokładna aktualizacja mapy, która dociera po wykonaniu trajektorii, nie ma wartości safety. Planner powinien znać timestamp mapy, obszar aktualizacji i status completeness.

Gradient ESDF przy granicy unknown zależy od polityki. Traktowanie unknown jako free pozwala eksplorować, ale wymaga ograniczenia prędkości do zasięgu percepcji i drogi hamowania. Traktowanie unknown jako occupied jest konserwatywne, lecz może uniemożliwić wejście do nowego obszaru.

Unknown, free i occupied#

Trzy stany nie są zamienne:

occupied — obserwacja wspiera obecność przeszkody;
free — promień sensora wiarygodnie przeszedł przez przestrzeń;
unknown — brak wystarczającej obserwacji.

Planowanie przez unknown wymaga warunku widoczności i zatrzymania. Dla prędkości v, maksymalnego opóźnienia percepcji t_l, maksymalnego hamowania a i marginesu m minimalny zasięg w przybliżeniu:

d_required ≥ v t_l + v²/(2a) + m

W 3D trzeba uwzględnić kierunek thrust, tilt i niemożność natychmiastowego hamowania w dowolnej osi. Błąd pozycji i mapy zwiększa m.

Unknown za przeszkodą nie może zostać wyczyszczone tylko dlatego, że sąsiedni voxel jest free. Sensor model powinien odpowiadać fizyce promienia.

Integracja mapy z planowaniem#

Planner potrzebuje nie tylko mapy, ale kontraktu:

frame_id
timestamp / valid_until
resolution
known bounds
unknown policy
inflation / vehicle geometry
map revision and reset counter
quality / localization status

Po loop closure globalna mapa i aktywna trajektoria mogą znaleźć się w innych ramach. Warstwa synchronizująca powinna albo przeliczyć trajektorię, albo zatrzymać i zaplanować ponownie. Wykonawca nie może użyć waypointu z poprzedniej rewizji mapy bez jawnej polityki.

Local collision map powinna mieć wyższy priorytet czasowy niż globalny mesh. Jeżeli global optimizer jest przeciążony, lokalna detekcja przeszkód i attitude control muszą nadal działać.

Footprint UAV nie jest stałą kulą w każdej misji. Zmienia się z podwoziem i payloadem; dodatkowy margines obejmuje tracking error, downwash i niedokładność sensora. Inflacja wyłącznie o promień kadłuba jest niewystarczająca.

Dynamika sceny#

Klasyczny SLAM zakłada w większości statyczny świat. Dynamiczne punkty mogą zepsuć odometrię i utrwalić ghosts w mapie. Strategie:

robust loss i geometric outlier rejection;
temporal occupancy decay;
tracking obiektów i usuwanie ich z static map;
semantic masking;
wiele hipotez ruchu;
osobna local dynamic obstacle layer.

Decay musi być dobrany do zadania. Zbyt szybki usuwa stałą przeszkodę po chwilowym zasłonięciu; zbyt wolny pozostawia człowieka jako ścianę. Static map i dynamic layer powinny mieć różne czasy życia.

Poruszający się obiekt dominujący pole widzenia może stać się fałszywym „światem”. Cross-check z IMU oraz minimalny udział statycznego tła są potrzebne. Semantic network nie jest źródłem prawdy; może nie rozpoznać nietypowego obiektu.

Czas, kalibracja i ekstrynsyka#

Wszystkie sensory muszą mieć timestamp akwizycji. Dla lidaru potrzebny jest czas punktu, dla kamery środek ekspozycji/rolling shutter, dla IMU czas próbki. Offset i drift zegarów tworzą deformację zależną od ruchu.

Ekstrynsyki T_body_sensor obejmują rotację i lever arm. Błąd translacji lidaru ujawnia się podczas obrotu; błąd rotacji miesza osie i deformuje mapę. Elastyczne mocowanie zmienia ekstrynsykę, czego stała macierz nie opisze.

Kwalifikacja kalibracji powinna mieć:

identyfikator egzemplarza sensora;
serializowaną macierz z konwencją kierunku;
covariance i metodę pomiaru;
offset czasu i model zegara;
temperaturę;
checksum konfiguracji;
test osi po montażu.

Po zmianie uchwytu, uderzeniu albo wymianie sensora mapa z poprzedniej kalibracji może nie być kompatybilna.

Obserwowalność i degeneracje#

Typowe degeneracje geometryczne:

Środowisko/ruch	Słabo obserwowalne składowe
długi prosty tunel	przesunięcie wzdłuż osi tunelu
pojedyncza płaszczyzna	ruch w płaszczyźnie i yaw zależnie od geometrii
zawis bez paralaksy	głębokość visual i część biasów
czysty obrót	skala/translacja monocular
powtarzalny korytarz	globalne miejsce i loop closure
otwarte niebo + daleki grunt	mała geometria 3D dla części lidarów
gęsty dynamiczny tłum	brak dominującego modelu statycznego

Estimator powinien raportować kierunkową niepewność. Jedna wartość score nie mówi, że x jest dobry, a yaw słaby. Information matrix scan matching lub covariance grafu może wskazać słabe wartości własne.

Planer i failsafe muszą reagować przed całkowitą utratą pozy. Możliwe ograniczenia: zmniejszenie prędkości, zachowanie wysokości, powrót po świeżej lokalnej mapie, zawis tylko jeśli velocity jest wiarygodna, albo kontrolowane lądowanie.

Zarządzanie pamięcią#

Mapa rośnie z objętością i czasem. Mechanizmy:

rolling local map wokół UAV;
blokowa alokacja voxel hash/octree;
usuwanie bloków poza promieniem;
keyframe culling;
descriptor database z limitami;
kompresja i zapis submap;
oddzielna mapa globalna na stacji;
ograniczenie historii raw scans.

Usuwanie mapy nie może naruszyć relokalizacji lub safety. Local planner może potrzebować tylko kilku sekund i metrów, a place recognition — descriptorów całej misji. Te dane można przechowywać osobno.

Memory pressure powinno być stanem obserwowalnym. OOM kill companion computera jest nagłą utratą pozycji. Proces ma rezerwę, limity i kontrolowane degradowanie: rzadszy mesh, mniej wizualizacji, mniejsza mapa globalna, ale nie zatrzymanie odometrii.

Architektura czasu rzeczywistego#

Priorytety przykładowe:

1. sensor ingest + timestamp + IMU propagation
2. local odometry and health publication
3. local collision map update
4. local planning
5. global loop detection and graph optimization
6. mesh, visualization and archival

Kolejki powinny być ograniczone. Przetworzenie każdej starej chmury po przeciążeniu zwiększa latency bez odzyskania czasu rzeczywistego. Dla local map często lepiej odrzucić stare ramki i przetworzyć najnowszą, zachowując jawny dropout counter.

Worst-case events to loop closure, duża liczba candidate matches, zapis mapy, alokacja nowego regionu i relokalizacja. Test CPU musi obejmować te zdarzenia, temperaturę oraz throttling, a nie tylko prostą trasę.

Watchdog rozróżnia alive od timely. Proces może odpowiadać, lecz publikować stan sprzed pół sekundy. Freshness liczy się od timestampu pomiaru do użycia przez regulator.

Health i integralność#

Stan SLAM powinien mieć co najmniej:

UNINITIALIZED
LOCAL_ODOMETRY_ONLY
MAPPING_HEALTHY
GLOBAL_OPTIMIZATION_PENDING
DEGRADED
RELOCALIZING
FAILED

Metryki health:

age i rate odometrii;
covariance oraz kierunki degeneracji;
liczba inlierów/scan fitness z conditioning;
IMU residual i bias plausibility;
liczba aktywnych keyframes/submap;
czas ostatniego loop closure;
wielkość korekty map→odom;
local map age i known fraction;
dropped frames/scans;
CPU, memory, temperature;
disagreement z niezależnym GNSS/VIO/LIO.

Health nie powinien skakać od jednego progu. Histereza i liczba kolejnych dobrych pomiarów zapobiegają szybkiemu przełączaniu. Po relokalizacji warto wymagać okresu stabilności przed ponownym użyciem w trybie pozycyjnym.

Logowanie i metryki#

Log potrzebny do replay obejmuje raw/near-raw sensors z timestampami, calibration, transform tree, keyframe decisions, constraint candidates, accepted/rejected loop closures, pose graph przed i po optymalizacji, map revisions, health i dane autopilota.

Metryki lokalizacji:

relative pose error na kilku horyzontach;
absolute trajectory error po jawnie opisanym alignment;
drift na dystans/czas;
scale error;
reset i relocalization error;
covariance consistency.

Metryki mapy:

surface error względem ground truth;
completeness/coverage;
precision/recall occupied voxels;
false free rate — szczególnie krytyczna;
ESDF distance error;
map age i update latency;
memory per mapped volume.

Metryki systemowe:

latency p50/p95/p99/max;
deadline misses;
CPU/GPU/memory/power;
time to initialize i recover;
false/true loop closure rate;
global correction magnitude;
planner stops wynikające ze stale map.

Ładny mesh nie jest metryką nawigacyjną. Może ukrywać holes, false free i opóźnienie.

Testy i regresja#

Warstwy testów:

unit tests geometrii, transformacji i serialization;
synthetic scans/images z dokładnym ground truth;
public datasets dla reprodukcji publikowanych pipelines;
własne bags z motion capture/total station/RTK;
deterministic replay;
fault injection;
HIL z autopilotem i plannerem;
lot z niezależnym ground truth;
długie misje i restart/relocalization;
regression dashboard.

Fault injection obejmuje:

przesunięcie czasu sensorów;
błędną ekstrynsykę;
drop i reorder pakietów;
frozen odometry;
dynamiczny obiekt dominujący;
false loop candidate;
utratę IMU/kamery/lidaru;
brak pamięci i throttling;
uszkodzony plik mapy;
skok GNSS anchor;
opóźnioną ESDF.

Sceny testowe powinny zawierać pętlę, korytarz, halę bez cech, powtarzalne regały, przejście indoor–outdoor, schody, cienkie przeszkody, szkło, ludzi i lot wstecz/obroty. Każda wersja oprogramowania przechodzi te same bags z progami metryk.

Kryteria odbioru#

System jest gotowy do określonej misji, gdy:

odometria lokalna i globalna mapa mają rozdzielone ramy;
regulatory nie otrzymują skoków loop closure;
wszystkie sensory mają zweryfikowane timestampy i ekstrynsyki;
deskew korzysta z per-point time i poprawnej trajectory;
degeneracje zwiększają kierunkową covariance przed failure;
false loop closure jest odrzucany w scenach powtarzalnych;
relokalizacja sygnalizuje reset i ma kontrolowany re-entry;
unknown/free/occupied mają jawną politykę;
false free rate i cienkie przeszkody spełniają wymaganie;
ESDF/local map są świeże względem drogi hamowania;
zmiana map revision wymusza replan lub transformację celu;
memory i CPU mają bounded worst-case;
local safety działa podczas global optimization;
log umożliwia odtworzenie constraintów i mapy;
fault injection aktywuje oczekiwany degraded mode/failsafe;
regression suite blokuje pogorszenie lokalizacji, mapy i latency.

Kryteria muszą mieć wartości liczbowe dla konkretnej prędkości, zasięgu sensora, rozmiaru UAV i środowiska. Wynik z publikacji lub datasetu nie jest automatycznie wynikiem własnej platformy.

Typowe błędy#

Podawanie globalnie korygowanej pozy bezpośrednio do regulatora.
Traktowanie point cloud jako mapy free-space.
Uznanie unknown za free bez ograniczenia prędkości i zasięgu.
Brak reintegracji/deformacji mapy po loop closure.
Timestamp całego skanu zamiast czasu punktów.
Brak deskew podczas szybkiego ruchu.
Ignorowanie słabych kierunków scan matching.
Jedna izotropowa covariance dla wszystkich scen.
Akceptacja loop closure tylko na podstawie descriptor score.
Łączenie map z różnych kalibracji bez wersjonowania.
Nieograniczona kolejka starych chmur.
OOM lub throttling bez stanu degraded.
Global mesh mający wyższy priorytet niż local odometry.
Inflacja przeszkód tylko o promień kadłuba.
Brak decay/warstwy dynamicznych przeszkód.
Próg keyframe niezależny od jakości geometrii.
Test tylko w statycznym laboratorium.
Ocena przez wygląd mapy zamiast false free i ESDF error.
Użycie nieutrzymywanego projektu bez analizy ryzyka zależności.

Powiązane tematy#

Visual odometry i VIO w UAV — frontend kamerowy i fuzja IMU.
Lidar w UAV — fizyka pomiaru, FOV, zasięg i błędy.
IMU w dronie — propagacja, bias i deskew.
Filtr Kalmana i EKF w UAV — covariance i innovations.
Układy współrzędnych UAV — map, odom, NED, ENU, body i sensor.
INS w UAV — mechanizacja inercyjna.
Dead reckoning UAV — praca po utracie zewnętrznego odniesienia.
Omijanie przeszkód UAV — konsument mapy lokalnej.
Mapowanie lidarowe UAV — pipeline mapy punktowej i powierzchniowej.
Terrain following UAV — mapa terenu i wysokość nad gruntem.

Przypisy#

Bibliografia jest generowana z centralnego rejestru. Rozdzielenie lokalnej odometrii, mapowania i pose graph oparto na architekturach ORB-SLAM3, LOAM, LIO-SAM oraz Cartographer. Informację o obecnym braku aktywnego rozwoju Cartographer zaczerpnięto z oficjalnego repozytorium i należy ją ponownie sprawdzić przed wyborem zależności. Semantyka map, odom i body wynika z ROS REP 105. Probabilistyczne stany occupied/free/unknown opisuje dokumentacja OctoMap. Pipeline TSDF→ESDF oraz zastosowanie ESDF do planowania on-board MAV oparto na publikacji i dokumentacji voxblox. Parametry, dokładność i obciążenie muszą być zweryfikowane dla konkretnego sensora, wzoru skanowania, komputera i środowiska.

Źródła z centralnego rejestru

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]
Cartographer — real-time 2D and 3D SLAM [repozytorium open source — projekt nie jest aktywnie rozwijany]
Hess et al.: Real-Time Loop Closure in 2D LIDAR SLAM [publikacja naukowa]
LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping [repozytorium open source]
Shan et al.: LIO-SAM — Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping [publikacja naukowa]
Zhang, Singh: LOAM — Lidar Odometry and Mapping in Real-time [publikacja naukowa]
ETH Zurich ASL: voxblox [repozytorium open source]
Oleynikova et al.: Voxblox — Incremental 3D Euclidean Signed Distance Fields for On-Board MAV Planning [publikacja naukowa]
OctoMap: probabilistic 3D occupancy mapping library [dokumentacja projektu]
ROS REP 105: Coordinate Frames for Mobile Platforms [standard projektu open source]
OpenVINS: documentation and estimator overview [dokumentacja projektu badawczego]