Optical flow w UAV

Optical flow w UAV mierzy pozorny ruch tekstury między kolejnymi obrazami. Czujnik skierowany w dół może dzięki temu dostarczać poziomą prędkość względem gruntu bez GNSS, ale jego surowym wynikiem nie są metry na sekundę. Jest nim kątowy ruch obrazu w czasie. Do wyznaczenia skali potrzebna jest odległość do obserwowanej powierzchni, a do oddzielenia translacji od obrotu — zsynchronizowane dane żyroskopu. Gładka podłoga, ciemność, nadmierna wysokość, rozmycie lub błędny timestamp mogą dać płynny, lecz fałszywy wynik.

Co sensor rzeczywiście mierzy#

Obraz punktu sceny przesuwa się po matrycy, gdy kamera porusza się lub obraca. Optical flow opisuje dwuwymiarowe pole prędkości obrazu:

u(x,y) = dx/dt
v(x,y) = dy/dt

W małym module typu PMW3901 wynik jest zwykle zintegrowanym przesunięciem lub kątem w dwóch osiach oraz jakością. Moduł nie zwraca globalnej pozycji. Integracja prędkości z czasem daje pozycję lokalną, ale każdy bias kumuluje dryf.

PX4 definiuje pixel_flow[2] jako zintegrowany przepływ w radianach, zgodny z prawoskrętną rotacją wokół osi sensora. Pole integration_timespan_us określa okres akumulacji. Te wartości nie są translacyjnym Δx/Δy platformy.

Model pinhole i skala#

Dla kamery patrzącej prostopadle na płaski grunt oraz małych kątów translacja pozioma powoduje angular flow w przybliżeniu:

ω_flow,x ≈ -v_y / h
ω_flow,y ≈  v_x / h

Znaki zależą od konwencji osi. h jest odległością wzdłuż optycznej/geometrii do obserwowanej powierzchni, a nie bezwarunkowo wysokością barometryczną.

Po kompensacji obrotu prędkość:

v_x ≈  h · ω_trans,y
v_y ≈ -h · ω_trans,x

Dlatego błąd skali dalmierza przenosi się prawie proporcjonalnie na prędkość. Jeżeli odległość jest zawyżona o 10%, estymowana prędkość również może być zawyżona o podobny rząd.

Przy pochyłym gruncie jedna wartość h nie opisuje całego obrazu. Punkty w górnej i dolnej części mają inną głębokość. Dedykowany sensor zwracający zagregowany przepływ zakłada zwykle lokalnie płaską powierzchnię; pełna kamera może użyć homografii lub modelu wielu głębokości.

Obrót kamery#

Roll, pitch i yaw przesuwają obraz nawet bez translacji. W zawisie szybka korekta attitude może generować flow większy niż powolny dryf pozycji. Dla małych przyrostów:

Δθ_image = Δθ_translation + Δθ_rotation
Δθ_translation = Δθ_image - Δθ_gyro_projected

Żyroskop musi opisywać ten sam przedział czasu co obraz. Odejmowanie bieżącej rate od przepływu z ramki zakończonej kilkanaście milisekund wcześniej pozostawia pozorną prędkość skorelowaną z manewrem.

PX4 proponuje test czystej rotacji: zintegrowany przepływ i odpowiadający mu zintegrowany żyroskop powinny mieć tę samą skalę w odpowiednich osiach. Różnica może wskazywać błędną ogniskową, orientation, znak, timestamp lub synchronizację.

Oś obrazu a oś platformy#

Kamera ma oś optyczną oraz osie pikseli, flight controller zwykle używa body FRD, a estymator lokalny NED. Przesunięcie platformy do przodu może pojawić się w innym kanale flow niż intuicyjne x.

PX4 podaje dla prawidłowo zamontowanego sensora:

Ruch platformy	Zintegrowany flow
do przodu	`+Y`
do tyłu	`-Y`
w prawo	`-X`
w lewo	`+X`

To test interfejsu, nie uniwersalna konwencja każdego chipa. Sterownik tłumaczy osie sensora do konwencji autopilota. Test na stole należy wykonać dla translacji i rotacji osobno.

Ogniskowa i skala kątowa#

Przesunięcie w pikselach staje się kątem przez efektywną ogniskową:

Δθ_x ≈ Δp_x / f_x
Δθ_y ≈ Δp_y / f_y

f_x/f_y są w pikselach. Zależą od obiektywu, cropu, binningu i resize. Zmiana rozdzielczości bez zmiany parametrów skali daje błąd prędkości.

Moduły z zamkniętym algorytmem zwracają już jednostki counts lub radians według własnej kalibracji. ArduPilot udostępnia korekty skali osi FLOW_FXSCALER/FYSCALER, a PX4 SENS_FLOW_SCALE. Korekta software nie powinna maskować błędnej wysokości, osi lub latency.

Kalibrację skali wykonuje się przez kontrolowany ruch o znanej odległości i wysokości albo czystą rotację z referencyjnym gyro. Ruch ręką bez ground truth pozwala wykryć znak, nie dokładną skalę.

Dlaczego potrzebny jest dalmierz#

Optical flow monocularny ma nieobserwowalną skalę: ten sam kątowy ruch obrazu może oznaczać małą prędkość nisko albo dużą wysoko. Dalmierz dostarcza głębokość.

PX4 wymaga ważnego rangefindera do fuzji optical flow w EKF2. ArduPilot również wskazuje rangefinder jako wymagany dla autonomicznych trybów z przepływem. Bitcraze Flow deck v2 łączy PMW3901 z ToF VL53L1x, a oba pomiary trafiają do EKF Crazyflie.

Para musi mieć wspólną geometrię:

dalmierz powinien mierzyć powierzchnię reprezentatywną dla środka pola flow;
jego FOV nie może wybierać innego obiektu niż kamera;
timestamp odległości musi odpowiadać flow;
minimalny i maksymalny zakres muszą pokrywać obwiednię kamery;
tilt compensation musi używać właściwej osi.

Wąski lidar może trafić w szczelinę, gdy kamera widzi głównie stół. Szeroki ToF może zwrócić najbliższą krawędź. Taka niezgodność skali daje skok prędkości mimo dobrego quality obrazu.

Tekstura i aperture problem#

Jedna krawędź pozwala obserwować ruch prostopadły do siebie, ale nie wzdłuż. To aperture problem. Narożniki i tekstura w dwóch kierunkach lepiej warunkują rozwiązanie.

Równanie jasności optical flow:

I_x u + I_y v + I_t = 0

ma jedno równanie i dwie niewiadome. Lucas–Kanade zakłada podobny ruch w małym oknie i rozwiązuje układ least squares z wielu pikseli. Macierz gradientów musi mieć energię w obu kierunkach; gładkie pole albo prosta linia są źle uwarunkowane.

OpenCV opisuje sparse pyramidal Lucas–Kanade dla wybranych feature points. Piramida pozwala obsłużyć większy ruch, ale zbyt duży displacement, blur lub zmiana wyglądu nadal łamie założenia.

Dedykowany chip flow może stosować korelację bloków zamiast LK. Z punktu widzenia integratora liczą się te same ograniczenia: kontrast, motion range, frame rate, jakość i oświetlenie.

Oświetlenie#

Algorytmy zakładają w przybliżeniu stałość jasności wzorca między ramkami. Auto-exposure, migotanie LED, cienie śmigieł i refleksy naruszają to założenie.

Trudne przypadki:

jednolita błyszcząca posadzka;
czarny matowy grunt przy małej ilości światła;
prześwietlenie w słońcu;
przejście z wnętrza na zewnątrz;
źródło LED z PWM i rolling shutter;
powierzchnia odbijająca poruszony obraz otoczenia;
woda i poruszająca się roślinność.

Bitcraze zaznacza, że PMW3901 działa najlepiej nad powierzchniami matowymi. Nie oznacza to, że każda matowa powierzchnia ma wystarczającą teksturę.

Aktywne doświetlenie podczerwone pomaga w ciemności, ale musi być zgodne z pasmem matrycy, ekspozycją i bezpieczeństwem optycznym. W słońcu jego kontrast może być znikomy.

Ekspozycja, blur i frame rate#

Podczas ekspozycji obraz przesuwa się. Długość smugi w pikselach:

b_px ≈ |ω_image| · f_px · t_exp

Przy dużej wysokości ten sam v daje mniejszy angular flow, ale szczegóły gruntu także zajmują mniej pikseli. Przy szybkim obrocie blur może dominować niezależnie od wysokości.

Krótka ekspozycja redukuje blur kosztem liczby fotonów i szumu. Wyższy frame rate zmniejsza displacement między ramkami, lecz zwiększa bandwidth oraz może wymusić krótszą ekspozycję.

Sensor ma maksymalny wiarygodny flow rate. PX4 przechowuje max_flow_rate w komunikacie. Przekroczenie nie powinno zostać zaklasyfikowane jako zwykły pomiar o małej prędkości; może wystąpić aliasing lub utrata korelacji.

Rolling shutter i global shutter#

Global shutter eksponuje całą ramkę w tym samym okresie. Rolling shutter odczytuje wiersze kolejno, więc każdy ma inny czas. Podczas obrotu prosta linia się wygina, a jednoznaczny timestamp ramki nie opisuje wszystkich pikseli.

W prostym sensorze o małej rozdzielczości efekt może być ograniczony przez krótki readout, ale trzeba sprawdzić datasheet. W algorytmie kamerowym kompensacja rolling shutter wymaga czasu wiersza i trajectory/gyro w czasie ekspozycji.

Timestamp powinien odnosić się do środka ekspozycji lub integracji, nie chwili odebrania pakietu przez autopilot.

Jakość pomiaru#

Pole quality jest wskazówką producenta, nie uniwersalnym prawdopodobieństwem poprawności. PX4 normalizuje quality 0…255, lecz znaczenie progu zależy od sensora i firmware.

Quality może odzwierciedlać:

liczbę dopasowanych cech;
peak ratio korelacji;
residual lub variance;
jasność/kontrast;
zgodność wektorów;
saturation lub motion limit.

Wysoka wartość nie wykrywa wszystkich błędów systematycznych. Kamera może bardzo pewnie śledzić poruszający się przenośnik, wodę albo cień i raportować prędkość powierzchni zamiast UAV.

Estymator powinien sprawdzać innovation i consistency z IMU, GNSS lub inną prędkością, a nie tylko quality threshold. PX4 wymaga quality powyżej EKF2_OF_QMIN, lecz dodatkowo działa logika estymatora.

Pole widzenia i footprint#

Dla płaskiego gruntu przy wysokości h footprint kamery rośnie:

W = 2h tan(FOV_x/2)
H = 2h tan(FOV_y/2)

Większy footprint widzi więcej tekstury, ale każdy szczegół ma mniej pikseli. Przy małej wysokości obraz zmienia się szybko i może przekroczyć flow range. Powstaje kwalifikowane pasmo wysokości, zależne od tekstury, prędkości i światła.

Oficjalna dokumentacja PX4 wspomina, że niska rozdzielczość typowych sensorów może powodować powolne oscylacje zawisu na większej wysokości, rzędu powyżej 20 m; to obserwacja dla klasy sensorów, nie gwarantowana granica.

Geometria montażu#

Sensor powinien patrzeć w dół bez zasłonięcia przez podwozie i przewody. Obiektyw musi być równoległy do założonej płaszczyzny body albo jego pełna rotacja musi być znana.

Montaż obejmuje:

yaw orientation;
roll/pitch boresight;
lever arm od środka obrotu;
pole widzenia dalmierza;
ochronę przed kurzem bez odbić;
sztywność względem IMU;
doświetlenie bez bezpośredniego glare.

Offset położenia ma znaczenie podczas obrotu:

v_sensor = v_body + ω × r

ArduPilot udostępnia FLOW_POS_X/Y/Z, a PX4 EKF2_OF_POS_X/Y/Z. Jeśli sensor jest daleko od środka, yaw generuje translację jego focal point nawet po odjęciu rotation flow.

Miękka wibroizolacja może wprowadzić zmienny boresight oraz opóźniony ruch względem IMU. Kamera zwykle nie wymaga takiej samej izolacji jak żyroskop; sztywność i brak blur są ważniejsze.

Interfejs i format#

W MAVLink PX4 oczekuje OPTICAL_FLOW_RAD, a odległość może przyjść przez DISTANCE_SENSOR. Dostępne są też I²C, SPI, DroneCAN i własne interfejsy modułów.

Komunikat powinien zawierać:

integrated flow w radianach;
delta angle z lokalnego gyro, jeśli dostępny;
integration timespan;
distance i jej ważność;
quality;
max flow rate;
min/max ground distance;
timestamp sample;
device ID i error count.

Pełna kamera przesyła obrazy do companion computera, który publikuje wynik. Dedykowany sensor liczy flow lokalnie i wysyła kilka bajtów. Drugi wariant ma mniejsze bandwidth i latency, pierwszy daje większą kontrolę oraz możliwość debugowania obrazów.

Synchronizacja z IMU#

Najlepszy moduł ma własny żyroskop zsynchronizowany z ekspozycją. Jeśli używa gyro flight controllera, trzeba znać offset zegarów i transport delay.

Budżet czasu:

t_sample = środek okresu integracji obrazu
t_publish = t_sample + exposure/readout + processing + transport

Estymator powinien cofać pomiar do t_sample w buforze stanów. Parametr stałego delay poprawia średni offset, lecz nie jitter. ArduPilot dokumentuje EK3_FLOW_DELAY jako część setupu.

Test synchronizacji polega na czystym oscylacyjnym roll/pitch bez translacji. Po kompensacji flow powinien być bliski zera i bez charakterystycznej pętli fazowej. Korelacja residual z gyro ujawnia delay.

Fuzja w EKF#

Optical flow obserwuje prędkość poziomą względem lokalnej powierzchni. EKF przewiduje ją z IMU i porównuje z pomiarem. Innovation:

ν = z_flow - h(x, range, attitude, extrinsics)

Covariance pomiaru powinna rosnąć przy małym quality, dużej wysokości, tilt, blur i niepewnym range. Stały mały noise powoduje, że filtr ufa flow również poza obwiednią.

Flow nie daje absolutnej pozycji ani yaw w jednolitej scenie. Bias prędkości integruje się do dryfu pozycji. Kompas, GNSS, VIO, beacon albo constraint misji może dostarczyć pozostałe obserwacje.

Przejście GNSS–flow musi kontrolować frame, offset pozycji i covariance. Optical flow może utrzymać ciągłość prędkości po utracie GNSS, ale nie zachowuje globalnej dokładności bez innego źródła.

Flow a visual odometry#

Downward optical flow + rangefinder daje głównie planar velocity przy założeniu powierzchni. Visual odometry śledzi wiele cech i estymuje pełniejszy ruch kamery; VIO dodatkowo łączy IMU.

Cecha	Optical flow + range	VIO
wynik	prędkość 2D	pose/prędkość 3D
compute	mały	większy
skala	rangefinder	stereo/depth/IMU/model
scena	grunt pod UAV	otoczenie w FOV
konfiguracja	prosta	kalibracja camera–IMU i companion
mapowanie	nie	możliwe jako warstwa wyższa

PX4 wskazuje, że optical flow ma mniej stanów i może być bardzo solidny w kontrolowanym wnętrzu, a VIO jest bardziej elastyczne, ale kosztowniejsze i trudniejsze w integracji.

Ruchoma powierzchnia#

Sensor mierzy względny ruch tekstury. Nad taśmociągiem, płynącą wodą, samochodem lub poruszaną matą wynik zawiera ruch powierzchni:

v_measured ≈ v_UAV - v_surface

Nie da się rozdzielić składników z samego flow. Fuzja z IMU nie wystarczy dla stałej prędkości powierzchni, bo akcelerometr po transient nie obserwuje prędkości. Potrzebne jest inne źródło lub ograniczenie środowiska.

Cień także może poruszać się po statycznym gruncie. Algorytm dopasowujący intensywność może go śledzić mimo braku ruchu materiału.

Wysokość i nachylenie terenu#

Dalmierz daje slant range. Na płaskim gruncie:

h_vertical = r cos(tilt)

Jednak flow skaluje się głębokością dla kierunków obrazu. Przy dużym tilt prosta korekta cos nie opisuje pełnego homography. Estymator powinien ograniczyć fusion do sprawdzonego kąta.

Przelot nad krawędzią stołu zmienia jednocześnie range i dominant texture. Dalmierz może przeskoczyć wcześniej lub później niż pole obrazu, tworząc chwilowy błąd skali. Quality flow może pozostać wysokie. Test edge transition jest konieczny.

Kalibracja#

Osi i znaku#

Przesunąć platformę do przodu, w prawo i obrócić wokół roll/pitch. Porównać z tabelą interfejsu. Motor pozostaje wyłączony.

Skali#

Na znanej wysokości przesunąć sensor po prowadnicy o znany dystans i prędkość. Powtórzyć w obu osiach, kierunkach, kilku wysokościach oraz temperaturach.

Boresight#

Stały przechył sensora miesza prędkość i wpływa na relation z dalmierzem. Wyznaczyć rotację do body, nie tylko yaw.

Czasu#

Wymusić oscylację rotacyjną i minimalizować residual flow po gyro compensation. Sprawdzić start, różne frame rates i obciążenie magistrali.

Quality/noise model#

Zebrać dane nad biblioteką powierzchni i oświetlenia. Wyznaczyć błąd względem ground truth jako funkcję quality, height, speed i exposure. Threshold pochodzi z ROC/mission tradeoff, nie z jednej próby.

Diagnostyka w logach#

Log powinien zawierać:

raw integrated flow X/Y;
integrated gyro X/Y/Z;
integration time;
range, quality i mode;
timestamp sample i receive;
compensated flow oraz velocity;
innovation, innovation variance i test ratio;
attitude, body rates i height;
saturation/error counters;
parametry scale/orientation/offset.

Typowe wzorce:

residual skorelowany z gyro — delay, scale lub osie;
prędkość rośnie proporcjonalnie do wysokości mimo zawisu — flow bias;
skok przy krawędzi — mismatch range/footprint;
quality spada przy wzroście RPM — blur, cień lub zasilanie;
poprawa po włączeniu światła — niedoświetlenie;
jedna oś ma inny gain — ogniskowa/skala lub montaż;
oscylacje tylko wysoko — kwantyzacja i zbyt duże zaufanie EKF.

Failsafe#

Utrata flow może być stopniowa. Reakcję wyzwala quality, innovation, stale dane, przekroczenie height/rate oraz brak range.

Możliwe strategie:

przejście do GNSS/VIO, jeśli spójne;
ograniczenie prędkości i tilt;
tryb attitude/altitude bez position hold;
kontrolowane lądowanie w zakwalifikowanym środowisku;
przerwanie autonomicznego zadania.

Nie należy utrzymywać ostatniej prędkości jako bieżącego pomiaru. Timestamp i sequence counter wykrywają zamrożony sensor.

ArduPilot ogranicza w określonej konfiguracji wznoszenie ponad maksymalny zasięg rangefindera, gdy optical flow jest jedynym źródłem poziomej pozycji/prędkości. To przykład powiązania obwiedni sensora z logiką lotu.

Plan prób#

Stanowisko obrazowe#

Przygotować matowe plansze: drobna tekstura, duża kratka, jedna krawędź, powierzchnia gładka, ciemna, błyszcząca. Regulować światło, wysokość i prędkość prowadnicy. Mierzyć bias, RMS, dropout i quality.

Rotacja#

Obracać sensor wokół znanej osi bez translacji. Sprawdzić znaki, scale gyro/flow, delay oraz max rate. Powtórzyć z rolling-shutter profile, jeśli używana jest kamera.

Ruch z rangefinderem#

Wykonać ten sam profil na kilku wysokościach. Estymowana prędkość powinna być niezależna od height w granicach budżetu. Następnie przejechać nad krawędzią i stokiem.

Warunki lotu#

Testy zaczynają się nisko, z dobrym światłem i ręcznym przejęciem. Rozszerza się speed, yaw rate, tilt, wysokość i bibliotekę powierzchni. PX4 publikuje osobną test card MC_06 dla lotu bez innych źródeł pozycji; własny plan powinien dodatkowo mieć ground truth oraz warunki odrzucenia.

Fault injection#

Zasłonić kamerę, odłączyć rangefinder, zamrozić pakiety, dodać delay, zmienić quality i przestawić skalę. Zweryfikować reakcję EKF i trybu lotu bez ryzykownego swobodnego lotu — najpierw SITL/HIL i uwięź.

Kryteria odbioru#

Podsystem jest gotowy, gdy:

osie i znaki są potwierdzone w translacji oraz rotacji;
skala X/Y mieści się w budżecie na kilku wysokościach;
residual po gyro compensation nie koreluje z body rate;
delay i jitter są zmierzone;
quality/noise model odrzuca niekwalifikowane sceny;
range oraz flow dotyczą tej samej powierzchni w obwiedni;
max speed, rate, tilt, height i light mają jawne limity;
po utracie tekstury lub dalmierza działa failsafe;
position drift jest zmierzony na trajektorii zamkniętej;
konfiguracja, firmware i biblioteka powierzchni są zapisane.

Odbiór nie polega wyłącznie na stabilnym zawisie nad jedną wykładziną. Taki test może ukryć bias, który ujawni się nad inną wysokością, światłem albo ruchomą powierzchnią.

Dobór modułu#

Parametr	Znaczenie
sensor/algorytm	zakres ruchu i dostęp do raw data
FOV i ogniskowa	footprint oraz skala
frame/integration rate	latency i max angular flow
exposure/shutter	blur i kompensacja czasu
min/max height	obwiednia optyki oraz range
quality definition	polityka fuzji
wbudowany gyro	synchronizacja obrotu
wbudowany rangefinder	spójność skali
interfejs	latency, długość przewodu, diagnostyka
masa i moc	integracja małego UAV
oświetlenie aktywne	lot w ciemności
sterownik i licencja	utrzymanie firmware

Bitcraze Flow deck v2 waży według producenta 1,6 g, ma PMW3901 i ToF VL53L1x do 4 m w warunkach zależnych od powierzchni oraz światła. To przykład lekkiego modułu dla małego UAV, nie ogólna obwiednia wszystkich sensorów.

Najczęstsze błędy#

Interpretacja flow counts jako metrów bez range.
Użycie wysokości barometrycznej zamiast odległości do powierzchni.
Brak kompensacji obrotu lub różny przedział integracji gyro.
Zamiana osi pikseli, body i NED.
Kalibracja tylko ręcznym przesunięciem bez ground truth.
Jeden globalny próg quality dla wszystkich wysokości.
Brak limitu max flow rate i ekspozycji.
Założenie, że wysoka jakość oznacza nieruchomy grunt.
Ignorowanie moving texture, wody i cieni.
Timestamp odbioru zamiast akwizycji.
Miękki uchwyt zmieniający ekstrynsykę.
Rangefinder patrzący na inną powierzchnię niż kamera.
Integracja prędkości jako globalnej pozycji bez kontroli dryfu.

Powiązane tematy#

Lidar w UAV — skala odległości i geometria powierzchni.
IMU w dronie — kompensacja rotation flow i predykcja EKF.
GNSS w UAV — przejścia źródeł prędkości i globalna pozycja.
Visual odometry w UAV — pełniejsza estymacja ruchu z obrazu.
SLAM dla UAV — lokalizacja oraz mapa.
Terrain following UAV — odległość do gruntu i obwiednia lotu.
Filtr Kalmana w UAV — innovation, covariance i gating.
Układy współrzędnych UAV — camera, body, FRD i NED.

Przypisy#

Bibliografia jest generowana z centralnego rejestru. Konwencje komunikatu i warunki fuzji pochodzą z bieżącej dokumentacji PX4, konfiguracja i testy z ArduPilot, przykład lekkiego modułu z Bitcraze, a równanie jasności i Lucas–Kanade z dokumentacji OpenCV. Limity wysokości, światła i prędkości muszą być zweryfikowane dla dokładnego sensora, obiektywu, rangefindera i firmware.

Źródła z centralnego rejestru

PX4 Guide: Optical Flow — setup, conventions and estimator integration [dokumentacja projektu]
PX4 SensorOpticalFlow message specification [specyfikacja interfejsu]
ArduPilot: Optical Flow Sensor Testing and Setup [dokumentacja projektu]
Bitcraze Flow deck v2 — optical flow and ToF module [dokumentacja producenta]
OpenCV: Optical Flow and Lucas–Kanade method [dokumentacja projektu]
Sumit Sharma, „Drone Development from Concept to Flight” [książka]
Ty Audronis, „Drony. Wprowadzenie” [książka]