Proč je plánování trajektorií obtížné
Plánování trajektorií pro bezpilotní létající prostředky (UAV) v reálném světě musí zároveň respektovat dynamická omezení platformy, bezpečnostní odstupy a prostorově i časově proměnné prostředí s překážkami. Úloha není pouze geometrická (najít průchozí cestu), ale kinodynamická: dron musí danou cestu také realizovat s limity na tah, rychlost, zrychlení, ráz (jerk), náklony (roll/pitch), úhlové rychlosti a s ohledem na aerodynamiku, energetická omezení a latence aktuátorů a senzorů. Tento článek systematicky pokrývá modelování omezení, reprezentace prostředí, metody plánování, optimalizační formulace a implementační strategie v autopilotech.
Modely dynamiky UAV a převod omezení do plánování
Pro multirotorové UAV se často používá model tuhé tělesné dynamiky s oddělením translační a rotační části. Minimálně realizovatelná reprezentace trajektorie je v poloze a jejích derivacích (rychlost, zrychlení, ráz). Klíčová omezení:
- Tah a náklon: součet tahů rotorů je vázán na maximální dostupný tah a limity náklonů, což místně omezuje možné zrychlení v tělesném rámci.
- Rychlostní limity: aerodynamický odpor a regulátor omezují horizontální rychlost, typicky 10–20 m/s pro menší drony.
- Úhlová dynamika: omezení na ω a (dot{ω}) vážou maximální křivost trajektorie a rychlost změny směru.
- Energetika a baterie: spotřeba roste s počtem prudkých manévrů; penalizace rázů a špičkových zrychlení pomáhá prodloužit dolet.
- Latence a regulační pásmo: trajektorie musí být sledovatelná v daném pásmu regulátoru; příliš vysoké křivosti mohou vést k saturaci aktuátorů.
Praktické je využít diferenciální plošnost multirotorů, kde ploché výstupy (typicky poloha a yaw) umožňují odvodit potřebné vstupy (tahy, momenty) z polynomiálně definované trajektorie a jejích derivací. Takto lze explicitně kontrolovat limity na rychlost/zrychlení/jerk a zároveň zpětně ověřit realizovatelnost.
Reprezentace prostředí: od mřížek po signované vzdálenosti
Přesnost a výpočetní náročnost plánování zásadně závisí na mapě:
- Okupační mřížky (2D/3D voxelové mapy): jednoduché, vhodné pro rychlé rozpoznání průchodnosti, ale hrubé pro jemné vyhýbání.
- ESDF/TSDF (signovaná vzdálenostní pole): poskytují hladký gradient vzdálenosti k překážkám, což je ideální pro gradientní optimalizace a generování bezpečnostních koridorů.
- Polygonové/mesh modely: přesné rozhraní, užitečné pro planární scénáře (např. vnitřní prostory), avšak náročnější na kolizní testy.
- Dynamické objekty: reprezentované predikovanými trajektoriemi (např. kalmanovské predikce) s neurčitostí; vyžadují časově parametrizované mapy (spacetime occupancy).
Bezpečnostní modely: tvrdá, měkká a pravděpodobnostní omezení
Kolize lze řešit několika způsoby:
- Tvrdá omezení: přísně zakazují vstup do zakázaných oblastí; garantují bezpečnost, ale mohou zvyšovat neuskutečnitelnost plánování.
- Měkká (penalizační) omezení: přidávají do nákladové funkce člen podle vzdálenosti k překážce (např. 1/d nebo kvadratická penalizace nad bezpečnostním okrajem).
- Chance constraints: při neurčitosti polohy UAV nebo překážek se vyžaduje, aby pravděpodobnost porušení byla pod prahovou hodnotou (např. < 1 %).
Geometrické vs. kinodynamické plánování
Geometrické plánování (A*, D*, PRM, RRT) řeší pouze průchodnost a až následně se cestám přiděluje časový profil (time-parameterization). Kinodynamické plánování přímo hledá trajektorie, které jsou fyzikálně realizovatelné. Výběr závisí na požadované rychlosti, jemnosti a garancích:
- A* na mřížce s heuristikou (např. eukleidovská vzdálenost) je rychlý, ale hrubý; následně se vyhlazuje a parametrizuje v čase.
- PRM/PRM* vhodné pro offline mapy; nalezne cestu přes roadmapu a poté provede lokální optimalizaci.
- RRT/RRT* (a jejich dynamické varianty) dobře škálují v 3D; RRT* konverguje k optimu, pokud je dostatek vzorků a správné místní plánování.
- Kinodynamické RRT používá dopřednou simulaci systémové dynamiky a lokální stabilizační řízení; generuje fyzikálně realizovatelné větve již během expanze.
Optimalizační formulace: od polynomiálních spline po MPC
Optimalizace je jádrem moderních plánovačů:
- Polynomiální B-spline / minimum-snap trajektorie: minimalizace integrálu druhé/čtvrté derivace (acc/jerk/snap) při uzlových bodech a kolizních omezeních. Výborně sledovatelné a energeticky efektivní.
- CHOMP/STOMP a gradientní metody: přímý sestup na hladkém nákladu s penalizací blízkosti překážek přes ESDF; rychlé lokální zlepšení.
- Convex decomposition & safe flight corridors: rozklad prostředí na konvexní buňky a optimalizace spline s lineárními/kvadratickými omezeními v koridoru (SOCP/QP).
- Nonlinear Programming (NLP): úplná formulace s dynamikou, kolizemi a limity (SQP/Ipopt); přesná, ale náročnější pro reálný čas.
- Model Predictive Control (MPC): přeplánování na horizontech 1–3 s; přirozeně zvládá dynamické překážky, saturace a změny cíle.
Časová parametrizace a sledovatelnost
Geometrická křivka se musí „oživit“ časem. Typické postupy:
- Time-Optimal Path Parameterization (TOPP): vypočítá maximálně povolené profily rychlosti/akcelerace po oblouku s respektem limitů.
- Heuristická alokace času na segmenty: čas úměrný délce a lokální křivosti; následně se iterativně upravuje podle průniků s omezeními.
- Iterativní reparametrizace: spouští se v cyklu se sledovačem; pokud regulátor saturuje, zpomalí se časový profil.
Vyhýbání se překážkám v čase: predikce, přeplánování, robustnost
U pohyblivých objektů je nutné plánovat v prostoru-čase. Používají se:
- Predikční modely cílů: konstantní rychlost/akcelerace s kovariancí a gating logikou při asociaci měření.
- On-line přeplánování: MPC nebo rychlý RRT*/CHOMP v cyklu 10–50 Hz dle dostupného výpočtu.
- Tube MPC / robustní koridory: plánuje se nominál a „trubice“ kolem s rezervami pro poruchy a vítr.
Bezpečnostní okraje, clearance a pravidla letu
Trajektorie musí respektovat minimální vzdálenosti od překážek (např. 0,5–2,0 m dle přesnosti lokalizace), letová pravidla (výškové limity, vyhrazené zóny), bezpečné rychlosti při blízkosti lidí a geo-fencing oblasti definované provozovatelem nebo regulátorem. Při nejistotě lokalizace je vhodné zvětšit clearance o 3σ odhad chyby.
Jemné tvarování trajektorie: hladkost, křivost, komfort aktuátorů
Penalizace křivosti, jerk a snap vede k hladkým příkazům pro regulátor, menšímu opotřebení a nižší spotřebě. B-spline a minimum-snap polynomy poskytují (C^3) kontinuitu, která je prakticky důležitá pro stabilitu vnitřní řídicí smyčky.
Integrace se stavovým odhadem a mapováním
Plánování je pevně provázáno s odhadem stavu (VIO/SLAM, GNSS/RTK) a lokálním mapováním (např. sliding-window ESDF z lidarů/kamer). Stabilní časovač plánovače musí pracovat se senzorickými latencemi a časovými razítky, jinak hrozí systematické chyby v kolizních testech.
Algoritmické vzory pro embedded implementace
- Dvouúrovňová architektura: global planner (zřídka spouštěný, hrubý) + local planner (často spouštěný, jemný) s konzistentním rozhraním.
- Teplý start (warm start): optimalizátor inicializovaný předchozí trajektorií výrazně zkracuje konvergenci.
- Pracovní koridory: generování konvexních bezpečných buněk (např. přes Voronoi či polyedrální eroze ESDF) a následná QP/SOCP optimalizace spline.
- Předvýpočet gradientů: ESDF gradient lze kešovat na mřížce; kolizní penalizace tak běží v reálném čase.
Specifika multirotorů vs. VTOL/pevné křídlo
Multirotory jsou všesměrové s nízkou dopřednou rychlostí a ostrými manévry, což umožňuje hustší polynomiální sítování. VTOL a pevná křídla mají minimální dopřednou rychlost a omezenou rychlost zatáčení (limit bank angle), takže se vhodněji modelují přes Dubins/Reeds–Shepp křivky doplněné o reálnou dynamiku a následnou optimalizaci profilu rychlosti.
Metodika návrhu nákladové funkce
Typická nákladová funkce je vážený součet: délka/čas letu, hladkost (∫ jerk²), vzdálenost od překážek (−∫ ESDF), spotřeba energie, příspěvek k úloze (např. pohled kamery). Váhy se ladí experimentálně, případně adaptivně dle kontextu (blízkost překážek zvyšuje koeficient bezpečnosti).
Režimy selhání a fail-safe strategie
- Ztráta mapy/odhadovače: přepnout na hover/land s lokálním radarem/sonarem a velkým bezpečnostním okrajem.
- Neuskutečnitelná optimalizace: fallback na konzervativní RRT v rozšířeném koridoru nebo na stop-and-go s rekonfigurací časování.
- Náhlá dynamická překážka: reaktivní vyhýbání s garantovanou zónou zastavení (stopping set) a prudké snížení rychlosti.
Testování, verifikace a metriky
Doporučené metriky: míra kolizí v simulaci, průměrná a maximální blízkost k překážkám, hladkost (max jerk/snap), energetická náročnost, výpočetní čas (medián, p95), robustnost vůči rušení větrem a latencím. Testování zahrnuje Monte Carlo scénáře s náhodnými překážkami a šumem senzorů, HIL (hardware-in-the-loop) a polygonové dráhy s přesně známými hranami.
Typický implementační pipeline
- Sběr senzorických dat (Lidar/Kamera/IMU/GNSS) → lokální ESDF/TSDF a odhad pózy.
- Global planner ve statické mapě najde hrubou cestu přes PRM*/RRT*.
- Generování bezpečných koridorů podél hrubé cesty (polyedry/ellipsoidy).
- Lokální optimalizace B-spline/minimum-snap s tvrdými clearance a dynamickými limity.
- Časová parametrizace (TOPP) a validace realizovatelnosti přes ploché mapování.
- MPC sledování s přeplánováním při detekci změn nebo pohyblivých objektů.
Praktická doporučení pro real-time
- Preferujte konvexní koridory + QP/SOCP pro deterministické časy výpočtu.
- Udržujte kratší plánovací horizont (1–3 s) s překrývajícími se segmenty a běžte v cyklu ≥ 10 Hz.
- Agresivně kešujte ESDF a používejte warm start z předchozí trajektorie.
- Zavádějte stop set: oblast, do které se dron vždy dokáže zastavit z aktuální rychlosti.
Příklad nákladové funkce a omezení (ilustrativně)
Minimalizovat (J = alpha T + beta int | dddot{mathbf{p}}(t) |^2 dt – gamma int phi(mathbf{p}(t)) dt), kde (phi) je ESDF (kladná v bezpečných zónách), za podmínek: (|dot{mathbf{p}}| le v_{max}), (|ddot{mathbf{p}}| le a_{max}), (|dddot{mathbf{p}}| le j_{max}), úhel náklonu ≤ θmax, a clearance ≥ dsafe. Pro multirotor lze ověřit realizovatelnost zpětným výpočtem tahů/momentů.
Škálování na více UAV (multi-agent plánování)
Pro rojové scénáře se používají dekompozice: centralizované (NLP s velkým počtem proměnných), distribuované (ADMM, prioritní plánování) nebo kolizní vyhýbání přes
