Nawigacja waypoint UAV

Nawigacja waypointowa nie polega na skierowaniu UAV kolejno na współrzędne zapisane w tabeli. Lista elementów misji jest programem: zawiera ramy współrzędnych, warunki ukończenia, polecenia zmiany prędkości i orientacji, akcje ładunku, pętle, lądowanie oraz reguły wznowienia. Autopilot przekształca ten program w ciąg wykonalnych trajektorii i setpointów, a jednocześnie pilnuje geofence, jakości pozycji, zapasu energii i failsafe.

Dwa identyczne zestawy szerokości i długości geograficznych mogą dać inny tor w zależności od typu platformy, wersji firmware, acceptance radius, sposobu wygładzania zakrętów, odniesienia wysokości i komend pomiędzy punktami. Dlatego misja powinna być wersjonowanym artefaktem przypiętym do profilu statku i sprawdzonym w symulacji, a nie plikiem przenoszonym bez kontroli między różnymi autopilotami.

Spis treści#

Waypoints jako program misji
Warstwy wykonania
Model elementu misji
Układy współrzędnych
Wysokość waypointu
Planned home i actual home
Od punktu do segmentu
Acceptance radius i pass radius
Przejście przez zakręt
Prędkość przyspieszenie i jerk
Profil pionowy
Yaw kierunek lotu i ROI
Komendy NAV DO i CONDITION
Pętle i skoki
Akcje payloadu
MAVLink Mission Protocol
Atomiczny upload i read back
Walidacja przed lotem
Geofence przeszkody i terrain
Budżet energii i łączności
Maszyna stanów wykonawcy
Pauza wznowienie i restart
Zmiana misji w locie
Utrata pozycji i failsafe
Multirotor fixed wing i VTOL
QGroundControl i Mission Planner
Logowanie i metryki
SITL HIL i fault injection
Kryteria odbioru
Typowe błędy
Powiązane tematy
Przypisy

Waypoints jako program misji#

Waypoint jest punktem lub stanem docelowym, ale misja zawiera znacznie więcej. Typowy przebieg może wyglądać następująco:

preflight checks
  → takeoff
  → change speed
  → waypoint A
  → start camera interval
  → survey segments
  → stop camera interval
  → loiter
  → return or landing pattern
  → land

Każdy element ma typ komendy, parametry, ramę współrzędnych i indeks. Niektóre zmieniają ruch, inne konfigurują kamerę albo czekają na warunek. Kolejność elementów jest częścią semantyki. Przesunięcie start camera interval o jeden indeks może zmienić kompletność danych, nawet jeśli trasa jest identyczna.

Misję należy rozpatrywać jako program deterministyczny dla określonego środowiska wykonawczego:

mission_behavior = f(
    mission_items,
    firmware_build,
    vehicle_type,
    parameters,
    home/origin,
    terrain_data,
    estimator_state,
    geofence,
    failsafe_config
)

Sam plik z punktami nie wystarcza do odtworzenia lotu. Raport operacyjny powinien przechowywać również build firmware, hash parametrów, wersję geofence i planowane home.

Warstwy wykonania#

W typowym autopilocie misja przechodzi przez kilka warstw:

mission storage
  → mission interpreter
  → navigation state / triplet
  → path and trajectory generator
  → position/velocity controller
  → attitude/rate controller
  → mixer and actuators

Interpreter wybiera aktywny element i decyduje, kiedy został ukończony. Generator trasy wyznacza linię, łuk, spline, orbitę albo profil lądowania. Kontroler pozycji śledzi referencję w granicach prędkości, przyspieszenia i jerk. Warstwa failsafe może w każdej chwili przejąć sterowanie.

Companion computer nie powinien zakładać, że current mission item = 7 oznacza znajdowanie się w punkcie 7. Może to znaczyć lot do niego, oczekiwanie, wykonywanie loiter albo obsługę skojarzonej komendy. Potrzebne są osobne stany: active item, navigation target, reached, command result i mission result.

Model elementu misji#

MAVLink Mission Protocol przenosi uporządkowane elementy typu mission, fence albo rally. Dla misji każdy element zawiera m.in. numer sekwencyjny, komendę MAV_CMD, coordinate frame, flagę current/autocontinue i siedem parametrów zależnych od komendy.[1]

Uproszczony model wewnętrzny:

MissionItem {
    sequence
    command
    frame
    params[1..4]
    x_or_lat
    y_or_lon
    z_or_alt
    autocontinue
}

Znaczenie param1...param4 nie jest wspólne dla wszystkich komend. Dla MAV_CMD_NAV_WAYPOINT specyfikacja opisuje hold time, acceptance radius, pass radius i yaw. Dla loiter te same pola oznaczają inne wielkości. Parser musi wybierać schemat na podstawie identyfikatora komendy, sprawdzać jednostki, zakresy oraz NaN używane jako „nie podano”, jeśli specyfikacja na to pozwala.[2]

Nie każda komenda z common dialect jest obsługiwana przez każdy firmware i każdy typ statku. GCS powinien filtrować listę na podstawie profilu pojazdu, a autopilot jawnie odrzucać komendę nieobsługiwaną. Ciche pominięcie zmienia program bez zgody operatora.

Układy współrzędnych#

Globalny waypoint najczęściej używa szerokości i długości WGS84. W MAVLink przy wariancie MISSION_ITEM_INT pola całkowite przechowują lat/lon przeskalowane przez 1e7, co ogranicza błąd kwantyzacji i jest preferowane względem pól zmiennoprzecinkowych dla współrzędnych globalnych.[1][2]

Rama definiuje nie tylko osie, ale także pionowe odniesienie:

Rama MAVLink	Poziom	Wysokość
`MAV_FRAME_GLOBAL`	WGS84 lat/lon	względem MSL
`MAV_FRAME_GLOBAL_RELATIVE_ALT`	WGS84 lat/lon	względem home
`MAV_FRAME_GLOBAL_TERRAIN_ALT`	WGS84 lat/lon	względem terenu
`MAV_FRAME_LOCAL_NED`	north/east od lokalnego origin	down od origin

Nazwy z sufiksem _INT zostały w specyfikacji zrównane lub zastąpione odpowiednimi nazwami globalnymi dla użycia z całkowitym wariantem komunikatu; starszy firmware i biblioteki mogą nadal eksponować historyczne identyfikatory.[2] Profil zgodności musi uwzględniać rzeczywistą implementację.

Lokalny NED jest wygodny w laboratorium i wewnątrz budynku, ale jego origin pochodzi z estymatora. Restart, relokalizacja VIO albo zmiana origin może zmienić globalne znaczenie punktu. Misja lokalna potrzebuje procedury kotwiczenia i testu ciągłości.

Transformacje ECEF, ENU i NED opisano szerzej w Układach współrzędnych w UAV.

Wysokość waypointu#

Wysokość jest najczęstszym źródłem niebezpiecznej niejednoznaczności. GCS może wyświetlać 80 m relative, teren może mieć 150 m AMSL, a autopilot przechowywać lokalny z. Konwersja wymaga znanego home, modelu geoidy i danych terenu.

Przykład:

h_waypoint_AMSL = h_home_AMSL + h_relative
h_waypoint_ellipsoid = h_waypoint_AMSL + geoid_separation
h_AGL = h_waypoint_AMSL - terrain_AMSL(lat, lon)

Każda wielkość ma niepewność. home_AMSL może pochodzić z GNSS i barometru, terrain_AMSL z siatki DEM, a geoid separation z modelu o określonej rozdzielczości. Nie wolno prezentować syntetycznego profilu jako dokładniejszego niż źródła.

Terrain frame bywa użyteczny nad zmiennym terenem, ale wymaga dostępności kafli i jawnej reakcji na brak danych. Sama wysokość AGL w waypointach nie zapewnia śledzenia profilu między punktami. Segment może przechodzić nad wzniesieniem. Potrzebny jest look-ahead i odpowiednio gęsty profil opisany w Terrain following UAV.

Przed uploadem GCS powinien zapisać frame każdego elementu. Po read-back trzeba porównać nie tylko liczby, ale też ramę. Zmiana GLOBAL_TERRAIN_ALT na GLOBAL_RELATIVE_ALT przy zachowaniu z=80 jest semantyczną korupcją.

Planned home i actual home#

Planowane home w GCS służy do rysowania profilu, obliczania wysokości względnych, czasu i energii. Rzeczywiste home jest ustawiane przez pojazd, zwykle przy uzbrojeniu albo po uzyskaniu wymaganej pozycji. QGroundControl wyraźnie rozróżnia expected home od actual home.[6]

Jeśli statek zostanie uruchomiony kilometr od miejsca planowania, waypoint relative-alt może nadal mieć tę samą wartość względem nowego home, ale inny AMSL. Pierwszy segment może stać się zbyt długi, a profil przecinać teren.

Procedura przedstartowa powinna:

odebrać actual home i jego jakość;
porównać dystans oraz różnicę wysokości z planned home;
ponownie przeliczyć wszystkie ramy relative/terrain;
uruchomić feasibility check dla aktualnej pozycji;
zablokować start albo wymagać jawnego zatwierdzenia przy przekroczeniu limitu;
zapisać obie pozycje w logu.

Zmiana home w trakcie misji wpływa na RTL i może wpływać na elementy relative, zależnie od sposobu ich interpretacji i przechowywania w firmware. Nie należy wysyłać DO_SET_HOME bez testu konkretnego stosu.

Od punktu do segmentu#

Autopilot nie teleportuje się między punktami. Element A → B tworzy segment lub krzywą, którą generator trajektorii ma śledzić. Potrzebne są:

cross-track error względem trasy;
along-track progress;
remaining distance;
desired course/path tangent;
profil prędkości;
przewidywane przejście do następnego segmentu;
kryterium osiągnięcia celu.

Dla lokalnego odcinka od a do b:

u = b - a
s = clamp(dot(p-a, u) / dot(u,u), 0, 1)
p_nearest = a + s · u
cross_track = norm(p - p_nearest)
along_track = s · norm(u)

W globalnej skali krótki segment można przeliczyć do lokalnej płaszczyzny stycznej. Dla długich odcinków trzeba określić, czy trasa jest geodezyjną, rhumb line czy projekcją GCS. Niejawna różnica może dać zauważalne odchylenie.

Generator powinien znać poprzedni, bieżący i następny element. Dzięki temu może wykonać fly-through zamiast zatrzymywania w każdym punkcie. Jeśli następny element wymaga zatrzymania, lądowania albo zmiany typu toru, przejście może być inne.

Acceptance radius i pass radius#

MAV_CMD_NAV_WAYPOINT definiuje w common dialect m.in. acceptance radius oraz pass radius.[2] Acceptance radius jest odległością, w której punkt może zostać uznany za osiągnięty. Pass radius określa sposób przejścia obok punktu i może kodować kierunek obejścia. Rzeczywista obsługa parametrów różni się między stosami.

Kryterium oparte wyłącznie na kuli:

reached = norm(position - waypoint) <= acceptance_radius

ma wady. Szybki statek może przeskoczyć obszar między próbkami. Wiatr może uniemożliwić wejście w mały promień mimo bezpiecznego minięcia punktu. Wysokość może wymagać osobnego progu. Dlatego implementacje używają również płaszczyzny przejścia, postępu po trasie i logiki dynamicznej.

ArduPilot Copter dokumentuje własną semantykę: część pól standardowej komendy waypoint nie jest używana, a zachowanie promienia i płynnego przejścia wynika z parametrów nawigacyjnych stosu. Dokumentacja ostrzega, że w locie przelotowym wirtualny punkt prowadzony przez generator może osiągnąć waypoint wcześniej niż pojazd.[4] Nie można przenieść wartości z PX4 do ArduPilot bez sprawdzenia.

Za mały acceptance radius powoduje oscylację albo długie oczekiwanie. Za duży może pominąć obszar wymagany dla pomiaru. Wymóg powinien wynikać z celu: na trasie tranzytowej liczy się ciągłość, przy punkcie kalibracyjnym rzeczywiste wejście w pozycję i stabilizacja.

Przejście przez zakręt#

Trasa złożona z ostrych odcinków ma nieskończone wymagane przyspieszenie w wierzchołku. Generator zastępuje narożnik łukiem, krzywą S, spline albo zatrzymaniem.

Dla prędkości v i dopuszczalnego przyspieszenia poprzecznego a_lat minimalny promień kinematyczny:

R_min = v² / a_lat

Dla fixed-wing przy koordynowanym zakręcie:

R ≈ V_air² / (g · tan(bank))

Wiatr zmienia ground track i promień względem mapy. Dla multirotora limit tilt musi zostawić część thrust na utrzymanie wysokości. Przy ciężkim ładunku albo niskim napięciu dostępne przyspieszenie maleje.

Krzywa wygładzająca może wyjść poza wielokąt geofence albo nad przeszkodę, mimo że oba waypointy są poprawne. Feasibility check powinien generować albo konserwatywnie otaczać faktyczną trajektorię, a nie tylko sprawdzać odcinki.

Spline poprawia ciągłość, ale zmienia lokalizację względem linii między punktami. ArduPilot opisuje MAV_CMD_NAV_SPLINE_WAYPOINT dla Coptera i zachowanie przy mieszaniu spline ze zwykłymi waypointami; dostępność dla innych pojazdów jest odmienna.[4] GCS musi wyświetlać tor zgodny z target firmware.

Prędkość przyspieszenie i jerk#

Misja może zawierać zmianę prędkości, lecz polecenie nie tworzy automatycznie wykonalnego profilu. Generator musi uwzględnić:

prędkość cruise i maksymalną;
prędkość poziomą i pionową;
przyspieszenie oraz hamowanie;
jerk;
ograniczenie tilt/bank;
prędkość powietrzną dla fixed-wing;
wiatr i oczekiwaną ground speed;
masę i zapas thrust;
prędkość wymaganą przez sensor/payload.

Profil trapezowy przyspiesza, utrzymuje prędkość i hamuje. Profil S ogranicza jerk, co zmniejsza pobudzenie konstrukcji i poprawia jakość obrazowania. Na krótkim segmencie statek może nigdy nie osiągnąć zadanej cruise speed.

Droga hamowania w prostym modelu:

d_brake = v² / (2 · a_brake) + v · t_latency

Jeśli odległość do waypointu jest mniejsza, generator musi wcześniej obniżyć prędkość albo zastosować fly-through. Późne polecenie hold nie może łamać ograniczeń dynamiki.

Komenda zmiany prędkości ma stan utrzymujący się do kolejnej zmiany lub resetu, zależnie od stosu. Wznowienie misji powinno odtworzyć aktywny profil, a nie tylko skoczyć do kolejnego waypointu. PX4 dokumentuje zachowanie związane z ponownym zastosowaniem ustawień prędkości i triggera kamery po wznowieniu określonych misji obrazujących.[3]

Profil pionowy#

Pionowa trajektoria nie może być prostym interpolowaniem wysokości bez sprawdzenia możliwości wznoszenia. Dla segmentu o długości poziomej L, ground speed v_g i różnicy wysokości dh:

t_segment ≈ L / v_g
required_climb_rate = dh / t_segment

Jeśli required_climb_rate przekracza bezpieczny limit, trzeba wydłużyć trasę, zmniejszyć prędkość poziomą albo rozpocząć zmianę wcześniej. Fixed-wing dodatkowo ma ograniczenie kąta toru i margines nad przeciągnięciem. Multirotor przy wznoszeniu zużywa więcej mocy i może osiągnąć limit thrust.

Segment opadający wymaga sprawdzenia sink rate, VRS dla multirotora, energii dla fixed-wing i clearance nad terenem. z waypointu nie opisuje całego profilu między punktami. Planner powinien próbkować terrain/obstacles i dodać margines błędu DEM, pozycji i śledzenia.

Przy przejściu relative-alt → terrain-alt nie powinno być skoku. Konwerter musi wyliczyć wspólną reprezentację AMSL i sprawdzić ciągłość. Gdy terrain data jest niedostępna, nie wolno zastąpić jej zerem.

Yaw kierunek lotu i ROI#

Multirotor może utrzymywać yaw niezależnie od kierunku prędkości. Fixed-wing zwykle orientuje nos wzdłuż prędkości powietrznej, więc osobny yaw waypoint ma ograniczone znaczenie. MAV_CMD_NAV_WAYPOINT w common dialect ma parametr yaw dla rotary-wing, ale NaN może oznaczać pozostawienie bieżącej polityki heading.[2] ArduPilot Copter dokumentuje, że pole yaw tej komendy nie jest obsługiwane w sposób oczekiwany z ogólnej definicji i zaleca osobne CONDITION_YAW.[4]

ROI może skierować kamerę/gimbal albo cały statek na punkt. Trzeba rozdzielić:

body yaw;
camera optical axis;
gimbal yaw/pitch;
course over ground;
track tangent;
kierunek względem wiatru.

Komenda ROI może utrzymywać stan przez wiele kolejnych elementów, dopóki nie zostanie wyzerowana lub zastąpiona. GCS powinien pokazywać zakres obowiązywania. Po pauzie i wznowieniu stan ROI musi być znany.

W misjach fotogrametrycznych gwałtowne obroty body zwiększają blur i zakłócają overlap. Profil yaw powinien mieć własne limity rate/acceleration i być zsynchronizowany z triggerem kamery.

Komendy NAV DO i CONDITION#

ArduPilot rozróżnia komendy nawigacyjne, DO oraz CONDITION. Dokumentacja Coptera wskazuje, że jednocześnie może wykonywać jedną komendę NAV i jedną DO, a nieukończone DO/CONDITION mogą zostać pominięte po przejściu do kolejnego NAV.[4] To ważna semantyka czasowa.

Przykład:

NAV_WAYPOINT A
CONDITION_DISTANCE 30 m
DO_SET_ROI target
DO_CHANGE_SPEED 4 m/s
NAV_WAYPOINT B

CONDITION_DISTANCE nie zatrzymuje statku; opóźnia wykonanie DO do spełnienia warunku względem kolejnego waypointu. Jeśli NAV zakończy się wcześniej, powiązane akcje mogą nie zostać uruchomione. Projektant nie może traktować listy jak jednowątkowego skryptu, w którym każda linia zawsze czeka na poprzednią.

Bezpieczny edytor powinien wizualizować zakres komend modalnych i zależności:

item 4 speed=5 m/s ────────────────┐
item 5 waypoint                    │ active state
item 6 start capture ──────────┐   │
item 7 waypoint                │   │
item 8 stop capture ───────────┘   │
item 9 speed=8 m/s ────────────────┘

Nieobsługiwana komenda powinna zatrzymać walidację. Ostrzeżenie „ignored” może być dopuszczalne tylko dla parametru jawnie opcjonalnego i po akceptacji użytkownika.

Pętle i skoki#

DO_JUMP oraz tagi pozwalają powtarzać fragment misji. Tworzą graf sterowania, a nie prostą listę. Walidator musi wykrywać:

skok do nieistniejącego indeksu lub tagu;
nieskończoną pętlę;
bardzo dużą liczbę iteracji;
pominięcie lądowania;
kumulację energii i czasu;
stan modalnych komend po powrocie;
różne znaczenie licznika po pauzie, zmianie itemu i restarcie.

PX4 dokumentuje, że ręczne ustawienie bieżącego itemu nie resetuje licznika Jump to item; pełny reset misji następuje w opisanych warunkach, m.in. po rozbrojeniu lub uploadzie nowej misji.[3] ArduPilot ma własne parametry reset/resume zależne od pojazdu.[5] Interfejs GCS powinien pokazywać nie tylko indeks, lecz również stan liczników.

Przed lotem warto rozwinąć graf do maksymalnej liczby iteracji i obliczyć upper bound czasu, drogi oraz energii. Pętla nieskończona może być celowa dla patrolu, ale wymaga jawnego warunku przerwania: baterii, czasu, komendy operatora albo failsafe.

Akcje payloadu#

W zastosowaniach cywilnych typowe akcje to wyzwalanie kamery, start/stop nagrania, sterowanie gimbalem, pobieranie próbki, włączenie oświetlenia lub rozpoczęcie pomiaru. Kluczowa jest synchronizacja z pozycją i czasem.

Trigger oparty na odległości powinien korzystać z along-track, a nie wyłącznie z prędkości razy czas. Przy wietrze i zakrętach odstępy czasowe nie odpowiadają równym odstępom naziemnym. Dla kamery trzeba uwzględnić:

latency komendy i shutter lag;
timestamp ekspozycji;
synchronizację GNSS/PPS lub trigger feedback;
czas zapisu i zajętość bufora;
minimalny odstęp między zdjęciami;
stan triggera po pauzie;
geotag pozycji w czasie ekspozycji, nie odbioru pliku.

Misja powinna zawierać warunki zdrowia payloadu. Jeśli celem jest kompletna mapa, awaria kamery może uzasadniać przerwanie i Return zamiast kontynuowania pustego przelotu. Jeśli payload jest opcjonalny, lot może trwać, lecz zdarzenie musi zostać oznaczone.

Nie należy sterować krytycznym urządzeniem przez ogólną komendę serwa bez potwierdzenia stanu. Warstwa payload powinna raportować accepted, executing, completed albo failed, a interpreter misji mieć politykę timeoutu.

MAVLink Mission Protocol#

Mission Protocol definiuje niezawodny dialog na zawodnym łączu. GCS wysyła liczbę elementów, autopilot żąda konkretnych indeksów, a po zakończeniu zwraca ACK. Pobieranie zaczyna się od list request i count, po czym odbiorca żąda pozycji. Protokół obsługuje mission types, aby rozdzielić mission, fence i rally.[1]

Ważne własności:

numer sekwencyjny identyfikuje element w bieżącej transakcji;
żądania mogą zostać ponowione po timeout;
duplikat powinien być idempotentny;
partnerzy muszą zachować ten sam mission type;
nieoczekiwany indeks lub nadawca nie powinien przejąć transferu;
rezultat końcowy ma odróżniać success od konkretnych klas błędu;
częściowy upload nie jest nową ważną misją.

MISSION_ITEM_INT jest preferowany dla precyzyjnych globalnych pozycji. Konwerter nie powinien przełączać się między wariantem float i int bez kontroli round-trip. Wszystkie parametry NaN, signed radius i frame muszą przetrwać.

Przepustowość nie jest jedynym ograniczeniem. Na łączu o dużym RTT timeout musi być dopasowany, a GCS powinien ograniczyć równoległe transakcje. QGroundControl dokumentuje diagnostykę upload/download oraz wpływ strat pakietów na niezawodność transferu.[6]

Atomiczny upload i read back#

Nowa misja powinna być aktywowana atomowo:

old mission remains active
        ↓ begin transaction
staging[expected_count]
        ↓ receive + validate all items
semantic/feasibility validation
        ↓ atomic commit
new active mission + revision + hash

Zerwanie łącza pozostawia poprzednią misję. Autopilot nie może wykonać pierwszych dziesięciu itemów nowego planu po utracie pozostałych. Jeżeli pamięć docelowa nie umożliwia pełnego stagingu, implementacja musi mieć inny mechanizm transakcyjny lub zabronić aktualizacji w locie.

Po ACK GCS powinien pobrać misję i wykonać semantyczny read-back:

liczba i kolejność elementów;
command ID;
frame;
wszystkie parametry po normalizacji;
współrzędne z dopuszczalnym błędem kwantyzacji;
autocontinue/current;
hash reprezentacji kanonicznej.

Porównanie bajtów pliku QGC z pamięcią MAVLink nie zawsze działa, bo GCS zawiera dodatkowe metadane i complex items. Potrzebna jest kanonizacja do wspólnego modelu. QGroundControl .plan jest JSON-em obejmującym misję oraz opcjonalnie geofence i rally; część wzorców złożonych jest rozwijana do elementów przeznaczonych dla pojazdu.[7]

Walidacja przed lotem#

Walidacja powinna działać w kilku poziomach.

Walidacja składni#

liczba elementów w limicie pamięci;
obsługiwane command i frame;
skończone wartości liczbowe;
lat/lon, wysokości, promienie i czasy w zakresie;
poprawne indeksy skoków;
kompletne required fields.

Walidacja semantyki#

właściwy początek i koniec dla typu pojazdu;
brak sprzecznych komend modalnych;
lądowanie ma pełny pattern, jeśli wymagany;
pętla ma limit lub jawny contingency;
akcje payloadu są domknięte;
wznowienie nie pozostawia nieznanego stanu.

Walidacja geometrii#

punkty, odcinki, łuki i spline mieszczą się w geofence;
clearance nad terenem i przeszkodami jest wystarczający;
minimalny promień zakrętu jest zachowany;
gradient pionowy jest wykonalny;
landing approach nie przecina przeszkód.

Walidacja zasobów#

energia z rezerwą na Return/contingency;
pamięć payloadu;
czas misji i okna operacyjne;
zasięg łącza, jeśli wymagany;
dostępność map/terrain i aktualność korekt.

PX4 wykonuje podstawowe mission feasibility checks przy uploadzie i przed startem, m.in. konflikt z geofence i wymagane elementy takeoff/land. Dokumentacja wskazuje również kontrolę odległości pierwszego waypointu, której skutek może mieć charakter ostrzeżenia zależny od ustawień.[3] Pokładowa walidacja jest ostatnią warstwą, ale nie zastępuje dokładniejszego planera operacyjnego.

Geofence przeszkody i terrain#

Misja i geofence muszą być sprawdzone we wspólnej reprezentacji. Nie wystarczy, że waypointy leżą wewnątrz. Trzeba ocenić pełny swept volume statku z footprintem, błędem pozycji i trackingiem.

W kolejności:

przelicz punkty i granice do spójnego układu;
wygeneruj nominalne segmenty i zakręty;
dodaj profil pionowy oraz terrain;
rozszerz tor o footprint i niepewność;
sprawdź inclusion/exclusion i floor/ceiling;
sprawdź alternatywną trasę Return;
zachowaj minimalny clearance i lokalizację konfliktu.

Szczegółowy model granic opisuje Geofencing UAV. Omijanie przeszkód w locie jest osobną warstwą. Planner lokalny może odchylić się od nominalnej trasy, ale nadal musi pozostać w korytarzu i wiedzieć, gdzie może bezpiecznie wrócić. Jeśli avoidance jest wyłączone, misja potrzebuje aktualnej mapy przeszkód lub operacji w kontrolowanym obszarze.

Terrain data powinny mieć wersję, rozdzielczość, vertical datum i coverage. Brak kafla na jednym segmencie nie może być interpretowany jako teren na poziomie zero. Możliwe reakcje to odrzucenie misji, przełączenie na konserwatywną wysokość AMSL albo lot wyłącznie w obszarze z niezależnym rangefinderem — zależnie od zatwierdzonej architektury.

Budżet energii i łączności#

Szacunek misji nie powinien być distance / cruise_speed. Potrzebuje osobnych modeli dla startu, wznoszenia, cruise, hover, zakrętów, transition VTOL, zniżania i rezerwy.

E_required = Σ(P_mode(state, wind, mass) · dt)
           + E_return_worst_case
           + E_contingency
           + reserve

Wiatr zwiększa czas odcinka pod wiatr i może uniemożliwić postęp fixed-wing mimo poprawnej airspeed. Dla multirotora hover power rośnie z masą, a lot szybki ma charakterystyczną krzywą mocy. Niska temperatura i starzenie obniżają użyteczną pojemność akumulatora.

Przed każdym odległym punktem można wykonać gate:

continue only if
  predicted_energy_to_finish
  + predicted_energy_to_safe_landing
  + reserve
  <= usable_energy_remaining

Łączność może być opcjonalna dla autonomicznej misji, ale regulacje lub procedura operacyjna mogą wymagać C2 przez cały czas. Link budget i coverage map powinny być powiązane z trasą. Failsafe po utracie łącza musi być zgodny z misją: kontynuacja, hold, Return albo land. Nie może jednocześnie obowiązywać „continue mission” w jednym komponencie i „immediate RTL” w drugim.

Maszyna stanów wykonawcy#

Interpreter misji powinien jawnie modelować stan:

NO_MISSION
  → VALIDATED
  → READY
  → STARTING
  → EXEC_NAV
  → EXEC_AUX
  → ITEM_REACHED
  → ADVANCE
  → COMPLETED

każdy stan → PAUSED / FAILSAFE / ABORTED

Dla każdego itemu potrzebne są:

entered_at;
warunki wejścia;
aktywne setpointy;
timeout;
warunek ukończenia;
skutki uboczne;
stan modalny po ukończeniu;
zachowanie przy pause/resume;
kod błędu.

autocontinue=false może oznaczać oczekiwanie po osiągnięciu elementu. Operator powinien widzieć, że statek nie utknął, lecz czeka na kontynuację. Timeout nie powinien być wspólną stałą: takeoff, camera ACK i loiter mają różne skale czasu.

Maszyna stanów musi być odporna na reset procesu. Jeśli stan jest utrwalany, trzeba atomowo zapisać indeks i modalne efekty. Jeśli nie jest, po restarcie autopilot powinien wykonać jawny failsafe zamiast zgadywać miejsce kontynuacji.

Pauza wznowienie i restart#

Pauza przerywa wykonanie, ale nie definiuje automatycznie bezpiecznej trajektorii wznowienia. Statek może oddalić się w hold, zostać zniesiony, ominąć przeszkodę lub operator może przejąć ręcznie.

Strategie wznowienia:

lecieć z bieżącej pozycji do aktywnego waypointu;
wrócić na najbliższy punkt poprzedniego segmentu;
cofnąć się o określoną odległość, aby zachować overlap;
wznowić od początku survey line;
pominąć ukończony item i przejść dalej;
ponownie wykonać akcje modalne;
wymagać zatwierdzenia po długiej pauzie.

PX4 dokumentuje, że misję można pauzować zmianą trybu i wznowić po powrocie do Mission. Dla misji z aktywnym triggerem obrazu zachowanie obejmuje powrót do poprzedniego waypointu i odtworzenie fragmentu, aby zachować pokrycie; stos ponownie stosuje ustawienia prędkości i kamery.[3] To zachowanie trzeba testować z wersją używaną w pojeździe.

ArduPilot dokumentuje odmienne zasady resetu wskaźnika misji dla Coptera, Rovera i Plane oraz parametry wpływające na restart przy wejściu do AUTO.[5] Nie istnieje przenośne „resume from item N” bez profilu stosu.

Przy wznowieniu należy ponownie sprawdzić:

pozycję i transformację origin;
aktualny geofence;
energię;
terrain data;
stan payloadu;
liczniki pętli;
aktywne speed/ROI/camera modes;
osiągalność segmentu powrotnego.

Zmiana misji w locie#

Aktualizacja planu w powietrzu jest możliwa w niektórych stosach, ale ma większe ryzyko niż upload przedstartowy. Należy ustalić, co dzieje się z bieżącym itemem, licznikami, stanem payloadu i trasą pomiędzy pozycją a nowym planem.

Bezpieczny workflow:

operator przygotowuje nową revision;
GCS waliduje ją wobec aktualnej pozycji, energii i fence;
pojazd przechodzi do Hold albo kontynuuje stary plan według polityki;
nowa misja jest wysyłana do stagingu;
autopilot wykonuje własne checks;
commit ma jawny punkt aktywacji;
wybrany start item i entry trajectory są zatwierdzane;
telemetria potwierdza revision/hash/current item.

Nie wolno rozpoczynać od itemu, którego warunki wstępne nie zostały wykonane. Jeśli nowa misja zakłada aktywną kamerę, ale pominięto start capture, stan jest błędny. Można generować synthetic entry actions albo wymagać startu od punktu inicjalizującego.

Rollback do poprzedniej misji również wymaga walidacji. Jej home, strefy lub terrain mogły się zdezaktualizować. „Poprzednia znana dobra” nie oznacza „obecnie wykonalna”.

Utrata pozycji i failsafe#

Mission mode wymaga pozycji w układzie odpowiadającym waypointom. PX4 dla misji multicoptera wymienia globalny stan 3D uzyskany bezpośrednio lub pośrednio z pozycji lokalnej jako wymaganie trybu.[3] Jakość pozycji musi być monitorowana także po starcie.

Przy degradacji:

position valid
  → degraded: reduce speed, enlarge margins
  → local continuity only: follow contingency if globally anchored
  → invalid: position-loss failsafe

Kontynuowanie przez kilka sekund z INS/VIO może być bezpieczniejsze niż natychmiastowy gwałtowny manewr, ale wymaga określonego time-to-limit. Powrót do GNSS może przesunąć globalną pozycję. Interpreter nie powinien wtedy arbitralnie uznać waypointu za osiągnięty na podstawie skoku.

Failsafe dla łącza, baterii, geofence, estymatora i silnika mogą wystąpić jednocześnie. Potrzebny jest arbiter priorytetów. Przykład: low battery żąda Return, lecz geofence-aware planner nie ma trasy; position loss uniemożliwia globalny Return; wtedy system musi wybrać lokalny contingency, a nie przełączać się w pętli między trybami.

Projektowanie przejść opisuje Failsafe jako maszyna stanów.

Multirotor fixed wing i VTOL#

Multirotor#

Może zatrzymać się w waypointcie, utrzymywać pozycję i niezależnie sterować yaw. Główne ograniczenia to thrust, tilt, prędkość pionowa, wiatr i czas hover. Mały promień acceptance jest możliwy, ale może zwiększać czas i zużycie energii.

Fixed wing#

Nie zatrzymuje się w punkcie. Waypoint jest elementem ścieżki, a osiągnięcie zwykle opiera się na geometrii przejścia. Loiter ma promień i kierunek. Landing wymaga patternu, heading, glide slope, airspeed i analizy wiatru. Koniec listy ma stosowo zależne zachowanie; dokumentacja ArduPilot wskazuje różnice między Copter, Rover i Plane.[5]

VTOL#

Ma dwa modele dynamiki i transition. Plan powinien jawnie oznaczać tryb segmentu. Transition potrzebuje dystansu, wysokości, prędkości i contingency w razie niepowodzenia. QGroundControl posiada ustawienia związane z odległością transition dla planu, ale finalną semantykę określa firmware i parametry pojazdu.[6]

UGV i USV#

Waypoint ma zwykle pozycję 2D i tolerancję. Kinematyka Ackermanna ogranicza skręt, UGV nie przejeżdża przez przeszkody mapy, a USV dryfuje od prądu i wiatru. Wspólny Mission Protocol nie oznacza wspólnego generatora toru.

QGroundControl i Mission Planner#

QGroundControl Plan View integruje misję, geofence i rally points. Pokazuje planned home, profile terrain, statystyki odległości/czasu i command list zależną od firmware oraz vehicle type.[6] Przy planowaniu offline operator musi poprawnie wybrać profil; w przeciwnym razie może zobaczyć komendy, których docelowy pojazd nie obsługuje.

Plik .plan QGC zawiera JSON i może przechowywać complex items, np. survey pattern, które GCS rozwija do prostych elementów dla autopilota.[7] Repozytorium operacyjne powinno zachować zarówno źródłowy plan wysokiego poziomu, jak i kanoniczny read-back z pojazdu.

Mission Planner eksponuje komendy ArduPilot i parametry znaczące dla podłączonego typu pojazdu. Dokumentacja rozróżnia NAV, DO i CONDITION oraz opisuje zachowanie końca i restartu.[4][5] Plan przygotowany w QGC i otwarty w Mission Planner może utracić część metadanych GCS nawet wtedy, gdy lista MAVLink pozostaje wykonalna.

Praktyczna zasada:

edytuj w zatwierdzonym narzędziu i wersji;
ustaw właściwy firmware/vehicle profile;
zapisuj source plan;
uploaduj do docelowego autopilota;
wykonuj read-back;
archiwizuj wynik z hashami;
nie uznawaj podglądu mapy za dowód zgodności toru.

Logowanie i metryki#

Log misji powinien pozwalać odtworzyć interpreter i rzeczywisty tor. Minimalny zestaw:

mission revision/hash/count;
source GCS i czas uploadu;
current item, previous item i next item;
command, frame oraz parametry aktywnego elementu;
moment enter/reached/exit i reason;
desired position/velocity/acceleration/yaw;
actual position/velocity/attitude;
cross-track, along-track i vertical error;
acceptance/pass radius użyte przez generator;
modal speed, ROI, camera state i loop counters;
pause/resume/restart/change-current events;
feasibility warnings;
estimator resets, geofence state i failsafe;
payload command ACK i timestamp zdarzenia.

Metryki całej misji:

Metryka	Znaczenie
max cross-track error	jakość śledzenia poziomego
max vertical error	zapas do terrain/ceiling
item completion latency	opóźnienie interpretacji i stabilizacji
segment time error	różnica plan–wykonanie
energy prediction error	jakość modelu energii
trigger spacing error	kompletność danych payloadu
resume overlap/gap	jakość wznowienia survey
minimum geofence clearance	zachowany margines

Samo MISSION_CURRENT w telemetrii nie wystarcza do analizy. Potrzebny jest setpoint i rzeczywisty stan. Zdarzenie „item reached” powinno zawierać warunek, który je wywołał.

SITL HIL i fault injection#

Testy powinny obejmować program misji, dynamikę i protokół.

Interpreter#

pojedynczy waypoint, hold i autocontinue;
NAV z wieloma DO/CONDITION;
warunek niespełniony przed przejściem NAV;
modalna zmiana speed/ROI/camera;
skończone i nieskończone pętle;
jump do nieistniejącego elementu;
komenda nieobsługiwana;
NaN, zero i wartości graniczne parametrów;
koniec misji dla każdego typu pojazdu.

Geometria i dynamika#

ostre zakręty przy różnych prędkościach;
bardzo krótkie segmenty;
overshoot acceptance sphere;
wiatr czołowy/boczny i zmienny;
maksymalna masa;
climb rate ponad limit;
terrain ridge między poprawnymi waypointami;
spline wychodzący poza geofence;
transition VTOL blisko granicy.

Protokół#

utrata co N-tego pakietu;
duplikacja i reorder;
timeout na count, request i ACK;
rozłączenie w połowie uploadu;
dwóch nadawców próbujących transferu;
niewłaściwy mission type;
float/int round-trip;
restart GCS i autopilota;
read-back po normalizacji.

Pauza i awarie#

pauza przed, w i po acceptance region;
ręczne przesunięcie current item;
wznowienie w misji z triggerem;
utrata GNSS i odzyskanie ze skokiem;
utrata łącza;
low battery w pętli;
aktywacja nowego geofence;
update misji w powietrzu;
reset companion computera;
niepowodzenie payload command.

SITL umożliwia deterministyczną regresję i Monte Carlo wiatru/opóźnień. HIL ujawnia scheduler, pamięć, rzeczywistą transmisję i reset hardware. Lot próbny powinien zacząć się od prostych planów w uwięzionym lub kontrolowanym środowisku, z gotowym przejęciem i bez niezweryfikowanych akcji payloadu.

Kryteria odbioru#

Obszar	Przykładowe kryterium
transfer	po przerwanym uploadzie aktywna pozostaje poprzednia revision
read-back	command, frame, params i współrzędne są semantycznie zgodne
walidacja	unsupported command i nieważna pętla blokują start
tor	swept trajectory zachowuje wymagany clearance
dynamika	nie przekracza kwalifikowanych limitów velocity/acceleration/jerk
pion	frame i datum każdego elementu są identyfikowalne
item reached	warunek ukończenia nie zależy od przeskoczonej próbki
resume	modalne stany i loop counters są zgodne ze specyfikacją
awaria	konflikt failsafe prowadzi do jednego stabilnego stanu
log	odtwarza plan, setpoint, tor i przyczynę przejścia
wersje	wyniki są przypięte do buildów autopilota i GCS

Kryteria powinny być liczbowe dla konkretnej platformy: maksymalny cross-track, minimalny terrain clearance, dopuszczalny błąd czasu triggera, maksymalne przekroczenie profilu i energia rezerwowa. Samo „misja ukończona” nie dowodzi jakości.

Typowe błędy#

Łączenie kropek#

Punkty końcowe nie opisują zakrętów, splines, prędkości ani profilu pionowego. Walidować trzeba pełną trajektorię.

Niejawna wysokość#

Relative, AMSL, terrain i ellipsoid nie są równoważne. Frame musi być częścią każdego elementu i read-back.

Kopiowanie misji między stosami#

PX4 i ArduPilot mogą inaczej interpretować parametry waypointu, koniec misji, resume i komendy pomocnicze. Potrzebny jest profil zgodności.

Zbyt mały promień osiągnięcia#

Statek oscyluje lub nigdy nie kończy elementu. Promień powinien wynikać z dynamiki i celu operacyjnego.

Zbyt duży promień osiągnięcia#

Punkt zostaje uznany wcześniej, a pomiar lub manewr nie zachodzi w wymaganym miejscu.

Brak atomicznego uploadu#

Prefiks nowego planu po utracie łącza nie jest poprawną misją. Aktualizacja potrzebuje stagingu i commit.

Brak read-back#

ACK potwierdza transakcję, lecz nie zastępuje porównania ram, parametrów i kwantyzacji.

Ignorowanie komend modalnych#

Speed, ROI i trigger mogą obowiązywać przez wiele itemów. Po skoku lub wznowieniu trzeba odtworzyć ich stan.

Nieskończona pętla bez warunku końca#

Patrol zużyje energię do failsafe. Warunek przerwania i rezerwa muszą być jawne.

Return traktowany jako magiczny#

RTL również potrzebuje trasy, geofence, energii i poprawnej pozycji. Może być niewykonalny w momencie awarii.

Test wyłącznie w GCS#

Podgląd mapy nie odwzorowuje dokładnie generatora autopilota, opóźnień, wiatru ani zachowania po błędzie. Potrzebne są read-back, SITL, HIL i kontrolowane próby.

Dojrzała nawigacja waypointowa jest zatem systemem wykonania, walidacji i obserwacji programu lotu. Dopiero spójność pliku, protokołu, modelu dynamiki, estymatora, granic i procedur daje przewidywalny wynik.

Powiązane tematy#

Przypisy#

MAVLink, „Mission Protocol” — transfer, mission types, sekwencjonowanie, warianty MISSION_ITEM i MISSION_ITEM_INT.
MAVLink Common Message Set — MAV_FRAME, MAV_CMD_NAV_WAYPOINT i parametry komend.
PX4, „Mission Mode — Multicopter” — wykonanie, feasibility checks, pause/resume, liczniki skoków i zachowanie misji obrazujących.
ArduPilot Copter, „Mission Command List” — semantyka NAV/DO/CONDITION, waypoint, spline, yaw i pętle.
ArduPilot Mission Planner, „Planning a Mission with Waypoints and Events” — planowanie, koniec misji i zasady reset/resume dla typów pojazdów.
QGroundControl, „Plan View” — planned home, profile, mission/geofence/rally, command profile i diagnostyka transferu.
QGroundControl, „Plan File Format” — JSON .plan, mission items, complex items, geofence i rally points.
PX4, „Geofence” — kontekst sprawdzania planu względem aktywnych granic.

Źródła z centralnego rejestru

MAVLink: Mission Protocol [specyfikacja protokołu]
MAVLink Common Message Set: coordinate frames and LOCAL_POSITION_NED [specyfikacja protokołu]
PX4 User Guide: Mission Mode — Multicopter [dokumentacja projektu]
ArduPilot Copter: Mission Command List [dokumentacja projektu]
ArduPilot Mission Planner: Planning a Mission with Waypoints and Events [dokumentacja projektu]
QGroundControl User Guide: Plan View [dokumentacja projektu]
QGroundControl Developer Guide: Plan File Format [dokumentacja formatu projektu]
PX4 User Guide: Geofence [dokumentacja projektu]