Plánování trajektorií s dynamickými a kolizními omezeními

Proč je plánování trajektorií obtížné

Plánování trajektorií pro bezpilotní letadla (UAV) v reálném světě musí současně respektovat dynamická omezení platformy, bezpečnostní odstupy a tvarově i časově proměnlivé 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í, náraz (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 využívá model tuhé tělesné dynamiky s oddělením translační a rotační části. Minimální realizovatelná reprezentace trajektorie bývá v poloze a jejích derivacích (rychlost, zrychlení, náraz). 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ž bodově 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{ω}) omezují maximální zakřivení trajektorie a rychlost změny směru.
• Energetika a baterie: spotřeba roste s počtem prudkých manévrů; penalizace nárazů a špičkových zrychlení pomáhá prodloužit dolet.
• Latence a regulační pásmo: trajektorie musí být sledovatelná při daném pásmu regulátoru; příliš vysoké zakřivení může 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í. Tak 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ů.
• Polygónové/mesh modely: přesné rozhraní, užitečné pro planární scénáře (např. vnitřní prostory), ale náročnější na kolizní testy.
• Dynamické objekty: reprezentované predikovanými trajektoriemi (např. kalmanovské predikce) s nejistotou; 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í: striktně zakazují vstup do zakázaných oblastí; garantují bezpečnost, ale mohou zvýšit neuskutečnitelnost.
• Měkká (penalizační) omezení: přidávají do nákladové funkce termín podle vzdálenosti k překážce (např. 1/d nebo kvadratická penalizace nad bezpečnostní hranicí).
• Chance constraints: při nejistotě polohy UAV nebo překážek se pož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 teprve 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ý; poté se vyhlazuje a parametrizuje v čase.
• PRM/PRM* vhodné pro offline mapy; najde cestu přes roadmapu a následně provede lokální optimalizaci.
• RRT/RRT* (a jejich dynamické varianty) výborně škálují v 3D; RRT* konverguje k optimu, pokud je dostatek vzorků a správné lokální 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 (akcelerace/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 pomocí ESDF; rychlé lokální zlepšení.
• Konvexní dekompozice & bezpečné letové koridory: rozklad prostředí na konvexní buňky a optimalizace spline s lineárními/kvadratickými omezeními v koridoru (SOCP/QP).
• Nelineární programování (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): re-plánování na horizontech 1–3 s; přirozeně řeší dynamické překážky, saturace a změny cíle.

Časová parametrizace a sledovatelnost

Geometrická křivka musí být „oživena“ časem. Typické postupy:

• Time-Optimal Path Parameterization (TOPP): vypočítá maximálně povolené profily rychlosti/akcelerace po oblouku s respektováním 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, re-plá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í.
• Online re-plánování: MPC nebo rychlý RRT*/CHOMP v rámci smyčky 10–50 Hz podle dostupného výpočetního výkonu.
• 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é předpisy (výškové limity, vyhrazené zóny), bezpečné rychlosti při blízkosti lidí a geo-fencing oblasti definované provozovatelem či regulátorem. Při nejistotě lokalizace se doporučuje zvětšit clearance o 3σ odhad chyby.

Jemné tvarování trajektorie: hladkost, zakřivení, komfort aktuátorů

Penalizace zakřivení, 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í regulační smyčky.

Integrace s odhadem stavu a mapováním

Plánování je pevně propojeno 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

• Dvoustupňová architektura: globální plánovač (zřídka, hrubý) + lokální plánovač (často, 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.
• Predpočítání gradientů: ESDF gradient lze cacheovat na mřížce; penalizace kolizí tak probíhají 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íťování. VTOL a pevná křídla mají minimální dopřednou rychlost a omezenou rychlost zatáčení (bank angle limit), a proto 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ř. zorné pole kamery). Váhy se ladí experimentálně, případně adaptivně podle 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 větší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

1. Sběr senzorických dat (Lidar/Kamera/IMU/GNSS) → lokální ESDF/TSDF a odhad polohy.
2. Globální plánovač ve statické mapě najde hrubou cestu přes PRM*/RRT*.
3. Generování bezpečných koridorů podél hrubé cesty (polyedry/elipsoidy).
4. Lokální optimalizace B-spline/minimum-snap s tvrdými clearance a dynamickými limity.
5. Časová parametrizace (TOPP) a validace realizovatelnosti přes ploché mapování.
6. MPC sledování s re-plánováním při detekci změn nebo pohyblivých objektech.

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 provozujte smyčku ≥10 Hz.
• Agresivně cacheujte ESDF a používejte warm start z předchozí trajektorie.
• Zavádějte stop set: oblast, do které se dron vždy vejde zastavit z aktuální rychlosti.

Příklad nákladové funkce a omezení (ilustrační)

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 ≥ d_safe. 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í)

V rojových scénářích se používají dekompozice: centralizované (NLP s velkým počtem promě