Plánování trajektorií pro bezpilotní letouny v prostředí s konstantním větrem

České vysoké učení technické v Praze Fakulta elektrotechnická Katedra kybernetiky

Bakalářská práce

Plánování trajektorií pro bezpilotní letouny v prostředí s konstantním větrem Petr Váňa

Vedoucí práce: Ing. Milan Rollo, Ph.D.

Studijní program: Kybernetika a robotika, Bakalářský Obor: Robotika 23. května 2013

iv

v

Poděkování Rád bych poděkoval mému vedoucímu, Ing. Milanu Rollovi, Ph.D., za příkladné vedení a podporu při tvorbě této práce. Dále také Ing. Martinu Seleckému a Ing. Tomáši Meiserovi za pomoc s experimenty na letounu Procerus.

vi

viii

x

Abstract The bachelor thesis deals with trajectory planning problem for unmanned aerial vehicles (UAVs). Existing solutions often neglect influence of the environment. When we use aircraft in real environment it is necessary to take effects of wind into account. In such conditions it is complicated or even impossible for UAVs to follow trajectories, which were planned without respect to the wind. This thesis describes modified algorithm for environment with constant wind, which addresses the drawbacks of existing solutions. Proposed solution is based on modification of Dubins curves approach and modifies shape of the turns from former circles to trochoids. That allows us to implement Accelerated A* algorithm for flight path planning of nonholonomic vehicles operating in dynamic continuous space with obstacles and constant wind. The proposed concept has been tested in simulations as well as on real aircraft and results had been compared to case without wind.

Abstrakt Tato bakalářská práce se zabývá plánováním trajektorií pro bezpilotní prostředky. Existující přístupy často zanedbávají působení vlivů prostředí. Při reálném nasazení bezpilotních letounů však musíme počítat s vlivem větru. Za takových podmínek je pro bezpilotní letoun komplikované nebo dokonce nemožné letět po trajektorii, která byla naplánovaná bez ohledu na vítr. Práce popisuje návrh plánovacího algoritmu pro prostředí s konstantním větrem, který by řešil nevýhody stávajících přístupů. Navržené řešení je založeno na úpravě algoritmů využívajících Dubinsových křivek, kdy je upraven tvar zatáček a namísto původních kružnic se používají prodloužené cykloidy. To umožňuje implementovat algoritmus Accelerated A* pro plánování trajektorií neholonomních prostředků pohybující se v dynamickém spojitém prostoru s překážkami a konstantním větrem. Navržený koncept byl otestován v simulacích i na reálném letounu a výsledky byly porovnány s řešením, které zanedbává vliv větru.

xi

xii

Obsah 1 Úvod

1

1.1

Motivace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1

1.2

Struktura práce . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

2

2 Plánování trajektorií

3

2.1

Neholonomní robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3

2.2

Kinematika robotů s koly . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3

2.3

Hledání nejkratší cesty . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

4

2.3.1

Dubinsovy křivky . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

5

2.3.2

Reeds-Shepp křivky . . . . . . . . . . . . . . . . . . . . . . . . . . . .

6

2.4

Model letadla . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

7

2.5

Plánovače . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

7

2.5.1

A* plánovač . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

7

2.5.2

RRT plánovač

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

8

2.5.3

Hexa-grid plánovač . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

8

3 Systém AgentFly

9

3.1

Plánování trajektorií . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

9

3.2

Accelerated A* . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

9

3.3

Vyhýbání se kolizím . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

10

3.4

Multi-agentní platforma AGlobe . . . . . . . . . . . . . . . . . . . . . . . . .

11

3.4.1

11

Součásti platformy AGlobe . . . . . . . . . . . . . . . . . . . . . . . .

4 Analýza a návrh řešení

13

4.1

Matematický model letadla . . . . . . . . . . . . . . . . . . . . . . . . . . . .

13

4.2

Tvar zatáček . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

14

4.3

Změna směru letu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

15

4.4

Možnosti řešení . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

16

4.4.1

Zvětšení minimálního poloměru zatáčení . . . . . . . . . . . . . . . . .

16

4.4.2

Změna tvaru zatáček . . . . . . . . . . . . . . . . . . . . . . . . . . . .

16

xiii

xiv

OBSAH

5 Plánování trajektorie s větrem 5.1 Elementy ve větru . . . . . . . . . . . . . . . . . 5.1.1 Element horizontální zatáčky . . . . . . . 5.2 Plánování trajektorie bez překážek . . . . . . . . 5.3 Shodná rotace . . . . . . . . . . . . . . . . . . . . 5.4 Rozdílná rotace . . . . . . . . . . . . . . . . . . . 5.4.1 Hledání tečny zadaným bodem k cykloidě 5.5 Optimalita při plánování v rovině . . . . . . . . . 5.6 Rozšíření na 3D prostor . . . . . . . . . . . . . . 5.7 Úprava algoritmu Accelerated A* . . . . . . . . . 5.7.1 Komprese letových plánů . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

. . . . . . . . . .

19 19 20 20 20 21 22 24 26 27 27

6 Testování navržených algoritmů 29 6.1 Výkonové testy výpočtu manévrů . . . . . . . . . . . . . . . . . . . . . . . . . 29 6.2 Výkonové testy výpočtu trajektorií s překážkami . . . . . . . . . . . . . . . . 30 6.3 Shrnutí výsledků . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31 7 Testování řešení na letounu 7.1 Technické parametry letounu Procerus 7.2 Autopilot KESTREL . . . . . . . . . . 7.3 Program Virtual Cockpit v2.6 . . . . . 7.4 Uživatelský počítač Gumstix . . . . . 7.5 Způsob testování na letounu . . . . . . 7.6 Experimenty se simulovaným letounem 7.7 Experimenty s reálným letounem . . . 7.8 Shrnutí výsledků . . . . . . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

33 33 34 34 36 36 36 38 40

8 Závěr 43 8.1 Budoucí práce . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43 Literatura

45

A Seznam použitých zkratek

47

B Obsah přiloženého CD

49

C Rozdělení Dubinsových křivek

51

Seznam obrázků 2.1 2.2

Model Jednoduchého Auta . . . . . . . . . . . . . . . . . . . . . . . . . . . . Rozdělení Dubinsových křivek pro otočení θ = π/3 . . . . . . . . . . . . . . .

4 6

3.1 3.2

Expandované uzly algoritmu AA* . . . . . . . . . . . . . . . . . . . . . . . . . Architektura platformy AGlobe . . . . . . . . . . . . . . . . . . . . . . . . . .

10 11

4.1 4.2 4.3

Vliv konstantního větru na tvar trajektorie . . . . . . . . . . . . . . . . . . . Geometrický význam úhlu angleXY . . . . . . . . . . . . . . . . . . . . . . . Porovnání nejkratší možné trajektorie bez větru a s světrem . . . . . . . . . .

15 15 17

5.1 5.2 5.3 5.4 5.5

Příklady manévrů vě větru . . . . . . . . Oblasti bodů s existencí orientované tečny Interval posunutí virtuálního cíle . . . . . Zóna neoptimálních manévrů . . . . . . . Rozšíření manévru do 3D prostoru . . . .

. . . . .

21 23 25 26 27

6.1

Testovací konfigurace s nalezenou trajektorií . . . . . . . . . . . . . . . . . . .

30

7.1 7.2 7.3 7.4 7.5 7.6 7.7 7.8 7.9

Experimentální letoun Procerus . . . . . . . . . . . . . . . . . . KESTREL autopilot . . . . . . . . . . . . . . . . . . . . . . . . Navigace letounu . . . . . . . . . . . . . . . . . . . . . . . . . . Uživatelské okno programu Procerus Virtual Cockpit . . . . . . Uživatelský počítač Gumstix Overo . . . . . . . . . . . . . . . . Záznam simulovaných letů spolu s naplánovanými trajektoriemi Pravděpodobnost vychýlení letounu simulovaného letounu . . . Záznam větru během experimentu . . . . . . . . . . . . . . . . Záznam reálných letů spolu s naplánovanými trajektoriemi . .

. . . . . . . . .

33 34 35 35 36 37 38 39 40

C.1 Rozdělení Dubinsových křivek pro různá otočení θ . . . . . . . . . . . . . . .

52

xv

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . . . . . .

. . . . .

. . . . . . . . .

. . . . .

. . . . . . . . .

. . . . .

. . . . . . . . .

. . . . .

. . . . . . . . .

. . . . .

. . . . . . . . .

. . . . .

. . . . . . . . .

xvi

SEZNAM OBRÁZKŮ

Seznam tabulek 2.1 2.2 2.3

Tabulka základních pohybů pro model Dubinsova auta . . . . . . . . . . . . . Tabulka možných Dubinsových křivek . . . . . . . . . . . . . . . . . . . . . . Tabulka možných nejkratších cest pro Reeds-Shepp auto . . . . . . . . . . . .

5 5 7

6.1 6.2

Časová náročnost výpočtu jednoho manévru . . . . . . . . . . . . . . . . . . . Časová náročnost algoritmu AA* . . . . . . . . . . . . . . . . . . . . . . . . .

30 31

xvii

xviii

SEZNAM TABULEK

Kapitola 1

Úvod V posledních letech výrazně roste počet bezpilotních letounů používaných pro civilní i vojenské účely. Jejich hlavními výhodami, oproti letounům s lidskou posádkou, jsou především nižší pořizovací cena a menší náklady na jejich provoz. Bezpilotní letouny lze využít k nejrůznějším účelům. Mohou například mapovat neznámá prostředí, vyhledávat pohřešované osoby, monitorovat dopravní situaci nebo pořizovat panoramatické fotografie. Cílem výzkumu v této oblasti je snaha automatizovat co největší počet úkonů, které vykonává operátor na zemi, například plánovat trajektorii přímo na palubě letounu na základě operátorem zadaných bodů zájmu. Plánování trajektorií je velmi rozsáhlý problém. Je nutné při něm respektovat fyzikální vlastnosti daného prostředku. Ty se navíc výrazně liší, v závislosti na typu prostředku, a proto je nutné pro různé kategorie letounů vyvinout odlišné algoritmy. Například při plánování trajektorie pro helikoptéry je možné používat manévry s vertikální změnou výšky beze změny polohy. Naopak běžná letadla touto možností nedisponují a jsou omezena minimální rychlostí letu. Je nutné zabývat se i možnými kolizemi mezi jednotlivými bezpilotními prostředky operujícími ve stejné oblasti. V reálném prostředí se často musí bezpilotní letouny vyrovnávat s vlivy větru. V takových podmínkách může být pro letoun těžké nebo i nemožné splnit trajektorii, kterou by v prostředí bez větru bez problémů proletěl. Manévrovací schopnosti výrazně závisí na orientaci letounu vzhledem ke směru větru. V této práci se budeme zabývat možnými změnami v plánování trajektorií pro prostředí s konstantním větrem.

1.1

Motivace

Hlavní motivací pro tuto práci byl výzkum prováděný v Centru agentních technologií na katedře počítačů v oblasti bezpilotních prostředků. Jsou zde řešeny teoretické problémy plánování letového provozu a navržená řešení jsou testována na simulovaných scénářích. Pro ověření možnosti nasazení na reálných letounech byly zakoupeny dva experimentální bezpilotní letouny Procerus od společnosti Lockheed Martin. V průběhu testování vyvíjených algoritmů se objevily problémy při letu ve větrném prostředí. Často docházelo k vychylování od naplánované trajektorie a tento problém bylo nutné řešit. Cílem této práce je proto upravit stávající algoritmy pro plánování trajektorií tak,

1

2

KAPITOLA 1. ÚVOD

aby byly letouny Procerus schopné naplánovanou trajektorii spolehlivě plnit i v prostředí s působícím větrem.

1.2

Struktura práce

Práce je rozdělena do několika kapitol. Nejprve se budeme v kapitole 2 zbývat plánováním trajektorií pro neholonomní roboty. V kapitole 3 si popíšeme řídicí systém AgentFly, který používáme na palubním počítači Gumstix bezpilotního letounu. Následně provedeme v kapitole 4 analýzu možných úprav plánovacích algoritmů pro prostředí s větrem. Navržené řešení podrobněji rozebereme v kapitole 5, ve které zároveň popíšeme nejdůležitější body implementace tohoto řešení. Výpočetní náročnost nových algoritmů budeme testovat v kapitole 6. K testování navrženého konceptu budeme v kapitole 7 používat jak simulovaný, tak i reálný letoun.

Kapitola 2

Plánování trajektorií Plánování trajektorií pro bezpilotní letouny často využívá znalostí získaných při výzkumu pohybu pozemních robotů. Obvykle se roboti pohybují po kolech nebo pásech, které zároveň určují možné směry jejich pohybů. V této kapitole si proto popíšeme různé modely pozemních robotů, jejich základní vlastnosti a možnosti vyžití získaných znalostí pro plánování bezpilotních letounů.

2.1

Neholonomní robot

Důležitou vlastností každého robota je, zda se jedná o holonomního nebo neholonomního robota. Neholonomní robot je takový robot, u kterého nemůžeme libovolně měnit rychlost ve všech směrech jeho pohybu nezávisle na jeho poloze ve stavovém prostoru. Formální definici tohoto pojmu můžeme nalézt například v [1]. Typickým příkladem neholonomního robota je osobní automobil. Z konstrukčních důvodů je u něj omezeno natočení předních kol, což zároveň určuje minimální poloměr zatáčení celého vozidla. Naopak příkladem holonomního robota je vozidlo vybavené všesměrovými koly.

2.2

Kinematika robotů s koly

Pozemní roboti mohou mít nejrůznější počet kol, pásů nebo jejich kombinací. Při kategorizaci těchto robotů budeme vycházet z knihy Planning Algorithms [1]. Pro větší přehlednost textu nebudeme uvažovat roboty se všesměrovými koly a budeme se dále zabývat pouze neholonomními roboty. Z našeho pohledu jsou nejdůležitější následující modely robotů: Tank Model tanku obsahuje dvě diferenciálně řízená kola nebo pásy. Aby se mohl rozjet určitým směrem, musí se nejdříve do toho směru otočit. Může také couvat. Trojkolka Model trojkolky se skládá ze dvou pevných kol jedné nápravy a otočného třetího kola, které je zároveň poháněno. Natočení osy poháněného kola není nijak omezeno.

3

4

KAPITOLA 2. PLÁNOVÁNÍ TRAJEKTORIÍ

Jednoduché Auto Model jednoduchého auta je inspirován běžnými osobními automobily. Tento model má na zadní nápravě dvě pevná kola a na přední dvě otočná kola. Natočení předních kol je limitováno úhlem φmax . Toto omezení spolu s rozvorem náprav L definuje minimální poloměr zatáčení ρmin . Příklad takového robota můžeme vidět na obrázku 2.1.

Obrázek 2.1: Model Jednoduchého Auta [1]

Reeds-Shepp Auto Tento model vychází z modelu jednoduchého auta, u kterého omezuje interval rychlostí na pouhé tři hodnoty. Auto může jet plnou rychlosti dopředu, nepohybovat se a nebo plnou rychlostí couvat. Zavádí pojem diskrétní motor, který je u reálných robotů nerealizovatelný. Zúžení intervalu povolených rychlostí nijak neovlivní dosažitelnost poloh stavového prostoru v daném čase. Někdy se vynechává možnost, že robot stojí na místě a nepohybuje se. Dubinsovo Auto Tento model je obdobný jako předchozí případ, ale nepovoluje zápornou ani nulovou rychlost robota. Robot se tedy po celou dobu pohybuje konstantní dopřednou rychlostí a může pouze zatáčet v povoleném rozsahu.

2.3

Hledání nejkratší cesty

Důležitou otázkou při plánování trajektorií neholonomních robotů je, jestli lze jednoduše najít nejkratší možnou cestu z počáteční polohy do cílové polohy. Tuto úlohu můžeme formulovat jako hledání trajektorie T, pro kterou je uražená vzdálenost L minimální možná. Vzdálenost L definujeme vzorcem (2.1). Z L(T ) = 0

tT

p

x(t) ˙ 2 + y(t) ˙ 2 dt

(2.1)

2.3. HLEDÁNÍ NEJKRATŠÍ CESTY

5

Přelom v této oblasti přinesl Lester Eli Dubins. Ten v roce 1957 publikoval svou matematickou práci [2], ve které se mu podařilo dokázat, jak musí vypadat cesty mezi dvěma orientovanými body, aby mohly být nejkratší možné v dvourozměrném prostoru při konstantní dopředné rychlosti a minimálním poloměru zatáčení robota. Tyto Dubinsovy křivky (cesty) označujeme v angličtině Dubins curves nebo Dubins paths.

2.3.1

Dubinsovy křivky

L. E. Dubinsovi se ve výše zmíněném článku podařilo dokázat, že pro dva orientované body v dvourozměrném Euklidovském prostoru R2 bude nejkratší možná trajektorie Dubinsova auta složená pouze z rovných úseků S (straight) a zatáček o minimálním možném poloměru ρmin L (left) nebo R (right). Dubinsovy křivky se proto budou skládat pouze z pohybů zapsaných v tabulce 2.1. Všechny křivky s jiným než nejmenším možným poloměrem zatáčení jsou tedy neoptimální. Při samotném důkazu používal dlouhou řadu argumentů založených na geometrické reprezentaci problému. Symbol S L R

Zatáčení u 0 1 -1

Rychlost v 1 1 1

Tabulka 2.1: Tabulka základních pohybů pro model Dubinsova auta Z tohoto důkazu také vyplývá, že jakoukoliv trajektorii lze zapsat slovem složeným z několika písmen S, L a R. Dokázal také, že slovo popisující optimální trajektorii obsahuje maximálně 3 písmena. Zatáčky R nebo L označíme písmenem C (circle). Slovo SCS se mu podařilo vyloučit jako neoptimální, a proto lze každou optimální trajektorii Dubinsova auta zapsat jako CSC nebo CCC. Ve slově CCC přitom nemohou následovat dvě zatáčky na stejnou stranu (RR nebo LL). Existuje tedy 6 různých trajektorií, které mohou být optimální. Ty jsou zapsány v tabulce 2.2, kde jsou uvedeny také omezující podmínky pro jednotlivé úseky. Nejkratší možnou trajektorii nazýváme Dubinsovou křivkou. Typ Rα Sd Rγ Lα Sd Lγ CSC Rα Sd Lγ Lα Sd Rγ Rα Lβ Rγ CCC Lα Rβ Lγ

α, γ < 0, 2π) < 0, 2π) < 0, 2π) < 0, 2π) < 0, 2π) < 0, 2π)

β < π, 2π) < π, 2π)

d < 0, ∞) < 0, ∞) < 0, ∞) < 0, ∞) -

Tabulka 2.2: Tabulka možných Dubinsových křivek, zdroj [1] Zbývá rozhodnout, který z 6 možných manévru bude při konkrétním zadání optimální. Touto otázkou se zabývá článek [3], který popisuje oblasti optimality jednotlivých Dubinso-

6

KAPITOLA 2. PLÁNOVÁNÍ TRAJEKTORIÍ

vých křivek. Zavedeme si proměnou θ, která vyjadřuje souhrnný úhel otočení mezi startovním a cílovým bodem. Můžeme ji vypočítat pomocí vzorce (2.2), kde ψ1 je úhel otočení auta v počátečním bodě a ψ2 v koncovém bodě. θ = ψ2 − ψ1

(2.2)

Optimalita trajektorií je invariantní vůči změně souřadné soustavy. Můžeme si proto zvolit polohu počátečního bodu do počátku souřadné soustavy a počáteční úhel natočení auta ve směru osy x. Zvolíme-li také hodnotu otočení θ, můžeme oblasti optimality zakreslit do obrázku 2.2. Souřadnice x a y určují polohu koncového bodu podělenou minimálním poloměrem zatáčení. Barva oblasti určuje typ optimální trajektorie. Obrázky pro různé hodnoty otočení θ jsou vyobrazeny v příloze C. Na obrázku C.1a pro otočení θ existují oblasti, kde jsou optimální dva manévry zároveň. To není možné pro žádný jiný úhel otočení θ. Na obrázcích jsou také vykresleny křivky se stejnou délkou L(θ, x, y) optimálního manévru. Tato funkce je po částech spojitá a její nespojitosti se vyskytují pouze v oblastech, kde mohou být manévry CCC optimální. Manévry CCC mohou být optimální pouze pokud cílový bod leží maximálně do vzdálenosti čtyř poloměrů zatáčení.

Obrázek 2.2: Rozdělení Dubinsových křivek pro otočení θ = π/3

2.3.2

Reeds-Shepp křivky

Další důležitý pokrok v této oblasti přinesli pánové J. A. Reeds a L. A. Shepp. Ve svém článku [4] z roku 1990 rozšířili problém hledání nejkratší cesty pro Dubinsovo auto o možnost zpětného pohybu. Obdobně jako u Dubinsových křivek se jim podařilo dokázat, že nejkratší možná cesta mezi dvěma orientovanými body je vždy složena pouze z rovných úseků a zatáček o minimálním možném poloměru. Dále také zjistili, že slovo popisující optimální

2.4. MODEL LETADLA

7

trajektorii obsahuje maximálně tři písmena. Změny směru jízdy robota jsou označované symbolem ”|”. Všechny možné typy optimálních trajektorií jsou shrnuté v tabulce 2.3. Počet kombinací pro daný typ je dán počtem možných změn orientací a změn orientace zatáček. Následně je toto číslo zdvojnásobeno, protože robot na začátku manévru může začít pohyb i opačným směrem (couvat). Důležitým poznatkem je také, že délka optimální trajektorie se skokově nemění jako tomu bylo v případě Dubinsových křivek. V tabulce 2.3 jsou některá čísla v závorce. Nezávorkované čísla odpovídají původnímu článku J. A. Reeds a L. A. Shepp. Čísla v závorkách vycházejí z článku [5], kde autoři ukázali, že manévry L− |S + |L− a R− |S + |R− nemohou být optimální. (+ a - vyjadřuje směr pohybu) Proto se počet možných kandidátů na nejkratší trajektorii zmenšil. Typ Cα |Cβ |Cγ Cα |Cβ Cγ Cα Cβ |Cγ Cα Sd Cγ Cα Cβ |Cβ Cγ Cα |Cβ Cβ |Cγ Cα |Cπ/2 Sd Cπ/2 |Cγ Cα |Cπ/2 Sd Cγ Cα Sd Cπ/2 |Cγ

α < 0, π < 0, β < 0, β < 0, π < 0, β < 0, β < 0, π < 0, π < 0, π

> > > > > > > > >

β < 0, π > < 0, π/2 > < 0, π/2 > < 0, π/2 > < 0, π/2 > -

γ < 0, π < 0, β < 0, β < 0, π < 0, β < 0, β < 0, π < 0, π < 0, π

> > > > > > > > >

d (0, ∞) (0, ∞) (0, ∞) (0, ∞) Celkem

Počet kombinací 4 4 4 8(6) 4 4 8 8 4 48(46)

Tabulka 2.3: Tabulka 48-mi možných manévrů pro Reeds-Shepp auto [1]

2.4

Model letadla

Při sestavování modelu pro letadlo budeme vycházet především z modelu Dubinsova auta, protože letoun nemůže zastavit ani couvat a obdobně jako běžný automobil je omezen jeho minimální poloměr zatáčení. Letadla se, na rozdíl od pozemních robotů, mohou pohybovat ve volném prostoru, a proto je při plánování trajektorií nutné přidat jednu dimenzi. Model letadla musí také obsahovat omezení na bezpečný úhel stoupání a klesání.

2.5

Plánovače

Plánování trajektorií bezpilotních letounů bylo v uplynulých letech předmětem mnoha výzkumů. Popíšeme si proto jen několik často používaných algoritmů.

2.5.1

A* plánovač

Algoritmus A* je určený k nalezení nejkratší cesty na grafu s kladně hodnocenými hranami. Používá stejné principy jako Dijkstrův algoritmus, ale navíc využívá znalosti prostředí. Definujeme zde proto tzv. heuristickou funkci, která určuje odhad nejmenší možné ceny cesty

8

KAPITOLA 2. PLÁNOVÁNÍ TRAJEKTORIÍ

od zadaného uzlu do cíle. Díky tomu algoritmus prochází menší počet uzlů grafu. Příkladem heuristické funkce je například kartézská vzdálenost, kterou můžeme použít pro plánování nejkratší cesty na mapě. Abychom mohli plánovat trajektorie letounu ve volném trojrozměrném prostoru, je třeba tento prostor diskretizovat plánovací mřížkou. Velikost mřížky ovlivňuje mezeru mezi naplánovanou trajektorií a překážkami. Proto je při zjemňování mřížky většinou nalezeno lepší řešení. Výhodou algoritmu A* je, že v případě použití přípustné heuristické funkce nalezne, vždy nejkratší možné řešení na zvolené mřížce.

2.5.2

RRT plánovač

Technika RRT (Rapidly-exploring random trees) je určena k náhodnému hledání cesty především pro neholonomní roboty ve vícerozměrném stavovém prostoru. Poprvé byla publikována roku 1998 v článku [6]. Výhodou této techniky je efektivní prohledávání vícerozměrného stavového prostoru. Hlavní nevýhodou je, že výsledné řešení je náhodné a zpravidla nebývá nejkratší možné.

2.5.3

Hexa-grid plánovač

Plánování na hexa-gridové mřížce je založen na rozdělení stavového prostoru na pravidelné šestiúhelníkové hranoly s konstantní výškou. Pro každou buňku se nejprve zjistí, jestli koliduje s nějakou překážkou. Kolidující buňky vyřadíme. Mezi sousedními buňkami budeme uvažovat možnou cestu jako trajektorii mezi jejich středy. Obdobně tak u buněk, které jsou nad sebou. Dostaneme tak ohodnocený graf, ve kterém najdeme nejkratší možnou trajektorii. Tento plánovač je vhodný především pro helikoptéry nebo kvadrokoptéry. Při plánování pro letouny bychom museli uvažovat velikost buňky v závislosti na poloměru zatáčení, aby letoun zvládal zatáčky na hexa-gridové mřížce. Proto se pro letouny příliš nepoužívá.

Kapitola 3

Systém AgentFly V této kapitole je popsán simulační framework AgentFly, který je používán k vývoji a testování řídicích algoritmů pro ovládání experimentálních letounů. Systém AgentFly byl vyvinut v Centru Agentních Technologií na Fakultě elektrotechnické na ČVUT v Praze jako multi-agentní software a simulační prostředí pro civilní leteckou dopravu a bezpilotní prostředky. Vyznačuje se především schopností distribuovaně simulovat velký počet letadel.

3.1

Plánování trajektorií

Každé letadlo je v systému reprezentováno vlastním agentem, který má na starosti plánování letových trajektorií. Každý agent může kdykoliv zavolat plánovač, aby naplánoval novou trajektorii. Taková situace nastává například v okamžiku, kdy letadlo obdrží nové pokyny, je detekována možná kolize dvou nebo více letadel nebo dojde k vychýlení letadla od naplánované trajektorie. V systému je implementováno několik různých plánovačů, mezi kterými je možné dle potřeby přepínat. Každý z plánovačů vrací pouze plány se spojitou trajektorií, které zároveň respektují fyzikální omezení daného prostředku. Takovými omezeními jsou především minimální poloměr zatáčení, minimální a maximální rychlost, maximální úhel stoupání nebo maximální úhel klesání. Nejčastěji se pro letadla používá plánovač Accelerated A*, který byl taktéž vyvinut v Centru Agentních Technologií. Pro helikoptéry, které se mohou zastavit a stoupat či klesat vertikálně, je naopak vhodnější Hexa-gridový plánovač založený na šestiúhelníkových buňkách.

3.2

Accelerated A*

Algoritmus Accelerated A* [7] (AA*) vychází z původního algoritmu A*. Jedná se o informovaný algoritmus prohledávání stavového prostoru s překážkami, který využívá heuristických znalostí prostředí. Pro variantu ve spojitém prostoru je prostor diskretizován mřížkou. Hustota této mřížky se ve verzi Accelerated A* dynamicky mění podle vzdálenosti od překážek. Je-li nejbližší překážka daleko, používá algoritmus větší expanzní krok. Je-li naopak překážka blíže, krok se zmenšuje a dochází tak ke zmenšování mezery mezi překážkami a naplánovanou trajektorií. Zmenšováním kroku se také algoritmus postupně blíží optimálnímu

9

10

KAPITOLA 3. SYSTÉM AGENTFLY

řešení. Adaptivní změna kroku je dobře patrná na obrázku 3.1. Algoritmus se také snaží zkrátit již naplánovanou trajektorii pomocí vyhlazování.

Obrázek 3.1: Expandované uzly algoritmu AA*, zdroj [7]

3.3

Vyhýbání se kolizím

Řídicí systém AgentFly je vybaven několika moduly pro řešení kolizních situací letadel (Collision avoidance). Základní myšlenkou celého řešení je tzv. Free Flight Concept [8] publikovaný v roce 1997. V tomto konceptu si každé letadlo samo určí svou nejvhodnější trajektorii a možné kolize jsou detekovány a řešeny dle potřeby až v průběhu letu. To se velmi odlišuje od aktuálně používaného způsobu řízení letového provozu. V dnešní době je letový provoz řízen operátory, kteří využívají pevně daných letových koridorů. Z tohoto důvodu je trajektorie letu zbytečně prodlužována a dochází k ekonomickým ztrátám. Naopak v nově navrženém konceptu dochází k úhybným manévrům pouze v případě detekované kolize, a proto je průměrná délka letu kratší. V systému AgentFly probíhá detekce potenciálních kolizí distribuovaným způsobem, tedy mezi jednotlivými letadly samostatně. Systém implementuje několik metod řešení kolizních situací. Můžeme je rozdělit na dvě základní skupiny. První skupinou jsou nekooperativní algoritmy, u kterých mezi sebou letouny aktivně nekomunikují a pouze znají svou vzájemnou polohu. Mají proto pevně daná pravidla, podle kterých se řídí. Například, že se letouny vyhýbají vždy vpravo. Druhou skupinou jsou kooperativní algoritmy, u kterých si letouny vyměňují kompletní informace o budoucích letových plánech. Na jejich základě volí nejvhodnější úhybné manévry. Příkladem jednoduchého algoritmu je prioritní vyhýbání, kdy se vždy letadlo s nižší prioritou vyhýbá letadlu s vyšší prioritou.

3.4. MULTI-AGENTNÍ PLATFORMA AGLOBE

3.4

11

Multi-agentní platforma AGlobe

Systém AgentFly je založen na multi-agentní platformě AGlobe, která zajišťuje základní služby agentům. Celá architektura AGlobe je napsána v jazyce Java. Díky tomu je tato platforma schopna běhu na odlišných operačních systémech zároveň. To je důležitá výhoda pro implementaci do reálných letounů. Důležitou vlastností je také distribuovatelnost. Systém je možné spustit na větším počtu počítačů. Tím se celkový výpočetní výkon rozloží a je možné zvládnout i rozsáhlejší simulace. Architektura platformy AGlobe je znázorněna na obrázku 3.2. Komunikace mezi agenty je implementována pomocí zpráv.

Obrázek 3.2: Architektura platformy AGlobe

3.4.1

Součásti platformy AGlobe

Agent Agent je základní entitou celého systému. Může komunikovat s ostatními agenty. Agent container Dokáže spravovat větší množství agentů. Stará se o jejich spouštění a ukončování. Container Manager Zajišťuje spouštění, kontrolu a ukončování agentních kontejnerů. Message Transport Zajišťuje efektivní výměnu zpráv mezi agenty v různých kontejnerech. Java Virtual Machine Celá platforma funguje na virtuálním stroji v jazyce JAVA.

12

KAPITOLA 3. SYSTÉM AGENTFLY

Kapitola 4

Analýza a návrh řešení Hlavním cílem této práce bylo přizpůsobit algoritmus plánování trajektorií pro bezpilotní letouny tak, aby byly schopny dodržovat své naplánované trajektorie i v prostředí s působícím větrem. Vítr chápeme jako proudění vzduchu v atmosféře. Jedná se o meteorologický jev, který je způsoben rozdílem tlaků v různých místech atmosféry. Pro tuto práci je potřeba rozdělit si vítr na dvě důležité složky. Za první složku považujeme nárazy větru, které jsou značně nepředvídatelné. Proto nejsme schopni přizpůsobit trajektorii tak, abychom snížili jejich vliv na odchylku od naplánované trajektorie při letu. Jako druhou složku chápeme stálejší proudění vzduchu, které se za delší časovou periodu téměř nezmění. Pro naše potřeby můžeme uvažovat periodu jedné minuty. Směr a rychlost takového proudění se mění výrazně pomaleji než je tomu u první složky, proto jsme schopni využít naměřené hodnoty v plánovacím procesu bezpilotního letounu. V následujícím textu −−−→ budeme jako vítr chápat laminární proudění vzduchu vyjádřené konstantním vektorem wind. Při předpokladu, že známe vektor větru, můžeme vyjádřit vztah mezi vzdušnou rychlostí −−−−−−→ −−−−−−−−−→ airSpeed a pozemní rychlostí groundSpeed letounu 4.1. −−−−−−−−−→ −−−−−−→ −−−→ groundSpeed = airSpeed + wind

4.1

(4.1)

Matematický model letadla

Pro účely plánování si sestavíme matematický model letadla (4.2). Uvažujme nejprve pouze dvourozměrný prostor. Letoun tedy bude létat v rovině s konstantní výškou. Souřadnice x a y udávají polohu letounu a úhel θ směr jeho natočení v horizontální rovině. Proměnná v je rychlost letounu a u je rychlost zatáčení letounu v horizontální rovině. x˙ = v · cos θ

(4.2)

y˙ = v · sin θ θ˙ = u Model, který bude počítat s konstantním působením větru, bude vypadat téměř totožně jako model bez větru. Rozšíří se pouze o zmíněné působení větru. Tento model můžeme

13

14

KAPITOLA 4. ANALÝZA A NÁVRH ŘEŠENÍ

popsat rovnicemi (4.3). x˙ = v · cos θ + windx

(4.3)

y˙ = v · sin θ + windy θ˙ = u Všechny běžné letouny jsou omezeny svým minimálním poloměrem zatáčení ρ. Nemohou tedy zatáčet libovolně a toto omezení můžeme zapsat vzorcem (4.4).  u∈

4.2

v v − , ρ ρ

 (4.4)

Tvar zatáček

Nyní budeme zkoumat jaké mají letouny manévrovací schopnosti v prostředí s větrem. Nejvíce zajímavý bude případ, kdy letoun zatáčí s největším možným úhlem náklonu. V prostředí bez větru by letoun stále dokola opisoval kružnici. Započítáme-li ovšem vliv větru, letoun začne být unášen jeho směrem a trajektorie se změní z kružnice na prodlouženou cykloidu. Prodloužená cykloida je transcendentní cyklická křivka, kterou je možné zapsat pomocí rovnic (4.5) a platí pro ně d > a. x = d cos(α) + a t

(4.5)

y = d sin(α) Existují pouze dva parametry, které ovlivňují tvar této cykloidy. Prvním je minimální poloměr zatáčení ρ a druhým poměr rychlosti větru a rychlosti letounu β, který může být zapsán jako vzorec (4.6). Je zajímavé, že váha letounu nemá na tvar trajektorie žádný přímý vliv. Může pouze ovlivňovat minimální poloměr zatáčení. Váha letounu může naopak hrát významnou roli při výskytu poryvů větru, ale ty v našem matematickém modelu neuvažujeme. −−−→ wind → − β = −−−−−−→ ; airSpeed

− → β = β

(4.6)

Budeme vycházet z matematického modelu (4.3) a budeme uvažovat maximální možné zatáčení letounu doleva. Získáme rovnice (4.7). vt x = x0 + ρ cos + windx t (4.7) ρ vt y = y0 + ρ sin + windy t ρ Tyto rovnice můžeme dále upravit substitucí úhlu natočení místo času t = α/ρ a následovně pomocí vzorce (4.6). Z výsledných rovnic (4.8) je dobře patrné, že tvar trajektorie závisí pouze na poloměru zatáčení ρ a poměru rychlosti větru vůči rychlosti letounu β. x = x0 + ρ cos α + ρ βx α y = y0 + ρ sin α + ρ βy α Příklady tvarů zatáčky pro různé rychlosti větru jsou znázorněny na obrázku 4.1.

(4.8)

4.3. ZMĚNA SMĚRU LETU

15

wind

wind

(a) β = 0,1

(b) β = 0,3

Obrázek 4.1: Vliv konstantního větru na tvar trajektorie

4.3

Změna směru letu

V prostředí s konstantním větrem se bude měnit i směr letu. Tento fakt je dobře patrný −−−−−−→ ze vzorce (4.1), kde směr vektoru airSpeed určuje směr natočení letounu v horizontální rovině −−−−−−−−−→ a směr vektoru groundSpeed určuje směr pohybu letounu vůči pozorovateli na zemi. Pokud známe úhel natočení letounu a jeho rychlost je výpočet směru pohybu jednoduchý. Použijeme zde vektorový součet. Pokud ovšem známe pouze směr pohybu a vzdušnou rychlost letounu je výpočet natočení letounu angleXY komplikovanější. Při výpočtu natočení letounu budeme vycházet z obrázku 4.2. y

wind

d|

ee

ou gr

pee

d

sp

ai r

rs

m

|ai

d

spe ed

n

angleXY x

Obrázek 4.2: Geometrický význam úhlu angleXY

Nejprve si zadefinujme proměnné m a n ze vzorce (4.9). Jejich geometrický význam je dobře patrný z obrázku 4.2. −−−−−−−−−→ groundSpeed =m+n

(4.9)

Délku n vypočteme pomocí skalárního součinu vektoru větru a normalizovaného vektoru rychlosti vůči zemi, viz vzorec (4.10). −−−→ −−−−−−−−−→ wind · groundSpeed n= −−−−−−−−−→ groundSpeed

(4.10)

16

KAPITOLA 4. ANALÝZA A NÁVRH ŘEŠENÍ

Dvojitým použitím Pythagorovy věty dostaneme vztah (4.11). q −−−−−−−−−→2 −−−→2 m = groundSpeed + b2 − wind (4.11) −−−−−−−−−→ Ze vzorců (4.2) až (4.11) jsme vypočítali velikost vektoru groundSpeed, jeho směr známe, a proto můžeme výsledek dosadit do rovnice (4.12). −−−−−−→ −−−−−−−−−→ −−−→ airSpeed = groundSpeed − wind

(4.12) −−−−−−→ Známe již vektor airSpeed a zbývá vypočítat jeho směr. To provedeme pomocí vzorce (4.13).   airSpeedy (4.13) angleXY = arctan airSpeedx

4.4

Možnosti řešení

V původní implementaci plánovacích algoritmů se s působením větru nepočítá. Protože jsou původní algoritmy založeny na hledání Dubinsových křivek, jsou všechny trajektorie plánované jako sekvence rovných úseček a zatáček s konstantním poloměrem. Pokud poletí letoun po takovéto trajektorii ve větru, bude se to jevit, jako že neletí po kružnicích, ale po prodloužených cykloidách. Efektivní poloměr zatáčení se bude měnit a může se stát, že letoun nezvládne proletět zatáčku. Nejhorší manévrovatelnost má letoun v okamžiku, kdy je jeho orientace stejná jako je směr působícího větru. Nabízejí se zde dvě základní řešení. Prvním je zvětšení plánovaného poloměru zatáčení tak, aby letoun měl dostatečnou rezervu a zvládl zatáčku proletět. Druhým řešením je změna tvaru zatáček již při plánování.

4.4.1

Zvětšení minimálního poloměru zatáčení

Zvětšení poloměru zatáčení při plánování je nejjednodušší řešení. Implementace je velmi jednoduchá, a proto se toto řešení ze začátku používalo. Původní algoritmy jsou postaveny na Dubinsových křivkách, což jsou nejkratší možné trajektorie pro letouny mezi dvěma orientovanými body na ploše. Stačilo tedy změnit jednu konstantu, která udává minimální poloměr zatáčení. Čím větší je rychlost větru, tím více je potřeba zvětšit poloměr zatáčení letounu. Toto řešení funguje spolehlivě, ale se zvyšováním poloměru zatáčení rapidně klesá manévrovatelnost letounu. Proto bylo potřeba najít jiné řešení.

4.4.2

Změna tvaru zatáček

Druhé řešení mění tvar zatáček tak, aby byly co nejvhodnější do prostředí s konstantním větrem. Nejvhodnější křivkou pro zatáčení je prodloužená cykloida, protože se jedná o zatáčku po kružnici zobrazenou ve vztažné soustavě pohybující se rychlostí větru. Takový způsob řešení byl navrhnut například již ve článku [9]. Jednoznačnou výhodou tohoto řešení je, že nesnižuje manévrovatelnost letounu. Nevýhody jsou složitější implementace a větší výpočetní náročnost. Příklad tohoto řešení je na obrázku 4.3. Vlevo je původní řešení pomocí Dubinsových křivek a napravo je nové řešení ve větru s využitím prodloužených cykloid. Pro svoji práci jsem si zvolil právě toto řešení, protože lépe odpovídá realitě.

4.4. MOŽNOSTI ŘEŠENÍ

17

wind

no wind

v2

v2 v1

v1

(a) bez větru

(b) s věrem β = 0,25

Obrázek 4.3: Porovnání nejkratší možné trajektorie bez větru a s světrem

18

KAPITOLA 4. ANALÝZA A NÁVRH ŘEŠENÍ

Kapitola 5

Plánování trajektorie s větrem V této kapitole se budeme zabývat hledáním trajektorie pro bezpilotní letouny v prostředí s konstantním větrem. Zvolené řešení bylo naznačeno v předchozí kapitole a vychází z Dubinsových křivek, které upravíme tak, aby zohledňovaly působení větru. Budeme předpokládat, že optimální trajektorie se opět bude skládat pouze z rovných úseků a zatáček s minimálním možných poloměrem. V případě působení větru budou mít zatáčky tvar prodloužené cykloidy namísto kružnice. Výhodou tohoto řešení je, že se snižující se rychlostí větru k nule, se z cykloid opět stávají kružnice a naše řešení bude totožné s řešením založeným na Dubinsových křivkách. Původní implementace plánování trajektorie bez překážek povolovala letounům pouze zatáčet nebo měnit výšku, ale nikoli oboje zároveň. Používaly se pouze čtyři manévry typu CSC (RSR, LSL, RSL, LSR), kde se střední rovný úsek S využíval ke změně letové hladiny. Manévry typu CCC (RLR, LRL) nebyly implementovány proto, že nemohly měnit výšku letu a také protože jsou optimální pouze pro krátké vzdálenosti a často při plánování nejsou potřeba. Z toto vyplývá, že pro malé vzdálenosti startovního a cílového bodu nemusela být původní implementace optimální. Příklady rozložení manévru CCC můžeme nalézt na obrázku C.1 v příloze C. Při implementaci vlastního algoritmu hledajícího trajektorii mezi dvěma orientovanými body budeme používat stejné principy, ale upravíme řešení pro prostředí s konstantním větrem. Budeme i nadále používat pouze manévry typu CSC. Každý manévr obsahuje jeden či několik elementů. Pod pojmem element chápeme úsek naplánované trajektorie se stejnými vlastnostmi, například zatáčka nebo rovný úsek.

5.1

Elementy ve větru

Původní elementy musely být upraveny tak, aby počítaly s působením větru. Budeme používat tři základní elementy, a to rovný úsek, horizontální zatáčku a vertikální zatáčku. Hlavní úpravou rovného elementu byla změna rychlosti způsobená působícím větrem. Naopak element horizontální zatáčky bylo potřeba vytvořit zcela odlišně od původního řešení založeného na implementaci Dubinsových křivek.

19

20

5.1.1

KAPITOLA 5. PLÁNOVÁNÍ TRAJEKTORIE S VĚTREM

Element horizontální zatáčky

Je třeba vytvořit element, který bude představovat zatáčky v horizontální rovině. Tvar jeho křivky bude prodloužená cykloida, která při nulovém větru přechází zpět v kružnici. V naší implementaci budeme tento element popisovat pomocí parametrických rovnic (5.3). Parametr zatočení α uvažujeme v intervalu (5.1) určeným počátečním úhlem otočením v horizontální rovině startAngle a úhlem zatočení angle, který je kladný pro zatáčku doleva a záporný pro zatáčku doprava. α ∈ h startAngle, startAngle + angle i

(5.1)

Poloměr zatáčení určuje proměnná radius. Dále jsou zde proměnné b a d, které popisují vliv větru. Ve vzorci (5.2) je pro jejich výpočet použito znaménko ±. To je zde proto, protože při zatáčce vlevo je potřeba použít kladné znaménko a doprava záporné. b = ± radius βx = ± radius · windx /speed

(5.2)

d = ± radius βy = ± radius · windx /speed Proměnné a, c určují pozici zatáčky a konstanta z0 její výšku. x = a + b α + radius cos(α)

(5.3)

y = c + d α + radius sin(α) z = z0 Počáteční parametr startAngle vypočítáme odečtením stejné hodnoty pro zatáčku doprava, viz vzorec (5.4).

π 2

startAngle = angleXY ±

5.2

pro zatáčku doleva a přičtením π 2

(5.4)

Plánování trajektorie bez překážek

Při plánování trajektorie mezi dvěma orientovanými body bez překážek budeme postupovat obdobně jako v původní implementaci. Používat budeme pouze manévry typu CSC, pro které existují čtyři možnosti. Proto budeme postupovat tak, že vypočítáme délky všech čtyř možných trajektorií a následně použijeme nejkratší z nich. Způsob výpočtu trajektorie se bude výrazně lišit v případě, kdy uvažujeme obě zatáčky se stejnou orientací (RSR, LSL) a kdy uvažujeme zatáčky s rozdílnou orientací (RSL, LSR). Příklady se shodnou a rozdílnou orientací zatáčení můžeme nalézt na obrázku 5.1.

5.3

Shodná rotace

Prvním případem je manévr se shodnou orientací obou zatáček. Příklad takového manévru můžeme nalézt na obrázku 5.1a. Při inicializaci takového manévru budeme využívat toho, že tvar obou křivek popisujících zatáčku je shodný, a tyto křivky jsou pouze posunuté. → − Při hledání středního rovného úseku ve skutečnosti hledáme vektor posunutí t , který je se

5.4. ROZDÍLNÁ ROTACE

21

wind

wind S1 S2

v2 v1

S3

v1

v2

S4 S5

(a) Shodná rotace

(b) Rozdílná rotace

Obrázek 5.1: Příklady manévrů vě větru

středním úsekem rovnoběžný a má dokonce i stejnou velikost. Máme tedy dvě prodloužené cykloidy stejné orientace zapsané pomocí vzorců (5.3). První zatáčku budeme označovat → − indexem 1 a druhou zatáčku indexem 2. Při výpočtu vektoru t budeme vycházet z předpokladu, že trajektorie má být spojitá. Proto musí být rovný úsek tečný na obě cykloidy. Konec první zatáčky budeme označovat parametrem α1 a začátek druhé zatáčky parametrem α2 . Směr trajektorie v obou bodech musí být shodný. Z vlastností prodloužené cykloidy proto vyplývá, že musí platit rovnice (5.5). α1 = α2 + 2 k π

(5.5)

→ − Vektor t můžeme vypočítat dosazením rovnice (5.5) do rovnic (5.3) popisujících elementy ve větru. Využijeme zde faktu, že se jedná o zatáčky se stejným směrem rotace a proto platí b1 = b2 a d1 = d2 . Získáme tak rovnice (5.6). tx = a2 − a1 + n b 2 π

(5.6)

ty = c2 − c1 + n d 2 π Prodloužená cykloida je periodická funkce, a proto bude existovat nekonečný počet mož→ − ných vektorů t . Vybírat budeme pouze taková řešení, která mají součet úhlů obou zatáček maximálně 360◦ .

5.4

Rozdílná rotace

Při výpočtu manévru s rozdílnou orientací zatáček využijeme především toho, že obě prodloužené cykloidy, které reprezentují zatáčky, jsou středově souměrné podle bodů Sn . Tento fakt je také dobře patrný z obrázku 5.1b. Budeme opět hledat střední rovný úsek, který je tečný k oběma křivkám zároveň. Při hledání středů symetrie Sn vycházíme z předpokladu,

22

KAPITOLA 5. PLÁNOVÁNÍ TRAJEKTORIE S VĚTREM

že natočení letounu musí být v obou bodech popsaných pomocí parametrů α1 a α2 shodný. Směr zatáčení je opačný a proto pro tyto parametry budou platit například vztahy (5.7). α1 = 0

α2 = (2 n + 1)π

(5.7)

Rovnice (5.7) dosadíme do původních rovnic (5.3). Při výpočtu je třeba si uvědomit, že jde o zatáčky s opačnou orientací a proto bude platit b1 = −b2 a d1 = −d2 . Výsledný tvar zapíšeme do rovnic (5.8). Z nich vyplývá, že středů symetrie je opět nekonečně mnoho. Sx = Sy =

a2 + a1 + n b (2 n + 1) π 2 c2 + c1 + n d (2 n + 1) π 2

(5.8)

Nalezli jsme rovnice, které popisují všechny středy symetrie mezi oběma zatáčkami. Nyní musíme určit parametr α1 , pro který platí, že tečna ke křivce vedená tímto bodem prochází vybraným středem symetrie S. Vybíráme pouze takové středy S, aby součet úhlů obou zatáček byl maximálně 360◦ .

5.4.1

Hledání tečny zadaným bodem k cykloidě

Máme orientovanou křivku, prodlouženou cykloidu, ke které budeme hledat navazující rovný úsek, který má procházet zadaným středem symetrie S. Pro lepší přehlednost textu si zavedeme pojem orientovaná tečna. Orientovaná tečna je polopřímka, která začíná v nějakém bodě na křivce a pokračuje ve směru tečném ke křivce s ohledem na její orientaci. Množina bodů orientované tečny je podmnožinou bodů tečny. Nyní můžeme úlohu formulovat jako hledání orientované tečny procházející zadaným bodem S. Orientace tečny zajišťuje, že se nezmění směr letu a letadlo nezačne ”couvat”. Zajímají nás pouze manévry, které mohou být optimální, a proto budeme předpokládat, že úhel zatáčky nemůže být větší než 360◦ . Hledání tečny k cykloidě zadaným bodem nelze řešit algebraicky, ale je nutné jej řešit numericky. Zavedeme si proto funkci f (α), která vrací vzdálenost bodu S od tečny z bodu popsaného parametrem α. Při výpočtu využíváme vektorového součinu směru křivky v zadaném bodě a pozice bodu S vůči zadanému bodu na křivce, viz vzorec (5.9). Znaménko návratové funkce se mění podle toho, jestli se bod S nalézá po pravé straně ve směru křivky nebo naopak. Při výpočtech budeme předpokládat, že pro tečnu, která prochází bodem S, je hodnota funkce f (α) nulová. Pro správnou funkci numerických algoritmů je důležité nalézt interval, ve kterém má rovnost f (α) = 0 právě jedno řešení. Funkce f (α) je spojitá, takže můžeme říci, že hledáme interval, ve kterém její hodnota přechází přes nulu právě jednou. −−−−−−→ f (α) = direction(α) × (S − position(α))

(5.9)

Pro interval 360◦ může existovat více hodnot α, ve kterých je funkce f (α) nulová. Proto si tento interval rozdělíme na čtyři části po 90◦ . V jednotlivých částech mohou existovat maximálně dvě řešení, pro která je funkce f (α) nulová. Funkce je spojitá, a proto při dvou přechodech přes nulu budou mít přechody navzájem opačnou orientaci. Z toho důvodu může

5.4. ROZDÍLNÁ ROTACE

23

v intervalu 90◦ existovat maximálně jedna orientovaná tečna procházející bodem S. Na obrázku 5.2a můžeme vidět barevně označené oblasti, ve kterých se musí nacházet bod S, aby existovala orientovaná tečna v intervalu < αA , αb >, která prochází bodem S. Body A a B jsou definovány pomocí parametrů αA a αb . Zadefinujme si dvě nové funkce. První funkce isLef t(α, S) vrací informaci, jestli je bod S na levé straně od tečny z bodu určeného parametrem α. Používá stejný výpočet jako funkce f (α). Druhou funkcí je funkce inT riangle(αA , αB , S), která vrací informaci, zda je bod S v trojúhelníku tvořeném tečnami z bodů A, B a úsečkou AB. Na obrázku 5.2a oblast s touto vlastností odpovídá oranžově vyznačené II. oblasti. Pomocí těchto dvou nově definovaných funkcí si můžeme na obrázku 5.2a rozdělit plochu do několika oblastí. Jednotlivé oblasti znázorňují, jestli v intervalu < αA , αb > existuje orientovaná tečna, která prochází bodem S z této oblasti. Pro lepší názornost je zde tečkovaně znázorněno několik možných orientovaných tečen. V červené I. oblasti nemůže orientovaná tečna existovat. V oranžové II. oblasti může orientovaná tečna existovat, ale také nemusí. V zelené III. oblasti existuje právě jedna orientovaná tečna. Abychom rozhodli, jestli existuje orientovaná tečna pro bod v II. oblasti, rozdělíme interval < αA , αb > na dvě poloviny. Prostřední bod C definujeme parametrem αC . Grafické znázornění můžeme vidět na obrázku 5.2b. Pomocí návratových hodnot funkcí isLef t(αC ), inT riangle(αA , αC , S) a inT riangle(αC , αB , S) se nám původní trojúhelník II. oblasti rozdělí na čtyři nové trojúhelníky. Obdobně jako v předchozím případě pro body v červeném trojúhelníku s číslem 1 neexistuje orientovaná tečna. Pro body v zeleném trojúhelníku s číslem 3 existuje právně jedna orientovaná tečna. Pro body v trojúhelnících s číslem 2 může existovat orientovaná tečna. Pro rozhodnutí, jestli tečna existuje, si opětovně rozdělíme interval na dvě poloviny a opakujeme postup než existenci orientované tečny potvrdíme nebo vyvrátíme.

3 C

III.

I.

1

II. A

I.

A B

(a) Prvotní rozdělení

2

B

(b) Půlení intervalu

Obrázek 5.2: Oblasti bodů s existencí orientované tečny Tento postup můžeme zapsat také pomocí pseudokódu jako algoritmus 1. Zápis je uveden pouze pro pravotočivou zatáčku. Pro levotočivou zatáčku je algoritmus totožný, pouze se používá negace funkce isLef t(). Díky tomuto algoritmu jsme našli interval, ve kterém má původní funkce f (α) = 0 právě jedno řešení. V nalezeném intervalu můžeme spustit numerickou metodu hledání kořene.

24

KAPITOLA 5. PLÁNOVÁNÍ TRAJEKTORIE S VĚTREM

Algorithm 1 Existence orientované tečny procházející bodem S (pravotočivá zatáčka) 1: 2: 3: 4: 5: 6: 7: 8: 9: 10: 11: 12: 13: 14: 15: 16: 17: 18: 19: 20: 21: 22: 23: 24: 25: 26: 27:

procedure findSubinterval(αA , αB , S) . Vrací interval, ve kterém existuje řešení if isLef t(αA , S) then return N U LL . Je v I. sektoru, řešení neexistuje end if if isLef t(αB , S) then return [αA , αB ] . Je v III. sektoru, řešení existuje end if if inT riangle(αA , αB , S) then . Je v II. sektoru, řešení může existovat while n < 50 do αC = (αA + αB )/2 . Půlení intervalu if isLef t(αC , S) then return [αA , αC ] . Řešení existuje v první polovině intervalu else if inT riangle(αA , αC , S) then αB = αC . Řešení může existovat v první polovině intervalu else if inT riangle(αC , αB , S) then αA = αC . Řešení může existovat v druhé polovině intervalu else return N U LL . Je v části 2, řešení neexistuje end if end if end while return N U LL . Chyba, algoritmus nekonverguje else return N U LL . Je v I. sektoru, řešení neexistuje end if end procedure

5.5

Optimalita při plánování v rovině

Důležitou otázkou je, jestli jsou nalezené manévry optimální. Používáme pouze manévry CSC, a proto víme, že v určitých oblastech nebudou optimální ani při plánování bez větru. V těchto oblastech jsou optimálními manévry CCC se třemi zatáčkami. Tato oblast je do vzdálenosti cíle rovné čtyřem minimálním poloměrům zatáčení ρ od počáteční polohy. Důkaz tohoto tvrzení můžeme nalézt například ve článku [3]. Graficky si jej také můžeme ověřit na obrázcích C.1 v příloze C. Vyslovíme tedy hypotézu, že pokud jsou Dubinsovy manévry CSC optimální od určité vzdálenosti cíle, budou i manévry CSC ve větru optimální od určité vzdálenosti cíle. Abychom mohli dokázat tuto hypotézu, použijeme koncept Moving Virtual Target (pohybující se virtuální cíl) publikovaný v článku [10]. Koncept je založen na představě, že letadlo letí v bezvětrném prostředí a naopak cílový bod se pohybuje rychlostí opačnou k rychlosti větru. Jinak řečeno zvolíme vztažnou soustavu pohybující se rychlostí větru. Díky tomu můžeme Dubinsovy křivky opět považovat za optimální trajektorie. Ve zmíněném článku

5.5. OPTIMALITA PŘI PLÁNOVÁNÍ V ROVINĚ

25

zavedli autoři proměnnou d, což je vzdálenost, kterou urazil virtuální cíl. Dále definují dvě funkce Ta (d) a Tvt (d), kde Ta (d) je čas potřebný k dosažení cíle letounem pomocí optimální trajektorie a Tvt (d) je čas, který potřebuje virtuální cíl, aby se posunul do vzdálenosti d. Autoři článku také zavádějí funkci G(d) jako rozdíl Ta (d) a Tvt (d), viz vztah (5.10). Optimální trajektorie ve větru bude taková, pro kterou bude platit rovnost G(d) = 0, pro nejmenší možné d. def

G(d) = Ta (d) − Tvt (d)

(5.10)

Funkce G(d) je po částech spojitá a všechny nespojitosti se vyskytují v oblastech, kde Dubinsovy manévry CCC mohou být optimální. To může nastat pouze v případě, že je cíl vzdálen maximálně 4ρ od startovní pozice letounu. Autoři ukázali, že pokud dojde k nespojitosti ve funkci G(d) právě při přechodu přes nulovou hodnotu, nemusí optimální manévr patřit mezi Dubinsovy křivky. Pro naše úvahy je důležité, že pokud nedojde k nespojitosti, jsou Dubinsovy křivky optimální i při zpětném zobrazení do vztažné soustavy spojené se zemí. Budeme se zajímat, v jakých polohách cíle je námi sestavený manévr ve větru optimální. Ze znalostí Dubinsových křivek víme, že optimální trajektorie může mít minimální délku lmin rovnou přímému manévru a maximální délku lmax rovnou přímému manévru s jednou celou zatáčkou. Pomocí těchto dvou délek můžeme vypočítat minimální a maximální velikost posunutí d, viz obrázek 5.3.

P1

dmax

P2

VT

dmin lmin wind

lmax S

Obrázek 5.3: Interval posunutí virtuálního cíle

Pokud virtuální cíl (VT) v intervalu od dmin do dmax nezasáhne do kružnice s poloměrem 4ρ od startovní pozice, bude nalezený manévr CSC optimální i v prostředí s konstantním větrem. Jako výchozí použijeme vzorec (5.11), kde β je poměr rychlosti větru vůči rychlosti letounu a byl popsán vzorcem (4.6). d=l·β

(5.11)

26

KAPITOLA 5. PLÁNOVÁNÍ TRAJEKTORIE S VĚTREM

Budeme zkoumat hraniční případy, kdy je vzdálenost virtuálního cíle od počátku přesně 4ρ. Pro tento případ je interval délky optimálního manévru popsán rovnicemi (5.12). lmin = 4 ρ,

lmax = 4 ρ + 2 π ρ

(5.12)

Dosazením vzorců (5.12) do vzorce (5.11) dostaneme rovnice (5.13) popisující možný interval proměnné d pro tento hraniční případ. dmin = 4 ρ β,

dmax = (4 ρ + 2 π ρ) β

(5.13)

Díky předcházejícím rovnicím můžeme sestavit oblast, ve které nemusí být nalezené manévry CSC v prostředí s větrem optimální. Graficky je tato oblast znázorněna na obrázku 5.4 oblastí pojmenovanou NEOPTIMÁLNÍ. Pokud se pozice cíle VT (původní poloha virtuálního pohybujícího se cíle) nachází mimo tuto oblast, je vždy některý z manévrů CSC optimální i v prostředí s konstantním větrem. wind

VT

4

dmin

VT

dmax -4

dmax

dmin

0

4

x [R] 4R

NEOPTIMÁLNÍ

OPTIMÁLNÍ

-4 y [R]

Obrázek 5.4: Zóna neoptimálních manévrů

5.6

Rozšíření na 3D prostor

Budeme chtít, aby bylo možné upravené Dubinsovy křivky použít i v třídimenzionálním prostoru. Zavedli jsme si již dříve pravidlo, že letoun může pouze zatáčet v horizontální rovině nebo měnit svou výšku, ale nikoliv oboje zároveň. Nejprve vypočteme obě horizontální zatáčky upraveného Dubinsova manévru. Každá může být v jiné výšce, a proto je potřeba udělat vertikální změnu výšky. K tomu využijeme střední rovný úsek. V tomto úseku necháme letoun měnit svou výšku tak, že nejprve bude následovat vertikální zatáčka, která změní směr letounu nahoru nebo dolů, rovný element a následně vertikální zatáčka s opačnou velikostí. Musíme zde zohlednit maximální úhel stoupání a klesání letounu. Pokud je tedy střední úsek příliš krátký, nestihne letoun dostatečně změnit svou výšku a tento manévr bude

5.7. ÚPRAVA ALGORITMU ACCELERATED A*

27

nerealizovatelný. V takovém případě musíme nalézt trajektorii jiným způsobem, například prohledáváním stavového prostoru pomocí algoritmu Accelerated A*. Příklad manévru se změnou výšky je na obrázku 5.5.

zm

ěna

y ýšk

h o riz o n t á l

ní z a

v

v1

tá č

ka

v2

wi nd

Obrázek 5.5: Rozšíření manévru do 3D prostoru

5.7

Úprava algoritmu Accelerated A*

Algoritmus Accelerated A* (AA*) na hledání cesty v prostředí s překážkami nebylo potřeba výrazněji upravovat. Upravili jsme pouze jednotlivé manévry tak, aby počítaly s vlivy větru. V původní implementaci byla délka Dubinsových křivek použita jako heuristická funkce. Použili jsme stejný koncept, ale upravili jsme Dubinsovy křivky do prostředí s konstantním větrem. Tyto manévry nemusí být optimální, a proto ani nalezené řešení pomocí algoritmu AA* nemusí být optimální. Rozdíl v délce manévrů CSC od těch optimálních není příliš velký a projevuje se pouze v blízkosti cíle. Z provedených pokusů vyplývá, že chyba způsobená nejmenším krokem expanze ve stavovém prostoru je řádově stejná, a proto se tímto problémem nebudeme dále zabývat. Z textu výše je také zřejmé, že pro velké změny výšky nemusí upravená Dubinsova křivka existovat. Zavádíme zde proto náhradní heuristickou funkci, která odhaduje nejmenší možnou délku středního úseku aby letoun stačil změnit výšku.

5.7.1

Komprese letových plánů

Při hledání cesty v komplikovanějších prostředích s vyšším počtem překážek obsahují naplánovány trajektorie vyšší počet letových elementů. Pokud se jedná o elementy v prostředí s konstantním větrem, nese si každý element informaci o vektoru větru, pro který byl naplánován, rychlosti letounu a případně i poloměru zatáčení. To jsou informace, které jsou pro větší počet elementů stejné. Trajektorie se relativně často vyměňují mezi letouny, a proto je nutné zajistit co nejmenší objem přenášených dat. Z tohoto důvodu byla implementována metoda, která posílá často se opakující informace pouze jednou nebo při jejich změně.

28

KAPITOLA 5. PLÁNOVÁNÍ TRAJEKTORIE S VĚTREM

Kapitola 6

Testování navržených algoritmů Důležitým aspektem každého algoritmu je jeho výpočetní náročnost. V této kapitole je popsáno testování rychlosti původních a upravených algoritmů. Testování bylo prováděno na běžném počítači a počítači Gumstix, který je na palubě letounu Procerus. Pro nasazení na reálný bezpilotní prostředek je důležité, aby bylo možné plánovat trajektorie letu autonomně přímo na palubě letounu.

6.1

Výkonové testy výpočtu manévrů

Prvním experimentem je test časové náročnosti výpočtu trajektorie mezi dvěma orientovanými body v prostředí bez překážek. V tomto experimentu porovnáváme původní implementaci založenou na výpočtu Dubinsových křivek s její upravenou verzí pro prostředí s konstantním větrem. Hledání trajektorie probíhá pouze v rovině, start a cíl leží ve stejné výšce. Poloha a směr obou bodů je generována pseudonáhodnou funkcí v jazyce Java. Abychom vyloučili vlivy náhodných jevů, je testování prováděno na jednom milionu náhodně vygenerovaných konfigurací. Výkonnostní testy jsou prováděny na běžném počítači 1 a palubním počítači Gumstix 2 . Testy probíhaly v prostředí s následujícími parametry: • pozice startu a cíle: x, y ∈ < −200 m, 200 m >; • natočení startu a cíle: natočení je vybráno náhodně; • poloměr zatáčení je uvažován 45 m. Každý vygenerovaný případ byl testován na obou počítačích. Výsledky experimentu jsou zaznamenány v tabulce 6.1. Je zde vidět, že upravené algoritmy pro výpočet trajektorie v prostředí s konstantním větrem jsou přibližně 11 krát pomalejší než původní implementace. Toto zpomalení odpovídá našim očekáváním, protože se v algoritmu používají náročnější výpočty, především algoritmus na hledání tečen k prodloužené cykloidě procházející zadaným bodem popsaný v sekci 5.4.1. Dále jsme naměřili menší výpočetní výkon palubního počítače Gumstix. V porovnání s běžným počítačem byl přibližně 15 krát pomalejší. To je způsobeno 1 2

PC - CPU Intel Core i5 2.67 GHz, RAM 8 GB 1066 MHz , Windows 7 Professional 64-bit Gumstix - Overo AirSTORM, CPU ARM Cortex-A8 800 MHz, RAM 512 MB, Linux overo 2.6.34 32-bit

29

30

KAPITOLA 6. TESTOVÁNÍ NAVRŽENÝCH ALGORITMŮ

především nižší taktovací frekvencí (přibližně 3 krát). Dalším důvodem může být jeho 32bitová architektura. Výpočty jsou totiž založeny na operacích s proměnnou desetinou čárkou a používáme zde 64-bitové proměnné typu double v jazyce Java. Typ prostředí Bez větru S větrem Porovnání

Běžné PC 2,794 µs 28,82 µs 10,3 krát

Gumstix 41,13 µs 459,7 µs 11,2 krát

Porovnání 14,7 krát 16,0 krát

Tabulka 6.1: Časová náročnost výpočtu jednoho manévru na vzorku 106 manévrů

6.2

Výkonové testy výpočtu trajektorií s překážkami

Druhým experimentem je test časové náročnosti hledání trajektorie v prostředí s překážkami pomocí algoritmu AA*. Tento test probíhal v prostředí s následující konfigurací: • start: souřadnice [0 m, 0 m], normalizovaný vektor směru [1, 0]; • cíl: souřadnice [1000 m, 0 m], normalizovaný vektor směru [1, 0]; • 1. překážka: souřadnice [450 m, 50 m], poloměr 80 m; • 2. překážka: souřadnice [630 m, -60 m], poloměr 70 m; • poloměr zatáčení 45 m.

Obrázek 6.1: Testovací konfigurace s nalezenou trajektorií

Vyhledávacímu algoritmus nemohl při testování měnit výšky letu. Nejmenší expanzní krok byl nastaven na 1,5 m. Tento krok se zvyšoval se zvyšující se vzdáleností od nejbližší překážky. Plánování trajektorie bylo testována na původní i upravené verzi algoritmu AA*. Abychom mohli porovnat výsledky obou algoritmů, jsou testy spouštěny v prostředí s nulovým větrem. V nově navržených algoritmech se tedy cykloidy redukovali zpět na kružnice.

6.3. SHRNUTÍ VÝSLEDKŮ

31

Dle očekávání byla naplánovaná trajektorie obou plánovacích algoritmů naprosto shodná. Tím jsme si zároveň ověřili správné fungování upravených algoritmů. Oba plánovače byly spouštěny na běžném počítači i palubním počítači Gumstix. Testovací konfigurace spolu s naplánovanou trajektorií je na obrázku 6.1. Naměřené výsledky nalezneme v tabulce 6.2. Při výkonnostních testech algoritmu Accelerated A* je nové řešení přibližně 5 krát pomalejší. Zpomalení je převážně způsobeno asi 10 krát pomalejším výpočtem heuristické funkce, neboli výpočtem upravených Dubinsových křivek pro prostředí s konstantním větrem. Její výpočetní výkon byl testován v sekci 6.1. Typ prostředí Bez větru S větrem Porovnání

Běžné PC 10,24 ms 59,20 ms 5,78 krát

Gumstix 216,4 ms 1057 ms 4,88 krát

Porovnání 21,1 krát 17,9 krát

Tabulka 6.2: Časová náročnost algoritmu AA*

6.3

Shrnutí výsledků

Z naměřených výsledků vyplývá, že nově navržené algoritmy pro plánování v prostředí s konstantním větrem jsou přibližně 5 až 10 krát pomalejší než původně implementované algoritmy. Na druhou stranu naplánované trajektorie se mnohem lépe hodí do prostředí s větrem. Z výsledků je dále patrný výrazně nižší výpočetní výkon počítače Gumstix. Oproti běžnému počítači je přibližně 15 až 20 krát pomalejší. Proto je možné počítat na palubě letounu pouze trajektorie v prostředí s menším počtem překážek. Implementace algoritmu AA* umožňuje i plánování v trojrozměrném prostoru, ale je ze zřejmých důvodů výpočetně náročnější. Proto, abychom mohli na palubním počítači plánovat trajektorie se změnou výšky, je nutné zvětšit základní expanzní krok algoritmu AA* oproti výpočtu na běžném počítači. Se zvyšujícím se krokem se však zvyšuje i chyba plánování.

32

KAPITOLA 6. TESTOVÁNÍ NAVRŽENÝCH ALGORITMŮ

Kapitola 7

Testování řešení na letounu Tato kapitola je věnována ověření navrženého konceptu plánování trajektorií v prostředí s konstantním větrem na reálných prostředcích. K tomuto byl využíván bezpilotní letoun Procerus. Nejprve jsou popsány jeho základní vlastnosti. Další část kapitoly se věnuje analýze naměřených trajektorií simulovaného a reálného letounu na upravených trajektoriích a jejich porovnání s výsledky původního řešení.

7.1

Technické parametry letounu Procerus

Jedná se o letoun s uspořádáním samokřídla, viz obrázek 7.1. Hlavní tělo je vyrobeno z modelářského EPP a má rozpětí křídla 150 cm. U náběžné hrany křídla jsou dvě místa určené pro Li-Pol baterie. Výdrž na jedno nabití je přibližně 20 minut. Křídlo je poháněno jedním střídavým modelářským motorem. Letoun je schopen letět rychlostí 14 až 22 m/s. Ovládání letounu je zajištěno pouze pomocí změny náklonu dvou křidélek na zadní straně křídla a otáček elektromotoru. Letoun nemá žádný podvozek a přistává přímo na louce nebo ve vyšší trávě. Ke vzletu se používá dlouhá tažná guma, která pomůže letounu nabrat potřebnou letovou rychlost a následně se od letounu oddělí.

Obrázek 7.1: Experimentální letoun Procerus

33

34

7.2

KAPITOLA 7. TESTOVÁNÍ ŘEŠENÍ NA LETOUNU

Autopilot KESTREL

Letoun je řízen automatickým systémem Kestrel Autopilot v2.4. Tento autopilot je navržen pro řízení bezpilotních letounů UAV a MAV. Váží pouhých 16,7 gramu a jeho rozměry jsou přibližně 51 x 35 x 12 mm. Jeho fotografii můžeme vidět na obrázku 7.2. Jádrem celého zařízení je 8-bitový procesor s taktováním 29 MHz. Autopilot je vybaven řadou senzorů. Především tříosým akcelerometrem a gyroskopem, které měří náklon a změnu orientace letounu. Dále je vybaven GPS senzorem, který slouží pro globální navigaci letounu. Ten je pak schopen samostatně plnit naplánovanou misi. Obsahuje i absolutní a diferenční senzor tlaku pro měření nadmořské výšky a vzdušné rychlosti letounu. Senzory tlaku jsou doplněny několika teplotními senzory pro korekci měření. Autopilot má přímé výstupy na obě serva ovládající křidélka a regulátor elektromotoru. Letoun s tímto autopilotem lze ovládat také ručně pomocí modelářské vysílačky.

Obrázek 7.2: KESTREL autopilot, zdroj [11]

V navigačním módu dostává autopilot naplánovanou trajektorii jako seznam bodů, které jsou zadány pomocí svých souřadnic, viz obrázek 7.3. Autopilot naviguje letoun tak, že vybere první bod a snaží se jím proletět. Když se letoun přiblíží k aktuálním navigačnímu bodu na určenou vzdálenost, považujeme tento bod za splněný a autopilot začne navigovat na další bod v pořadí. Vzdálenost, při které autopilot považuje bod za splněný, lze konfigurovat a při testování byla nastavena na 70 m. Na obrázku 7.3 je tato vzdálenost vyznačena červenou kružnicí.

7.3

Program Virtual Cockpit v2.6

Softwarový program Virtual Cockpit v2.6 je určen pro ovládání a komunikaci s autopilotem Kestrel. Komunikace je zajištěna pomocí bezdrátových datových modemů. V hlavním, zobrazeném na obrázku 7.4, okně tohoto programu můžeme zjistit například letový mód letounu, stav baterie, přesnou polohu i výšku. Dále je zde k dispozici mapa, na které je znázorněn pohyb letounu. Na této mapě můžeme také zadávat, kam má letoun letět nebo okolo kterého bodu a v jaké výšce má kroužit. Software dále umožňuje simulaci letu v režimu

7.3. PROGRAM VIRTUAL COCKPIT V2.6

X

X

X

35

X

X

X

trajektorie

aktuální bod

X X X X X X X X

letoun

Obrázek 7.3: Navigace letounu

Hardware-in-the-Loop. Při této simulaci se spojí autopilot přímo s počítačem a počítač autopilotovi simuluje stavy senzorů. Autopilot na ně reaguje jako by byl ve vzduchu a díky tomu je možné testovat téměř všechny funkce letounu na zemi, včetně připojeného palubního počítače.

Obrázek 7.4: Uživatelské okno programu Procerus Virtual Cockpit

Hlavními letovými módy autopilota jsou: RC mode při kterém je letoun ovládán manuálně pomocí standardní modelářské vysílačky;

36

KAPITOLA 7. TESTOVÁNÍ ŘEŠENÍ NA LETOUNU

Nav mode při kterém se letí podle trajektorie popsané pomocí sekvence bodů s konkrétními souřadnicemi; Loiter now mode při kterém letoun začne kroužit kolem své aktuální polohy.

7.4

Uživatelský počítač Gumstix

Pro potřeby výzkumu je letoun Procerus vybaven uživatelským počítačem Gumstix Overo AirSTORM, viz obrázek 7.5. Jedná se o malý výpočetní modul založený na 32bitovém procesoru ARM Cortex-A8. Jeho taktovací frekvence je 800 MHz a má přístup k 512 MB RAM paměti a 512 MB NAND paměti. Dále je zde SD slot, který umožňuje výrazně rozšířit celkovou kapacitu NAND paměti. Je také možné využít vestavěného Wi-Fi a Bluetooth modemu. Počítač disponuje několika sériovými porty, z nich jeden je například využit pro komunikaci s Kestrel autopilotem.

(a) Vrchní strana

(b) Spodní strana

Obrázek 7.5: Uživatelský počítač Gumstix [12]

7.5

Způsob testování na letounu

Při testování konceptu plánování v prostředí s konstantním větrem jsme porovnávali naměřenou trasu letounu při navigaci po naplánované trajektorii. Trajektorii byla plánována vždy původními i nově navrženými algoritmy. Našim cílem bylo ukázat, že odchylka letounu od trajektorie naplánované upravenými algoritmy bude nižší. Pro testování jsme měli k dispozici bezpilotní letoun Procerus a jeho simulovaný fyzikální model.

7.6

Experimenty se simulovaným letounem

Nejprve jsme provedli experimenty se simulovaným letounem. Zvolili jsme si tři body WP1 až WP3, kterými má letoun prolétat. Mezi zadanými body si palubní počítač naplánoval trajektorii, která se stále dokola opakovala. Nejprve byly použity původní algoritmy, které s vlivy větru nepočítaly. Následně se experiment opakoval s novými algoritmy určenými pro plánování s větrem. Rychlost letounu byla nastavena na 15 m/s a rychlost větru na 5 m/s. To znamená, že poměr rychlosti větru vůči rychlosti letounu byl β = 0, 33.

7.6. EXPERIMENTY SE SIMULOVANÝM LETOUNEM

37

Testovací scénář • rychlost letounu 15 m/s; • vektor větru [-5 m/s, 0 m/s]; • poloměr zatáčení 45 m; • souřadnice 1. bodu WP1 [0 m, 0 m]; • souřadnice 2. bodu WP2 [150 m, 0 m]; • souřadnice 3. bodu WP3 [100 m, 300 m]. Naměřené výsledky jsou na obrázcích 7.6. Naplánované trajektorie jsou označeny červeně a naměřené trajektorie modře. Na levém obrázku 7.6a je záznam letu s trajektorií naplánovanou původními algoritmy. Je zde dobře patrné, že letoun při průletu bodem WP3 nezvládá zatáčku. To je způsobeno větrem, který v tomto okamžiku působí ve shodném směru s natočením letounu. Naopak na pravém obrázku 7.6b je záznam letu s trajektorií naplánovanou pomocí upravených algoritmů. Ty již s vlivy větru počítají, a proto je zatáčka v bodě WP3 méně ostrá. Naopak v bodech WP1 a WP2 můžeme využít opačného směru větru a naplánovat zde zatáčku s nižším poloměrem zatáčení. 350

350

WP3

300

300

250

250

200

200

y [m]

y [m]

WP3

150

150

wind

wind

100

100

50

50

0

0

WP1 −50 −50

WP2 0

50

100 x [m]

(a) Plánování bez větru

150

200

−50 −50

WP1

WP2 0

50

100

150

x [m]

(b) Plánování s větrem

Obrázek 7.6: Záznam simulovaných letů spolu s naplánovanými trajektoriemi

200

38

KAPITOLA 7. TESTOVÁNÍ ŘEŠENÍ NA LETOUNU

Při vyhodnocování experimentu jsme použili metodiku popsanou ve článku [13]. Budeme zkoumat pravděpodobnost, že se letoun odchýlí od zadané trajektorie o vzdálenost větší než je zadaný limit. Při výpočtu této pravděpodobnosti využijeme naměřených dat. Z letounu dostáváme pravidelně jeho aktuální polohu. Máme tedy k dispozici diskrétní údaje o poloze letounu. Ke statistickému zpracování budeme používat vzorec (7.1). Vyjadřuje pravděpodobnost p(δ), že výchylka letounu  je menší než zvolená maximální odchylka δ. Kde n(>δ) je počet naměřených poloh letounu, pro které platí nerovnost  > δ, a n(celkem) je celkový počet naměřených poloh letounu. p (δ) =

n(ε>δ) n(celkem)

(7.1)

Spočítali jsme pravděpodobnosti odchýlení se od zadané trajektorie pro různé odchylky a výsledky jsme nechali vykreslit do grafu na obrázku 7.7. Je z něj dobře patrné, že pravděpodobnost vychýlení je u nově navržených algoritmů výrazně nižší. Například pro maximální odchylku 20 m je pravděpodobnost překročení stanovené výchylky o 60 % nižší. Pro různé pozice bodů WPn vycházeli obdobné výsledky. 0

10

Pravděpodobnost

bez větru s větrem

−1

10

−2

10

−3

10

0

5

10

15 20 Odchylka [m]

25

30

Obrázek 7.7: Pravděpodobnost vychýlení letounu simulovaného letounu

7.7

Experimenty s reálným letounem

Testování konceptu plánování s větrem bylo prováděno i na reálném letounu Procerus. V době testů měl vítr rychlost okolo 10 m/s. Proto jsme museli zvýšit rychlost letounu na 20 m/s. Poměr rychlosti větru a rychlosti letounu jsme tak snížili na β = 0, 5. To je maximální hodnota, pro kterou byly upravené algoritmy uvažovány. Protože jsme zvýšili rychlost letu na 20 m/s, bylo potřeba zvýšit poloměr zatáčení na 100 m. Obdobně jako při testech na simulovaném letounu byla plánovaná trajektorie mezi body WP1 až WP3. Opět jsme porovnávali odchylky letounu od naplánované trajektorie pomocí původních i upravených algoritmů. Naměřené výsledky jsou na obrázku 7.9.

7.7. EXPERIMENTY S REÁLNÝM LETOUNEM

39

Testovací scénář • rychlost letounu 20 m/s; • rychlost větru okolo 10 m/s; • poloměr zatáčení 100 m; • souřadnice 1. bodu WP1 [0 m, 0 m]; • souřadnice 2. bodu WP2 [-100 m, -300 m]; • souřadnice 3. bodu WP3 [-150 m, 0 m].

13

13

12.5

12.5

12

12 rychlost větru [m/s]

rychlost větru [m/s]

Rychlost a směr větru byly v průběhu experimentu měřeny pomocí autopilota, který porovnával vzdušnou rychlost měřenou pomocí pitotovy trubice1 a absolutní rychlost měřenou pomocí palubního přijímače GPS. Naměřené údaje byly zaznamenávány do souboru. Průběh rychlosti větru v čase je znázorněn na obrázku 7.8a. Průměrná rychlost větru byla v době testu 10,6 m/s. Na druhém obrázku 7.8b můžeme vidět naměřenou rychlost větru společně s jeho směrem. Úhel větru je měřen od jihu, tedy záporné osy y a jeho orientace pro západ je kladná. Průměrný vektor větru v době testu měl hodnotu [-10,3 m/s, 2,4 m/s].

11.5 11 10.5

11.5 11 10.5

10

10

9.5

9.5

9

0

2

4 6 čas [min]

8

(a) Rychlost větru v průběhu času

10

9 90

95

100 105 směr větru [stupně]

110

(b) Rychlost a směr větru

Obrázek 7.8: Záznam větru během experimentu Na levém obrázku 7.9a je zobrazena trajektorie naplánovaná pomocí původních algoritmů bez uvažování vlivu větru. Je dobře patrné, že v zatáčce bodem WP2, letoun nezvládá zatáčet a vylétává z naplánované trajektorie. Jsou zde zachycené pouze necelé dva průlety, protože se nám nepodařilo naměřit delší spojitý úsek letu. To je zapříčiněné způsobem navigace autopilota popsané v sekci 7.2. Za provedené považoval autopilot body ve vzdálenosti 70 m. Pokud se tedy letoun odchýlil od trajektorie o více než 70 m, letoun přeletěl aktuální bod trajektorie a začal se na něj vracet. 1

pitotova trubice - měří rychlost proudění vzduchu pomocí měření náporového tlaku vzduchu v ústí trubice

40

KAPITOLA 7. TESTOVÁNÍ ŘEŠENÍ NA LETOUNU

Na pravém obrázku 7.9b vidíme záznam letu po trajektorii naplánované pomocí upravených algoritmů pro prostředí s konstantním větrem. Je zřejmé, že zatáčka kolem bodu WP2 má výrazně větší poloměr. Díky tomu letoun zvládá kopírovat tuto trajektorii. Naopak zatáčky u bodů WP3 a WP1 jsou mnohem ostřejší, ale protože se zde letoun pohybuje proti směru větru, nečiní to výraznější problém. Můžeme také vidět velké oscilace letounu mezi body WP3 a WP1. Tento fenomén se začíná vyskytovat, pokud se letoun pohybuje proti silnějšímu větru. Je to způsobené pravděpodobně špatným naladěním regulačních mechanizmů v autopilotu. Může to také souviset se způsobem zadávání trajektorie pouze pomocí seznamu bodů. Bylo by proto vhodné do budoucna upravit nastavení autopilota. Z oscilací letounu mezi body WP2 a WP3 můžeme také vysledovat dobré manévrovací schopnosti letounu při letu proti větru. 150

150

100

100

50

50

WP3

WP1

0

0

−50

−50

−100

wind

y [m]

y [m]

−100

−150

−150

−200

−200

−250

−250

−300

−300

−400 −350 −300 −250 −200 −150 −100 x [m]

−50

(a) Plánování bez větru

WP1

wind

WP2

WP2

−350

WP3

−350

0

50

100

−400 −350 −300 −250 −200 −150 −100 x [m]

−50

0

50

100

(b) Plánování s větrem

Obrázek 7.9: Záznam reálných letů spolu s naplánovanými trajektoriemi

7.8

Shrnutí výsledků

Navržený koncept plánování trajektorie letu bezpilotního letounu v prostředí s konstantním větrem jsme testovali na simulovaném i reálném letounu. V obou případech jsme naměřili lepší chování letounu při letu po trajektoriích naplánovaných pomocí upravených algoritmů. Proto můžeme považovat navržený koncept plánování s větrem za vhodnější. Při experimentech s reálným letounem se letounu podařilo plnit naplánovanou trajektorii

7.8. SHRNUTÍ VÝSLEDKŮ

41

v prostředí s větrem o rychlosti 10 m/s, což považujme za velmi dobrý výsledek. Při letu proti větru se také vyskytly problémy s navigací letounu. Letoun měl problémy i na rovném úseku, kde začal oscilovat kolem naplánované trajektorie. Tento problém by bylo vhodné lépe prozkoumat a nalézt do budoucna vhodné řešení.

42

KAPITOLA 7. TESTOVÁNÍ ŘEŠENÍ NA LETOUNU

Kapitola 8

Závěr Cílem této práce bylo přizpůsobit plánování trajektorií bezpilotních letounů pro prostředí s konstantním větrem. Původní řešení bylo založeno na hledání Dubinsových křivek, jako nejkratší možné trajektorie pro prostředek s omezeným poloměrem zatáčení. V praxi se však ukázalo, že když letoun zatáčel ve směru větru, byl unášen a nezvládal kopírovat naplánovanou trajektorii. Dříve se tento problém řešil zvyšováním nastaveného minimálního poloměru zatáčení při procesu plánování. Letoun sice dokázal letět po naplánované trajektorii, ale výrazně se snižovaly jeho manévrovací schopnosti. Bylo proto navrženo nové řešení, které upravuje původní Dubinsovy křivky a mění tvar zatáček. Toto řešení bylo zároveň implementováno do původního algoritmu Accelerated A* určeného k hledání trajektorie letounu v prostředí s překážkami. Abychom ověřili správnost konceptu navrženého řešení provedli jsme několik testů. První test zkoumal výpočetní náročnost upravených algoritmů. Zjistili jsme, že upravené algoritmy pro prostředí s konstantním větrem jsou přibližně 5 až 10 krát pomalejší než původní algoritmy. Dále jsme také zjistili, že pokud budeme chtít počítat trajektorii přímo na palubním počítači Gumstix, budou výpočty přibližně 10 až 15 krát pomalejší. Druhým testem bylo ověření správnosti navrženého konceptu na simulovaném a reálném letounu. V obou případech byly naměřené hodnoty dostatečně průkazné na to, abychom mohli prohlásit, že upravené algoritmy jsou vhodnější v prostředí s konstantním větrem než jejich původní implementace. Při experimentech na reálném letounu Procerus jsme také zjistili špatné chování autopilota při silném protivětru. Letoun začal oscilovat kolem naplánované trajektorie a výchylky byly až 100 m. Výsledky této práce byly publikovány v článku [14] v impaktovaném časopisu International Journal of Advanced Robotic Systems.

8.1

Budoucí práce

V budoucí práci by bylo možné zabývat se úpravou navržených algoritmů v trojrozměrném prostoru. Stávající řešení poskytuje pouze suboptimální řešení, protože neumožňuje při zatáčce v horizontální rovině měnit výšku letu. Mohli bychom také testovat použitelnost upravených Dubinsových manévrů pro použití v jiných plánovacích algoritmech, jako

43

44

KAPITOLA 8. ZÁVĚR

například BFS. Upravené Dubinsovy manévry by bylo také pravděpodobně možné použít pro vyhlazování trajektorií, které byly nalezené jinými metodami plánování.

Literatura [1] S. M. LaValle. Planning Algorithms. Cambridge University Press, Cambridge, U.K., 2006. Available at http://planning.cs.uiuc.edu/. [2] L. E. Dubins. On cures of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. pages 79:497–516, 1957. [3] Xuan-Nam Bui, J.-D. Boissonnat, P. Soueres, and J.-P. Laumond. Shortest path synthesis for dubins non-holonomic robot. In Robotics and Automation, 1994. Proceedings., 1994 IEEE International Conference on, pages 2–7 vol.1, may 1994. [4] J. A. Reeds and L. A. Shepp. Optimal paths for a car that goes both forwards and backwards. Pacific Journal of Mathematics, 145(2):367–393, 1990. [5] H. Sussmann and G. Tang. Shortest paths for the Reeds-Shepp car: A worked out example of the use of geometric techniques in nonlinear optimal control. Technical Report SYNCON 91-10, Dept. of Mathematics, Rutgers University, Piscataway, NJ, 1991. [6] Steven M LaValle. Rapidly-exploring random trees a new tool for path planning. 1998. [7] D. Sislak. Autonomous Collision Avoidance in Air-Traffic Domain. PhD thesis, Czech Technical University in Prague, Faculty of Electrical Engineering, Department of Cybernetics, 2010. [8] Robert L Schultz and Yiyuan Zhao. Free-flight concept. In Guidance, Navigation, and Control Conference, 1997. [9] E. Bakolas and P. Tsiotras. Time-optimal synthesis for the zermelo-markov-dubins problem: The constant wind case. In American Control Conference (ACC), 2010, pages 6163–6168, 30 2010-july 2 2010. [10] Timothy G. McGee, Stephen Spry, and J. Karl Hedrick. Optimal path planning in a constant wind with a bounded turning rate. In AIAA Guidance, Navigation, and Control Conference and Exhibit, pages 1–11, 2005. [11] Oficiální stránky Procerus Technologies. http://www.lockheedmartin.com/us/products/procerus.html, stav z 13. 5. 2013. [12] Oficiální stránky společnosti Gumstix. https://www.gumstix.com, stav z 13. 5. 2013.

45

46

LITERATURA

[13] C. Goerzen and M. Whalley. Minimal risk motion planning: a new planner for autonomous uavs in uncertain environment. In AHS International Specialists’ Meeting on Unmmaned Rotorcraft, Tempe, Arizona, 2011. [14] Martin Selecký, Petr Váňa, Milan Rollo, and Tomáš Meiser. Wind corrections in flight path planning. International Journal of Advanced Robotic Systems, 10(248), 2013.

Příloha A

Seznam použitých zkratek ARM architektura procesorů (Advanced RISC Machine) BFS algoritmus prohledávání do šířky (Breadth-first search) EPP extrudovaný polypropylen GPS globální navigační systém (Global Positioning System) Li-Pol Lithium-Polymerové články MAV bezpilotní letecký prostředek s omezenou velikostí (Micro Aerial Vehicle) PC osobní počítač (personal computer) RAM paměť s náhodným přístupem, (random-access memory) RRT algoritmus prohledávání stavového prostoru založený na rychle náhodně rostoucích stromech (Rapidly-exploring random trees) SD typ paměťové karty vyvinutý firmou ARM Limited (Secure Digital) UAV bezpilotní letecký prostředek (Unmanned Aerial Vehicle) Wi-Fi standardy IEEE 802.11 pro bezdrátovou komunikaci (Wireless-Fidelity) 2D dvoudimenzionální 3D trojdimenzionální

47

48

PŘÍLOHA A. SEZNAM POUŽITÝCH ZKRATEK

Příloha B

Obsah přiloženého CD Na přiloženém disku je kopie této práce spolu s jejími zdrojovými kódy psanými v systému LATEX. Dále zde může nalézt zdrojové kódy v jazyce JAVA a záznamy provedených experimentů. Disk je rozdělen do několika složek: Text obsahuje zdrojové kódy práce v LATEXu, použité obrázky a výsledný soubor BP vana.pdf Zdrojové kódy obsahuje zdrojové kódy implementace v jazyce JAVA Experimenty obsahuje naměřená data z experimentů Práce byla implementována v systému AgentFly, který je licencován, a není jej proto možné umístit na přiložený disk. Implementované části kódu nejsou samostatně spustitelné, a tudíž nelze poskytnout ani žádnou spustitelnou distribuci demonstrující funkci navržených algoritmů.

49

50

PŘÍLOHA B. OBSAH PŘILOŽENÉHO CD

Příloha C

Rozdělení Dubinsových křivek

51

52

PŘÍLOHA C. ROZDĚLENÍ DUBINSOVÝCH KŘIVEK

(a) Úhel otočení θ = 0

(b) Úhel otočení θ = π/3

(c) Úhel otočení θ = 2π/3

(d) Úhel otočení θ = π

(e) Úhel otočení θ = 4π/3

(f) Úhel otočení θ = 5π/3

Obrázek C.1: Rozdělení Dubinsových křivek pro různá otočení θ