Terrain following UAV

Terrain following utrzymuje zadaną odległość pionową lub normalną od modelu terenu, wykorzystując cyfrową mapę wysokości, pomiar skierowany w dół albo fuzję obu. Nie jest synonimem omijania przeszkód. DEM może opisywać grunt z siatką dziesiątek metrów i nie zawierać drzewa, przewodu ani budynku; rangefinder widzi fragment powierzchni pod platformą, ale nie ostrzega wystarczająco wcześnie o stoku przed szybko lecącym fixed-wingiem.

System musi rozdzielać wysokość platformy, wysokość gruntu, AGL, clearance od przeszkód i zapas wynikający z osiągów. Najczęstsza wada projektu polega na bezpośrednim przepisaniu zaszumionej odległości na setpoint z. Prawidłowy tor ma model geometrii sensora, filtr stanu terenu, look-ahead, ograniczenia prędkości/wznoszenia/jerk, kontrolę aktualności danych i maszynę stanów po utracie źródła.

Spis treści#

Wysokość nie jest jedną liczbą
Trzy klasy systemu
DEM kontra pomiar lokalny
Geometria pomiaru w dół
Footprint i powierzchnia
Lidar, sonar i radar
Estymacja wysokości terenu
Sterowanie AGL
Look-ahead i osiągi
Fixed-wing i multirotor
DEM, siatka i interpolacja
MAVLink Terrain Protocol
Cache i kompletność danych
Przeszkody a teren
Roślinność, woda i śnieg
Latency i bandwidth
Przejścia źródeł
Failsafe
Logowanie i diagnostyka
SITL, HIL i próby
Kryteria odbioru
Typowe błędy
Powiązane tematy
Przypisy

Wysokość nie jest jedną liczbą#

W systemie występują różne odniesienia:

Wielkość	Odniesienie	Typowe źródło
wysokość elipsoidalna	elipsoida WGS84	GNSS
wysokość ortometryczna	geoid/średni poziom morza	GNSS + model geoidy, DEM
relative altitude	home/origin	autopilot
local NED z	lokalny origin, dodatnie w dół	EKF
AGL	lokalna powierzchnia terenu	DEM lub rangefinder
slant range	wzdłuż osi promienia	lidar/sonar/radar
clearance	najkrótsza odległość do przeszkody	mapa 3D/proximity

AGL z mapy można zapisać:

h_AGL = h_vehicle_MSL - h_terrain_MSL

Jeżeli GNSS podaje wysokość nad elipsoidą, a DEM wysokość ortometryczną, potrzebna jest separacja geoidy. Pominięcie transformacji może dać błąd dziesiątek metrów. Pole nazwane alt bez frame i datum jest niepełnym interfejsem.

Rangefinder mierzy slant range. Po pochyleniu platformy nie jest to automatycznie pionowa wysokość. Dodatkowo punkt trafienia znajduje się obok rzutu pionowego UAV. Te różnice stają się istotne nad stokiem.

Trzy klasy systemu#

Terrain-referenced mission z DEM#

Waypoints mają wysokość względem terenu. Autopilot interpoluje DEM i generuje profil wysokości. System działa także wysoko, poza zasięgiem rangefindera, lecz dokładność ogranicza mapa.

Surface tracking z sensorem#

Pomiar w dół aktualizuje estymatę powierzchni bez globalnej mapy. Dobrze działa nisko i wolno w obwiedni sensora. Reaguje na drzewa/dachy, choć nie zawsze wiadomo, czy są „terenem”, czy przeszkodą.

Fuzja mapy i sensora#

DEM daje look-ahead, a rangefinder koryguje lokalny offset i wykrywa rozbieżność. Jest to najbardziej użyteczna architektura, ale wymaga polityki konfliktu. Sensor może widzieć koronę drzewa, a DEM grunt; żadna wartość nie jest bezwarunkowo błędna.

ArduPilot dokumentuje terrain following zarówno z danych terenu dostarczanych przez GCS/SD, jak i z downward lidar/sonar w odpowiednich trybach. Dokumentacja PX4 wersji 1.13 opisuje terrain following/hold z distance sensorem i osobną estymatą terenu. Dokładne parametry są wersjozależne i trzeba je sprawdzić dla zainstalowanego firmware.

DEM kontra pomiar lokalny#

Cecha	DEM	Rangefinder
zasięg do przodu	duży	zwykle mały/zerowy
aktualność	zależy od źródła	bieżąca
drzewa i budynki	często niepełne	widoczne w footprint
datum	musi być znane	pomiar względny
GNSS dependency	zwykle tak	nie dla samej AGL
rozdzielczość	grid	footprint wiązki
woda/szkło	zależne od mapy	sensor-zależne
lot wysoko	możliwy	ograniczony zasięgiem

DEM nadaje się do przewidywania profilu, lecz grid nie gwarantuje zachowania wąskiego grzbietu. Rangefinder mierzy lokalną powierzchnię, ale działa reaktywnie. Dla fixed-winga sama reakcja pod platformą może nastąpić po minięciu punktu, w którym należało rozpocząć wznoszenie.

System powinien przechowywać osobno:

terrain_map_height + map_uncertainty
terrain_sensor_height + sensor_uncertainty
obstacle_clearance + unknown status

Nie należy przedwcześnie scalać ich w jedną liczbę bez informacji o pochodzeniu.

Geometria pomiaru w dół#

Dla sensora o osi jednostkowej u_s i rotacji R_ns kierunek promienia w układzie nawigacyjnym:

u_n = R_ns u_s
p_hit = p_sensor + r u_n

Na płaskim gruncie pionowa różnica wysokości jest związana z komponentem promienia:

h_vertical ≈ r · cos(theta)

theta jest kątem osi względem pionu. To przybliżenie zakłada, że return pochodzi z płaszczyzny pod sensorem. Nad stokiem punkt trafienia ma inną wysokość niż pionowy rzut środka UAV.

Lever arm sensora względem IMU/CG wpływa podczas roll/pitch. Położenie punktu pomiaru:

p_sensor = p_body + R_nb r_body_sensor

Wysunięty sensor zatacza łuk. Brak kompensacji może tworzyć pozorny ruch gruntu zsynchronizowany z attitude.

W praktyce tor potrzebuje timestampu odległości oraz attitude z tego samego czasu. Użycie bieżącego roll do pomiaru sprzed 50 ms daje błąd skorelowany z manewrem.

Footprint i powierzchnia#

Wiązka ma rozbieżność, a kamera depth — pole widzenia. Footprint rośnie z odległością:

d_footprint ≈ 2 r tan(FOV/2)

Sensor może zwracać pierwszy return, strongest return, centroid albo inny statystykę. Nad krawędzią część wiązki widzi dach, część grunt. Skok wyniku nie musi być usterką.

Pytania kwalifikacyjne:

co oznacza pojedynczy wynik dla wielu returnów;
jak zmienia się footprint z dystansem;
czy quality wykrywa mixed pixels;
czy minimalny/maksymalny zasięg jest mierzony dla docelowej powierzchni;
jak działa filtr producenta i jego latency;
czy brak returnu ma osobny status od maksymalnej odległości.

Wąski lidar może wpaść między gałęzie i zobaczyć grunt, gdy kadłub leci w koronę. Szeroki radar może wykryć wcześniejszą powierzchnię, ale z gorszą rozdzielczością. Terrain sensor nie zastępuje pełnej mapy przeszkód.

Lidar, sonar i radar#

Lidar#

Mały footprint i duża częstotliwość, ale zależność od reflektancji, słońca, mgły, deszczu i geometrii powierzchni. Near-IR nad wodą lub czarną powierzchnią może tracić return. Deklarowany max range musi być zweryfikowany dla albedo i światła.

Sonar#

Szersza wiązka, zależność od temperatury, wiatru, miękkich powierzchni i echa wielodrogowego. Propwash i hałas mogą pogorszyć pomiar. Może być użyteczny na małej wysokości, ale obwiednia jest silnie platformowa.

Radar altimeter#

Większa odporność na część warunków optycznych, często szerszy footprint i inna charakterystyka returnów. Integracja RF/EMC, masa i cena są zwykle większe. Nad roślinnością wynik może odpowiadać innemu poziomowi niż lidar.

Dobór powinien wynikać z biblioteki powierzchni i pogody, nie tylko z tabeli zasięgu producenta.

Estymacja wysokości terenu#

Pomiar range nie powinien bezpośrednio ustawiać pozycji z. Estymator może utrzymywać wysokość terenu h_T w lokalnym układzie:

z_range_pred = geometry(p_vehicle, attitude, h_T)
innovation = z_meas - z_range_pred

Vehicle altitude pochodzi z barometru/GNSS/VIO/INS, a range aktualizuje terrain state. To rozdziela ruch UAV od zmiany podłoża. PX4 opisuje osobny jednowymiarowy terrain estimator w funkcjach terrain hold/follow.

Model procesu terenu określa, jak szybko h_T może się zmieniać wraz z ruchem poziomym. Zbyt mały process noise opóźnia stok; zbyt duży przepisuje szum sensora na setpoint.

Gating powinien uwzględniać:

min/max range i quality;
tilt i footprint;
zmianę względem możliwej slope oraz speed;
innovation względem stanu;
rate limit;
consistency z DEM;
czas ostatniego valid return.

Odrzucenie gwałtownego wzrostu terenu może być niebezpieczne, jeśli to prawdziwa przeszkoda. Dlatego safety proximity layer powinna niezależnie reagować na bliski return, nawet gdy terrain estimator klasyfikuje go jako outlier.

Sterowanie AGL#

Setpoint pozycji pionowej może być zbudowany:

h_vehicle_target = h_terrain_est + h_AGL_target

Następnie trajectory generator ogranicza velocity, acceleration i jerk. Nie wolno skakać setpointem przy każdej zmianie range.

Pasmo sterowania AGL musi być niższe niż pasmo attitude i zgodne z dynamiką napędu. Za szybka reakcja na drobną teksturę terenu tworzy falowanie wysokości i duży pobór mocy. Za wolna reakcja zwiększa błąd nad stromym stokiem.

Można rozdzielić:

wolny profil z DEM;
lokalną korekcję range o ograniczonej amplitudzie;
natychmiastowy avoidance dla przeszkody;
baro/GNSS jako stabilne odniesienie długookresowe.

Target AGL nie jest gwarantowany, gdy ograniczenia wznoszenia mają pierwszeństwo. System powinien raportować tracking error i predicted clearance, a nie tylko zadany target.

Look-ahead i osiągi#

Look-ahead próbuje rozpocząć wznoszenie przed stokiem. Minimalna odległość zależy od ground speed, opóźnienia, maksymalnego climb angle/rate i profilu przyspieszeń.

Dla uproszczonego stałego climb rate w_max i wymaganej zmiany Δh:

t_climb ≥ Δh / w_max
d_lookahead ≥ v_ground (t_latency + t_climb) + margin

Dla fixed-winga ograniczeniem jest nie tylko w_max, ale energia, airspeed, load factor, zakręt i wiatr. ArduPilot Plane opisuje look-ahead korzystający z ograniczenia wznoszenia i ground speed; odcinek misji nie może wymagać osiągów, których płatowiec nie ma.

Profil powinien być wyliczony przed lotem dla całej trasy:

próbkuj DEM gęściej niż waypoints;
dodaj niepewność mapy i clearance;
wyznacz envelope możliwego wznoszenia/opadania;
wykryj odcinki infeasible;
uwzględnij zakręty i wariant wiatru;
zapewnij procedurę po utracie DEM.

„Waypoint 60 m AGL” na obu końcach doliny nie gwarantuje 60 m AGL pomiędzy nimi.

Fixed-wing i multirotor#

Multirotor może zwolnić lub zawisnąć, ma duży zakres pionowej kontroli, ale ogranicza go thrust margin, bateria i downdraft. Reaktywny range-based surface tracking jest możliwy przy małej prędkości.

Fixed-wing musi zachować airspeed i nie może zatrzymać się przed stokiem. Potrzebuje mapowego look-ahead oraz zapasu energii. Opadanie może być równie ograniczone jak wznoszenie; zbyt stromy spadek terenu nie powinien wymuszać dive tylko po to, aby utrzymać target AGL.

VTOL ma różne obwiednie w MC i FW. Przełączenie trybu zmienia dopuszczalne rate, sensor footprint i politykę terrain following. Profil zaplanowany dla zawisu może być nierealny po transition.

DEM, siatka i interpolacja#

DEM jest próbkowany w gridzie. Interpolacja biliniowa wygładza wartości między czterema komórkami, ale nie odtwarza cechy niewidocznej w próbkach. Grid spacing s nie jest równy dokładności pionowej.

Źródła błędu:

vertical datum;
rozdzielczość i interpolacja;
czas akwizycji mapy;
roślinność/budynki;
void filling;
błąd poziomej pozycji przesuwający próbkę na stromym stoku;
quantization formatu.

Na stoku błąd poziomy δx daje błąd terenu około:

δh_T ≈ slope · δx

Dlatego dokładny DEM nie wystarczy przy słabym GNSS. Map uncertainty powinna obejmować gradient terenu i horizontal covariance.

ArduPilot dokumentuje typowy grid dostarczany przez używane serwery oraz ostrzega, że ustawienie mniejszego spacing nie poprawia danych, jeśli źródło ma mniejszą rozdzielczość. Wartości i zachowanie cache są wersjozależne.

MAVLink Terrain Protocol#

MAVLink Terrain Protocol pozwala pojazdowi pobierać kafle z GCS. Sekwencja:

vehicle → TERRAIN_REQUEST(mask, origin, spacing)
GCS     → TERRAIN_DATA(tile values)
vehicle → TERRAIN_REPORT(pending, loaded, terrain/current height)

TERRAIN_REQUEST.mask koduje wymagane bloki, a TERRAIN_DATA przekazuje siatkę wysokości. Pojazd utrzymuje stan brakujących kafli i może je ponawiać. TERRAIN_CHECK pozwala GCS zapytać cache.

Protokół transportuje wartości, ale nie rozwiązuje:

pochodzenia i jakości DEM;
aktualności przeszkód;
niezawodności Internetu GCS;
datum poza założeniami pola;
polityki lotu bez kompletnego kafla.

Misja krytyczna nie powinna polegać na pobraniu kafla w ostatniej chwili przez łącze. Dane należy prefetch, zweryfikować i przechowywać lokalnie wraz z checksum i coverage.

Cache i kompletność danych#

Cache ma stany: requested, received, verified, resident i stale. Sam fakt istnienia pliku nie oznacza pokrycia całej trasy. Preflight powinien sprawdzić corridor powiększony o możliwe odchylenie, RTL, alternatywne lądowiska i geofence.

Raport powinien podać:

procent próbek trasy z DEM;
min/max spacing;
źródło i datum;
maksymalny wiek;
checksum kafli;
brakujące obszary;
pamięć/cache headroom;
zachowanie bez GCS.

ArduPilot może zapisywać dane na SD i wczytywać je do pamięci. Failsafe po braku danych jest zależny od trybu i wersji; dokumentacja Copter opisuje przejście po utracie wymaganych danych. To zachowanie trzeba odtworzyć w SITL, bo RTL może użyć innego odniesienia wysokości niż pierwotna misja.

Przeszkody a teren#

Terrain following utrzymuje profil względem powierzchni, lecz nie planuje wokół pionowej ściany. Budynek może być potraktowany przez downward sensor jak nagły wzrost „terenu”, co wywoła climb; przewód przed dronem pozostanie niewidoczny.

Potrzebne są osobne warstwy:

DEM terrain profile
local ground estimator
3D obstacle map / proximity
vehicle dynamics envelope

Finalny clearance jest maksimum wymagań z warstw, nie wyborem jednego sensora. Unknown przed platformą powinno ograniczać prędkość względem drogi zatrzymania.

Roślinność, woda i śnieg#

„Grunt” zależy od zadania. Dla zdjęć upraw target może być względem canopy; dla geodezji względem ziemi; dla safety względem najwyższej powierzchni.

lidar może trafiać koronę lub przechodzić przez szczeliny;
radar może penetrować część roślinności inaczej;
sonar ma szerokie echo od canopy;
woda może dawać brak/odbicie zależne od kąta;
śnieg zmienia reflektancję i rzeczywistą powierzchnię;
fale są dynamiczną powierzchnią.

Należy zdefiniować percentyl/return policy. Użycie minimum range jest konserwatywne dla przeszkód, ale nad pojedynczą gałęzią może tworzyć gwałtowny climb. Median może zignorować cienką, realną przeszkodę. Safety layer i terrain control powinny mieć różne statystyki.

Latency i bandwidth#

Opóźnienie obejmuje sensor integration, transport, filtr, estimator, trajectory generator i aktuator. Przy prędkości v pomiar dotyczy punktu oddalonego o v t_latency od bieżącego położenia wzdłuż trasy.

Mierzyć trzeba:

sample age, nie czas odebrania;
jitter i p99/max;
dropouts;
bandwidth filtra terenu;
phase lag AGL command→response;
saturation climb/descent.

Zwiększenie low-pass redukuje szum, ale zwiększa lag. Dobór jest kompromisem liczonym razem z prędkością. Ten sam filtr nie pasuje do zawisu 2 m/s i fixed-wing 30 m/s.

Przejścia źródeł#

Typowe przejścia:

DEM → range po wejściu w zasięg;
range → DEM po wyjściu poza max range;
range → baro-relative po utracie quality;
GNSS/DEM → local VIO/range indoor;
terrain mode → normal altitude mode w failsafe.

Przed przełączeniem trzeba oszacować offset źródeł:

delta = h_terrain_range - h_terrain_DEM

Setpoint może być blendowany z ograniczonym rate, ale tylko gdy oba źródła są healthy. Jeśli jedno jest niewiarygodne, płynne przejście przez błędną wartość nie jest zaletą.

Maszyna stanów przechowuje frame aktywnego celu. Po utracie range nie wolno interpretować tej samej liczby jako height above home bez przeliczenia.

Failsafe#

Failsafe zależy od platformy i otoczenia. Możliwe reakcje:

zatrzymanie poziome i zawis, jeśli pozycja/prędkość są wiarygodne;
climb do wcześniej sprawdzonej bezpiecznej wysokości;
przejście na DEM z większym marginesem;
RTL po trasie z kompletnym profilem;
kontrolowane lądowanie;
manual takeover.

„Climb” nie zawsze jest bezpieczny indoor lub pod przeszkodą. „RTL” nie jest bezpieczny, jeśli jego wysokość zmienia odniesienie po utracie terrain data. Każda reakcja musi zostać oceniona dla scenariusza.

Warunki wyzwolenia:

stale range/DEM;
invalid quality lub out-of-range;
AGL error przekracza limit;
predicted clearance poniżej limitu;
wymagany climb przekracza możliwości;
cache missing przed trasą;
konflikt DEM–sensor;
estimator terrain covariance zbyt duża.

Logowanie i diagnostyka#

Log powinien zawierać:

raw range, quality, min/max i timestamp;
attitude oraz lever-arm corrected range;
terrain state i covariance;
DEM height, grid cell, spacing i source;
AGL target, estimate i error;
vertical velocity/acceleration/thrust saturation;
look-ahead profile i predicted clearance;
active source/state transition;
tile pending/loaded/cache miss;
obstacle layer output;
failsafe reason.

Wykresy diagnostyczne:

range raw/corrected vs terrain state;
DEM terrain vs sensor terrain;
target AGL vs measured AGL;
vertical rate target/actual i saturation;
sample age i quality;
map coverage przed pozycją;
roll/pitch vs range residual.

Residual skorelowany z tilt sugeruje geometrię/lever arm/czas. Błąd rosnący ze speed sugeruje latency. Schodki w DEM odpowiadają grid/interpolacji.

SITL, HIL i próby#

SITL powinien zasymulować profil terenu, brak kafla i zmianę datum. Fault injection:

utrata TERRAIN_DATA;
opóźnienie odpowiedzi GCS;
błędny grid spacing;
range freeze/out-of-range;
skok range nad przeszkodą;
rosnący delay;
konflikt DEM–range;
niespełnialny stok;
transition VTOL;
RTL po utracie danych.

Stanowisko rangefindera obejmuje kilka powierzchni, kąty, wysokości, światło i ruch. Turntable sprawdza tilt compensation, prowadnica — latency i edge response.

Loty zaczynają się nad płaskim, znanym terenem z GNSS/laser tracker jako odniesieniem, range tylko logowany. Następnie włącza się korekcję z małym limitem, łagodny stok, krawędzie, roślinność i dopiero pełną obwiednię. Fixed-wing wymaga profilu infeasible i sprawdzenia, czy planner odrzuca misję przed startem.

Kryteria odbioru#

Wszystkie wysokości mają frame i vertical datum.
Slant range jest poprawnie przeliczany z attitude i lever arm.
Timestamp opisuje pomiar, a p99/max latency są zmierzone.
Sensor ma zakwalifikowaną bibliotekę powierzchni i pogody.
DEM ma źródło, spacing, datum, coverage i uncertainty.
Corridor misji, RTL i alternatywy są w cache przed lotem.
Look-ahead uwzględnia ground speed, wiatr i realny climb envelope.
Odcinek niemożliwy do wykonania jest odrzucany przed startem.
Terrain, obstacle i unknown mają osobne warstwy.
Range/DEM conflict zwiększa margines lub aktywuje stan degraded.
Przejścia źródeł nie zmieniają znaczenia setpointu.
Utrata danych wywołuje scenariuszowo poprawny failsafe.
AGL tracking error mieści się w budżecie bez saturacji.
Thin obstacle i krawędź nie są maskowane przez filtr terenu.
Log pozwala odtworzyć mapę, stan i decyzję failsafe.
SITL/HIL obejmuje cache miss, stale sensor i niespełnialny stok.

Typowe błędy#

Mieszanie wysokości elipsoidalnej, MSL, home i AGL.
Traktowanie slant range jako pionowej wysokości przy tilt.
Brak lever arm i zgodnego timestampu attitude.
Bezpośrednie wpisanie raw range do setpointu z.
Uznanie DEM za mapę przeszkód.
Uznanie rangefindera za look-ahead.
Grid spacing utożsamiony z dokładnością.
Brak pokrycia DEM dla RTL.
Pobieranie kafli dopiero w locie przez Internet GCS.
Brak jawnej polityki unknown.
Filtr odrzucający prawdziwy nagły wzrost gruntu jako outlier.
Jeden próg quality dla wody, trawy i asfaltu.
Max range ustawiony według datasheetu bez testu powierzchni.
Płynne przejście do błędnego źródła tylko dla uniknięcia skoku.
RTL zmieniający odniesienie wysokości bez analizy.
Fixed-wing mission bez sprawdzenia climb envelope.
Ocena tylko średniego AGL bez minimum clearance.

Powiązane tematy#

Lidar w UAV — zasięg, footprint, returny i integracja.
Barometr w UAV — stabilne odniesienie wysokości i ground effect.
GNSS w UAV — pozycja pozioma i odniesienia wysokości.
Układy współrzędnych UAV — NED, ECEF, WGS84 i datum.
SLAM dla UAV — lokalna mapa occupancy/ESDF.
Dead reckoning UAV — zachowanie po utracie źródła.
Omijanie przeszkód UAV — clearance poza modelem terenu.
Mapowanie lidarowe UAV — tworzenie modelu 3D.
Failsafe jako maszyna stanów — przejścia i recovery.

Przypisy#

Bibliografia jest generowana z centralnego rejestru. Semantykę terrain following z DEM, SD/GCS cache, look-ahead i zachowanie Plane/Copter oparto na bieżącej dokumentacji ArduPilot. Sekwencję kafli i pola komunikatów opisuje oficjalny MAVLink Terrain Protocol. Terrain follow/hold z osobnym terrain estimator i distance sensorem porównano z dokumentacją PX4 strony wersji 1.13; konfigurację trzeba ponownie zweryfikować dla dokładnej wersji firmware. Informacje o rodzaju rangefinderów i ograniczeniach max range pochodzą z dokumentacji ArduPilot. Liczby dotyczące spacing, cache i failsafe są wersjozależne i nie zastępują testów konkretnej platformy.

Źródła z centralnego rejestru

ArduPilot Copter: Terrain Following [dokumentacja projektu]
ArduPilot Plane: Terrain Following [dokumentacja projektu]
ArduPilot: RangeFinder Setup Overview [dokumentacja projektu]
MAVLink: Terrain Protocol [specyfikacja protokołu]
PX4: Terrain Following, Terrain Hold and Range Aid [dokumentacja projektu — strona wersji 1.13, ponownie sprawdzić dla używanego firmware]
PX4: Using PX4 Navigation Filter EKF2 [dokumentacja projektu]
OctoMap: probabilistic 3D occupancy mapping library [dokumentacja projektu]