Diplomov´a pr´ace

(1)

Fakulta elektrotechnick´a

Diplomov´ a pr´ ace

Martin Blaha

Generov´ an´ı hladk´ ych trajektori´ı ve 3D

Katedra kybernetiky

Vedouc´ı diplomov´e pr´ace: RNDr. Miroslav Kulich, Ph.D.

Praha 2014

(2)

Katedra kybernetiky

ZADÁNÍ DIPLOMOVÉ PRÁCE

Student: Bc. Martin B l a h a

Studijní program: Kybernetika a robotika (magisterský) Obor: Robotika

Název tématu: Generování hladkých trajektorií ve 3D

Pokyny pro vypracování:

1. Seznamte se s volně dostupnými knihovnami pro generování Voroného diagramů množiny bodů ve 3D, zejména s knihovnou Voro++ (http://math.lbl.gov/voro++/).

2. Seznamte se s metodami reprezentace hladkých křivek ve 3D.

3. Implementujte (a) Voroného diagramy pro množinu mnohostěnů a (b) operace průniku Voroného buněk s hladkými křivkami (např. B-splines).

4. Navrhněte a implementujte algoritmus pro optimalizaci počáteční trajektorie založený na efektivním výpočtu funkce vzdálenosti od překážek.

5. Funkčnost implementovaných algoritmů ověřte experimenty. Experimentální výsledky popište se zaměřením na rychlost a robustnost algoritmů a kvalitu generovaných výsledků.

Seznam odborné literatury:

[1] M. Kulich: Vyhlazování cesty pro autonomní vozítko. Diplomová práce, MFF UK Praha, 1996.

[2] C. H. Rycroft: Voro++: a three-dimensional Voronoi cell library in C+, http://math.lbl.gov/voro++/doc/voro++_overview.pdf [online]

[3] W.J. Schroeder, K. Martin, W. Lorensen:The Visualization Toolkit: An Object-Oriented Approach to 3D Graphics, Third Edition. Kitware, Inc. 2006.

[4] E.G. Gilbert, D. W. Johnson: Distance functions and their application to robot path planning in the presence of obstacles, IEEE Journal of Robotics and Automation, vol.1, no.1, pp.21-30, Mar 1985.

Vedoucí diplomové práce: RNDr. Miroslav Kulich, Ph.D.

Platnost zadání: do konce letního semestru 2014/2015

L.S.

doc. Dr. Ing. Jan Kybic vedoucí katedry

prof. Ing. Pavel Ripka, CSc.

děkan

(3)

Prohlaˇsuji, ˇze jsem pˇredloˇzenou práci vypracoval samostatnˇe a ˇze jsem uvedl veˇskeré pouˇzité informaˇcn´ı zdroje v souladu s Metodickým pokynem o dodrˇzován´ı etických princip˚u pˇri pˇr´ıpravˇe vysokoˇskolských závˇereˇcných prac´ı.

V Praze dne... ...

(4)

Dˇekuji vedouc´ımu práce RNDr. Miroslavu Kulichovi, Ph.D. za cenné rady, oceˇnuji jeho vstˇr´ıcný a konstruktivn´ı pˇr´ıstup. Dˇekuji své rodinˇe za podporu v pr˚ubˇehu celého studia.

(5)

Tato práce se zabývá optimalizac´ı poˇcáteˇcn´ı trajektorie zaloˇzené na efektivn´ım výpoˇctu funkce vzdálenosti od pˇrekáˇzek pro 3D prostˇred´ı.

Nejprve je pˇredstaven algoritmus pro tvorbu aproximac´ı 3D Voroného diagramu, který je nutný k výpoˇctu funkce vzdálenosti trajektorie od pˇrekáˇzky. Trajektorie je reprezentována B-spline kˇrivkou. Následnˇe je popsán algoritmus výpoˇctu funkce trajektorie od pˇrekáˇzky zaloˇzený na prohledáván´ı do ˇs´ıˇrky. Pˇri optimalizaci trajektorie jsou pouˇzita jako hodnot´ıc´ı kritéria vzdálenost trajektorie od pˇrekáˇzek a délka trajektorie.

Funkˇcnost navrˇzeného algoritmu byla otestována na mapˇe tvoˇrené sedmi kvádry, pro r˚uzné váhy funkce vzdálenosti od pˇrekáˇzek a délky trajektorie. Návrhy na zlepˇsen´ı jsou diskutovány.

Abstract

This thesis deals with the optimization of the initial trajectory. The optimization is based on calculation of the effective function of the distance between obstacles and path in 3D environments. First, an algorithm for the calculation of approximations of 3D Voronoi diagram is described.

The Voronoi diagram is necessary to calculate the distance function between obstacles and a trajectory. The trajectory is represented by a B- spline curve. The algorithm for calculation of the distance function of path based on breadth-first search is described. The criteria of the distance function and the length of the trajectory are used to optimize trajectory.

Functionality of the proposed algorithm was tested on a map. The map consists of seven obstacles and the test was performed for various weights of both criteria. Suggestions for improvement are discussed.

(6)

Obsah

1 Uvod´ 1

2 Popis algoritmu 3

2.1 Mapa prostˇred´ı . . . 3

2.2 Voron´eho diagramy . . . 4

2.2.1 Voron´eho diagram pro svˇet robota . . . 5

2.3 Pl´anov´an´ı cesty . . . 11

2.3.1 Pˇrevod Voron´eho bunˇek pro pl´anovac´ı algoritmus . . . 11

2.4 Popis trajektorie . . . 15

2.4.1 Pˇrevod napl´anovan´e cesty na B-spline kˇrivku . . . 17

2.5 Optimalizace trajektorie . . . 19

2.5.1 Bezpeˇcnostn´ı funkce . . . 19

2.5.2 Funkce χ. . . 20

2.5.3 Funkce τ . . . 20

2.5.4 Funkce vzd´alenosti od vrcholu pˇrek´aˇzky . . . 21

2.5.5 Funkce vzd´alenosti od hrany pˇrek´aˇzky . . . 21

2.5.6 Funkce vzd´alenosti od stˇeny pˇrek´aˇzky . . . 22

2.5.7 Algoritmus v´ypoˇctu bezpeˇcnostn´ı funkce . . . 22

2.5.8 Kombinace d´elky a bezpeˇcnosti . . . 24

2.6 Zhodnocen´ı navrˇzen´eho algoritmu . . . 25

3 Implementace 26 3.1 Knihovna VTK . . . 26

3.2 Knihovna Voro++ . . . 26

3.3 Knihovna OPT++ . . . 27

4 Experimenty 28 4.1 Objet´ı pˇrek´aˇzky . . . 37

(7)

5 Z´avˇer 38

6 Pˇr´ıloha 41

6.1 Obsah pˇriloˇzen´eho CD . . . 41

(8)

Seznam obr´ azk˚ u

1 Voron´eho diagram pro mnoˇzinu bod˚u v rovinˇe, pˇrevzato z [4] . . . 4

2 Výsledná aproximace pˇrekáˇzek a hrany body . . . 6

3 Z´avislosti hrany e ve struktuˇre DCEL, pˇrevzato z [4] . . . 7

4 Vypoˇctené Voroného buˇnky pro jednotlivé body z´ıskané z pˇrekáˇzek . . . . 8

5 Voron´eho buˇnka pˇred a po redukci stˇen . . . 10

6 Voron´eho buˇnky po redukci stˇen . . . 11

7 Nezpracovan´e stˇeny Voron´eho buˇnky . . . 12

8 Nejkratˇs´ı cesta vygenerovan´a pl´anovaˇcem . . . 16

9 B-spline kˇrivka sloˇzen´a ze tˇr´ı Coonsov´ych kubik . . . 17

10 B-spline kˇrivka z napl´anovan´e cesty . . . 18

11 Pr˚ubˇeh funkce χ . . . 20

12 Pr˚useˇc´ıky Voron´eho buˇnky a B-spline kˇrivky . . . 21

13 Mapa pˇrek´aˇzek s napl´anovanou cestou . . . 28

14 Poˇc´ateˇcn´ı trajektorie v zadan´e mapˇe . . . 29

15 V´ysledn´e trajektorie po optimalizaci . . . 31

16 Pr˚ubˇeh bezpeˇcnostn´ı funkce, d´elky trajektorie a hodnot´ıc´ı funkce 1 . . . . 32

19 Pr˚ubˇeh experimentu ˇc´ıslo 2 . . . 35

20 Pr˚ubˇeh experimentu ˇc´ıslo 5 . . . 36

21 Objet´ı pˇrek´aˇzky . . . 37

(9)

Seznam tabulek

1 Tabulka v´ysledk˚u proveden´ych experiment˚u . . . 30 2 Obsah CD . . . 41

(10)

Seznam algoritm˚ u

1 Algoritmus aproximace hrany pˇrek´aˇzky, pˇrevzato a upraveno z [6] . . . 5

2 Algoritmus hled´an´ı prvn´ıho vrcholu nov´e stˇeny . . . 9

3 Algoritmus hled´an´ı dalˇs´ıch krajn´ıch vrchol˚u nov´e stˇeny . . . 9

4 Algoritmus redukce vrchol˚u nov´e stˇeny . . . 10

5 Algoritmus vytvoˇren´ı seznamu vrchol˚u pro pl´anovaˇc . . . 13

6 Dijkstr˚uv algoritmus . . . 14

7 Expanze vrcholu pro Dijkstr˚uv algoritmus . . . 15

8 Pˇrevod napl´anovan´e cesty na B-spline kˇrivku . . . 18

9 Algoritmus v´ypoˇctu bezpeˇcnostn´ı funkce . . . 23

10 Algoritmus v´ypoˇctu pr˚useˇc´ık˚u Voron´eho bunˇek a B-spline kˇrivky . . . 24

(11)

1 Uvod ´

Mezi základn´ı funkcionality mobiln´ıho robota patˇr´ı plánován´ı, které pˇrevád´ı poˇzadavky nadˇr´ızených specifikac´ı do popisu, kudy se má robot pohybovat. Výstupem plánovac´ıho algoritmu m˚uˇze být posloupnost akc´ı, které mus´ı robot vykonat, aby bylo dosaˇzeno zadaného c´ıle, nebo jenom posloupnost bod˚u, které mus´ı robot projet. Mezi poˇzadavky patˇr´ı pˇr´ı- pustnost plánu, kdy se nebere ohled na jeho efektivitu, ale na proveditelnost, která muˇze být podm´ınˇena napˇr´ıklad rozmˇery robota. Dalˇs´ım poˇzadavkem je kvalita plánu, tj. je sna- hou nalézt proveditelný plán, který je z nˇejakého hlediska optimáln´ı. Na optimáln´ı trajektorii jsou kladeny pˇredevˇs´ım poˇzadavky na energetickou nároˇcnost, délku trajektorie, kterou mus´ı robot urazit k c´ıli a maximáln´ı vzdálenost trajektorie od okoln´ıch pˇrekáˇzek.

Plánovac´ı algoritmy lze rozdˇelit podle stavového prostoru na diskrétn´ı a spojité. Toto rozdˇelen´ı se odráˇz´ı i v pouˇzité reprezentaci prostˇred´ı, ve kterém se robot pohybuje. Diskrétn´ı plánovac´ı algoritmy, napˇr´ıklad Dijkstr˚uv algoritmus, prohledávaj´ı koneˇcný prostor stav˚u, který m˚uˇze být napˇr´ıklad zadán tzv. rastrovou mapou, ve které jednotlivé buˇnky obsahuj´ı informaci o pravdˇepodobnosti výskytu pˇrekáˇzky.

Pˇri plánován´ı trajektorie robota v prostˇred´ı reprezentovaném geometrickou mapou m˚uˇze být stavový prostor spojitý a nespoˇcitatelný. Sloˇzitost geometrického modelu záleˇz´ı na ˇreˇseném problému, ve 2D vˇetˇsinou postaˇc´ı reprezentace pˇrekáˇzek a robota pomoc´ı polygon˚u. Nespoˇcitatelný stavový prostor lze také popsat tzv. konfiguraˇcn´ım prostorem. Tento popis je vhodný pro plánován´ı, pokud je transformace ze spojitého do diskrétn´ıho popisu a dalˇs´ı pouˇzit´ı diskrétn´ıho plánovaˇce pˇr´ıliˇs sloˇzité. Dimenze konfiguraˇcn´ıho prostoru je závislá na ˇreˇseném problému, napˇr´ıklad pro pohyb ve 3D prostˇred´ı kvadrokoptéry je dimenze 6.

K dosaˇzen´ı proveditelného plánu je z konfiguraˇcn´ıho prostoru odebrán prostor pˇrekáˇzek a prostor konfigurac´ı, které nesplˇnuj´ı rozmˇerové a fyzikáln´ı omezen´ı. Výsledný plán je pak z´ıskán napˇr´ıklad algoritmem

”Rapidly-exploring Random Tree“ [1]. Mnoho plánovac´ıch algoritm˚u je pouˇzitelných i v jiných vˇedn´ıch oborech.

Problematikou optimalizace trajektorie zaloˇzené na výpoˇctu funkce vzdálenosti od pˇre- káˇzek se zabývá ˇclánek [2]. V tomto ˇclánku je uveden postup, který popisuje plánován´ı trajektorie v závislosti na pˇrekáˇzkách omezen´ım stavového prostoru. Omezen´ı daná pˇrekáˇzkami jsou zohlednˇena vzdálenost´ı mezi trajektorii a pˇrekáˇzkou, s kterou hroz´ı sráˇzka. V tomto ˇ

clánku jsou uvedeny vlastnosti funkce vzdálenosti, pˇredevˇs´ım jej´ı diferencovatelnost. Tato funkce obecnˇe nemus´ı m´ıt derivaci, ale pro striktnˇe konvexn´ı pˇrekáˇzky ji má, coˇz umoˇzˇnuje k optimalizaci pouˇz´ıt metody, které rychle konverguj´ı. Autoˇri ˇclánku zmiˇnuj´ı jako nevýhodu výpoˇcetn´ı nároˇcnost.

(12)

Tato nevýhoda je odstranˇena v [3]. Nam´ısto poˇc´ıtán´ı funkce vzdálenosti od vˇsech pˇre- káˇzek je poˇc´ıtána funkce vzdálenosti jen od nejbliˇzˇs´ı pˇrekáˇzky. K urˇcen´ı oblast´ı, které jsou nejbl´ıˇze, je pouˇzito Voroného diagramu. Algoritmus popsaný v [3] je urˇcen pro 2D prostˇred´ı pˇrekáˇzek reprezentované seznamem vrchol˚u a hran polygon˚u. Plánovac´ı algoritmus dokáˇze pracovat s jednobodovým i polygonáln´ım robotem.

C´ılem této práce je navrhnout na základˇe [3] algoritmus pro optimalizaci trajektorie ve 3D prostˇred´ı. Hlavn´ı tˇeˇziˇstˇe práce je vˇenováno zmˇenám výpoˇctu funkce vzdálenosti pro pˇrekáˇzky, dále návrhu nových algoritm˚u pro efektivn´ı výpoˇcet hodnot´ıc´ı funkce.

Popis navrˇzeného algoritmu je v kapitole 2. Informace o pouˇzitých knihovnách a implementace je uvedena v kapitole 3. V kapitole 4 jsou zjiˇstˇeny vlastnosti algoritmu proveden´ım nˇekolika experiment˚u. V posledn´ı kapitole 5 je uvedeno zhodnocen´ı celé práce.

(13)

2 Popis algoritmu

V této kapitole je popsán navrˇzený algoritmus generován´ı hladké trajektorie. Algoritmus lze rozdˇelit na tˇri hlavn´ı ˇcásti:

• výpoˇcet Voroného bunˇek pˇrekáˇzek mapy

• v´ypoˇcet poˇc´ateˇcn´ı trajektorie

• optimalizace poˇc´ateˇcn´ı trajektorie.

Vstupn´ı promˇenné jsou mapa pˇrekáˇzek, startovn´ı a c´ılová pozice, výstup algoritmu je optimalizovaná trajektorie popsaná B-spline kˇrivkou.

Pˇri optimalizaci trajektorie je vyuˇzito výpoˇctu funkce vzdálenosti od pˇrekáˇzek na základˇe vzdálenosti ˇcásti trajektorie k nejbliˇzˇs´ı pˇrekáˇzce. Tato funkce je také oznaˇcovaná jako funkce bezpeˇcnosti [3]. Prostor kolem pˇrekáˇzek je rozdˇelen pomoc´ı Voroného diagramu, který kaˇzdé pˇrekáˇzce pˇriˇrad´ı ˇcást prostoru. Dle naˇsich znalost´ı neexistuje pouˇzitelná knihovna pro výpoˇcet Voroného diagramu pro 3D tˇelesa, proto byl navrˇzen postup výpoˇctu, který je popsaný v kapitole 2.2.1.

Optimalizace poˇzaduje na zaˇcátku pˇr´ıpustnou trajektorii od startovn´ıho do c´ılového bodu. Nejdˇr´ıve se vypoˇc´ıtá pˇr´ıpustná cesta pomoc´ı jiˇz spoˇc´ıtaných Voroného bunˇek a Di- jsktrova algoritmu. Následnˇe se cesta pˇrevede na B-spline kˇrivku.

V posledn´ı ˇcásti se B-spline kˇrivka optimalizuje na základˇe poˇzadavk˚u na bezpeˇcnost a délku trajektorie, viz kapitola 2.5. V následuj´ıc´ıch kapitolách jsou jednotlivé kroky po- drobnˇe vysvˇetleny.

2.1 Mapa prostˇ red´ı

Pro vlastn´ı algoritmus plánován´ı cesty je nutné vhodnˇe reprezentovat pˇrekáˇzky v 3D prostˇred´ı robota. Pˇri vytváˇren´ı mapy jsou uvaˇzovány pˇrekáˇzky ve tvaru kvádru nebo krychle. Zadaná mapa pˇrekáˇzek mus´ı splˇnovat pˇredpoklad, ˇze se pˇrekáˇzky vzájemnˇe ne- dotýkaj´ı ani do sebe nezasahuj´ı. Tento pˇredpoklad je d˚uleˇzitý pro vytváˇren´ı Voroného diagramu, který je popsán v kapitole 2.2. Jednotlivé pˇrekáˇzky jsou popsány seznamy vrchol˚u, hran a stˇen, které je tvoˇr´ı. Pˇrekáˇzky se do mapy nemus´ı zadávat danými seznamy, ale staˇc´ı zadat tˇeˇziˇstˇe pˇrekáˇzky a jednotlivé délky v osách x, y, z. Takto vytvoˇrenou pˇrekáˇzku lze posunout, pˇr´ıpadnˇe otoˇcit o zadaný úhel kolem definované osy.

(14)

2.2 Voron´ eho diagramy

Co jsou Voroného diagramy lze ukázat na následuj´ıc´ım pˇr´ıkladu. Pˇredpokládejme mno- ˇ

zinu obchodn´ıch dom˚u, tzv. m´ıst, které poskytuj´ı urˇcité sluˇzby. Naˇs´ım úkolem je pro vˇsechna m´ısta zjistit, kde ˇzij´ı zákazn´ıci, kteˇr´ı poˇzaduj´ı sluˇzby. Dále pˇredpokládejme, ˇze cena sluˇzby je ve vˇsech obchodn´ıch domech stejná, cena z´ıskané sluˇzby je rovná jej´ı cenˇe a cenˇe pˇrepravy k obchodn´ımu domu, cena transportu je úmˇerná Eukleidovské vzdálenosti a zákazn´ıci se snaˇz´ı minimalizovat cenu z´ıskané sluˇzby. Z tˇechto pˇredpoklad˚u vyplývá, ˇze celá plocha je rozdˇelena na regiony jednotlivých obchodn´ıch dom˚u a to tak, ˇze lidé ze stejného regionu jezd´ı do stejného m´ısta. Region je sloˇzen ze vˇsech bod˚u (obydl´ı zákazn´ık˚u), které jsou bl´ıˇze danému m´ıstu neˇz k ostatn´ım m´ıst˚um [4].

Formálnˇe je Voroného diagram definovaný následuj´ıc´ım zp˚usobem. Mˇejme mnoˇzinu n bod˚u S ={p1, , ..., pn} v rovinˇe, pro kaˇzdý prvekp z S je otevˇrený Voroného region dán

D(p, S) = \

q∈S,q6=p

D(p, q), (1)

kde p, q ∈S, p6=q a D(p, q) je polorovina, kter´a patˇr´ı bodu pvypoˇcten´a podle D(p, q) =

z ∈R²;|p−z|<|q−z| . (2) Hranice vˇsech region˚u, která se nazývá Voroného diagram mnoˇziny S, je definována jako

V (S) = [

q∈S

∂D(p, S). (3)

Na obrázku 1 je ukázka Voroného diagramu pro mnoˇzinu bod˚u v rovinˇe.

Obr´azek 1: Voron´eho diagram pro mnoˇzinu bod˚u v rovinˇe, pˇrevzato z [4]

Algoritmus výpoˇctu je popsaný napˇr´ıklad v [4]. Pro mnoˇzinu, které nen´ı zadána jenom body, je nutné pouˇz´ıt algoritmy, které dˇel´ıc´ı úseˇcky nahrazuj´ı obecnou dˇel´ıc´ı kˇrivkou, pˇresný

(15)

popis je v [5]. Dle naˇsich znalost´ı neexistuje pouˇzitelná knihovna, která dokáˇze vypoˇc´ıtat Voroného buˇnky pro 3D tˇelesa. V d˚usledku toho byl navrˇzen algoritmus, který je popsán v následuj´ıc´ı ˇcásti.

2.2.1 Voron´eho diagram pro svˇet robota

Pˇri výpoˇctu bezpeˇcnostn´ı funkce je tˇreba poˇc´ıtat vzdálenost trajektorie od pˇrekáˇzky. Je vhodné pˇrekáˇzky rozdˇelit to tˇrech kategori´ı, protoˇze se vzdálenost trajektorie od vrcholu, hrany a stˇeny pˇrekáˇzky poˇc´ıtá odliˇsným zp˚usobem. Pˇrekáˇzky v mapˇe se nejprve aproximuj´ı mnoˇzinou bod˚u, pro které se vypoˇc´ıtaj´ı Voroného buˇnky. Poˇcet Voroného bunˇek je v této fázi vˇetˇs´ı, neˇz je poˇcet ˇcást´ı pˇrekáˇzek, proto jsou jednotlivé Voroného buˇnky slouˇceny. Slouˇceny jsou i rozdˇelené stˇeny, které vznikly v d˚usledku aproximace pˇrekáˇzek body. V následuj´ıc´ı sekci jsou popsány jednotlivé krovky vytváˇren´ı aproximace Voroného bunˇek pro pˇrekáˇzky.

Pˇrevod mapy Vrcholy, hrany a stˇeny pˇrek´aˇzek ze zadan´e mapy se aproximuj´ı body.

Pˇresnˇejˇs´ı aproximaci pˇrekáˇzek lze dosáhnout zvˇetˇsen´ım poˇctu aproximuj´ıc´ıch bod˚u. Pˇr´ıliˇs velký poˇcet aproximuj´ıc´ıch bod˚u má ale za následek výrazné zpomalen´ı bˇehu algoritmu.

Dalˇs´ım d˚uleˇzitým aspektem je vzdálenost mezi jednotlivými body. Tato vzdálenost je urˇcena pomoc´ı délky nejkratˇs´ı hrany z celé zadané mapy a poˇctu bod˚u, které ji maj´ı aproximovat. Experimentálnˇe bylo zjiˇstˇeno, ˇze dostateˇcný poˇcet bod˚u na nejkratˇs´ı hranˇe pˇrekáˇzek jsou dva. Na obrázku 2a je ukázka aproximace dvou pˇrekáˇzek body. Barevné rozdˇelen´ı oznaˇcuje pˇr´ısluˇsnost bod˚u Voroného bunˇek k jednotlivým ˇcástem pˇrekáˇzek. Postup vzorkován´ı popisuje algoritmus 1.

Vstup: vrcholy hranyx0 a x1, minimáln´ı vzdálenost minL, minimáln´ı poˇcet vzork˚u minS

V´ystup: seznam bod˚u aproximuj´ıc´ıch hranu pˇrek´aˇzky sb

1: Vypoˇcti d´elku kroku d=^minL_minS. Vypoˇcti d´elku hrany l =|x₀ x₁|.

2: Poˇcet aproximuj´ıc´ıch bod˚unp=round(_d^l + 2).

3: for all id∈ h1, np−1) do

4: Pˇridáván´ı bod˚usb=sb∪ {x₀+t(x₁−x₀)}, kdet = _np−1îd .

5: end for

Algoritmus 1: Algoritmus aproximace hrany pˇrek´aˇzky, pˇrevzato a upraveno z [6]

(16)

Poˇcet aproximuj´ıc´ıch bod˚u se urˇc´ı na ˇrádku 2 a následnˇe se vypoˇc´ıtaj´ı jednotlivé aproximuj´ıc´ı body hrany. Na obrázku 2b je zobrazena ukázka vzorkován´ı hrany. Skuteˇcná vzdálenost mezi body aproximuj´ıc´ımi ˇcást pˇrekáˇzky je obecnˇe menˇs´ı neˇz zadaná, coˇz zp˚usobuje zaokrouhlován´ı poˇctu aproximuj´ıc´ıch bod˚u na celé ˇc´ıslo, viz ˇrádek 2.

Modifikac´ı algoritmu 1 lze vzorkovat stˇeny pˇrekáˇzek. Stˇena je zadána ˇctyˇrmi body x₀ aˇz x₃, které jsou vyuˇzity pro urˇcen´ı délky hranl₁ =|x₀x₁|, l₂ =|x₀x₃| a urˇcen´ı poˇctu aproximuj´ıc´ıch bod˚unp₁ anp₂. Jednotlivé body jsou vypoˇcteny podle

x=x0+s(x1 −x0) +t(x3−x0) t∈ h1;np1−1)s∈ h1;np2−1). (4)

(a) Výsledná aproximace pˇrekáˇzek body (b) Vzorkovan´ı hrany

Obrázek 2: Výsledná aproximace pˇrekáˇzek a hrany body

Pro body v seznamu sb se vypoˇc´ıtaj´ı 3D Voroného buˇnky. Pˇresné urˇcen´ı pˇr´ısluˇsnosti Voroného buˇnky k bodu, respektive k ˇcásti pˇrekáˇzky, je zajiˇstˇeno t´ım, ˇze aproximuj´ıc´ı body se vyskytuj´ı v seznamu pouze jednou. Jedineˇcnost poloˇzek seznamu bod˚u je splnˇena splnˇen´ım podm´ınky na mapu pˇrekáˇzek. Pˇri výpoˇctu Voroného bunˇek je nutné definovat ˇ

cást prostoru, ve kterém se výpoˇcet provede. Definovaný prostor výpoˇctu je volen podle pˇrekáˇzek mapy. Má tvar kvádru, který obklopuje vˇsechny pˇrekáˇzky. Pˇri výpoˇctu Voroného bunˇek jsou stˇeny, které soused´ı s definovaným ohraniˇcen´ım indexovány zápornˇe.

Pˇredpokládaný poˇcet Voroného bunˇek pro jednu pˇrekáˇzku ve tvaru kvádru je osm pro vrcholy, dvanáct pro hrany a ˇsest bunˇek pro stˇeny pˇrekáˇzky. Poˇcet Voroného bunˇek je v této fázi daleko vˇetˇs´ı neˇz oˇcekávaný. Následuj´ıc´ı postup je pˇredevˇs´ım zamˇeˇren na redukci

(17)

poˇctu Voroného bunˇek jejich sluˇcován´ım. Na obrázku 4 jsou zobrazeny Voroného buˇnky jednotlivých ˇcást´ı pˇrekáˇzek pˇred dalˇs´ım zpracován´ım.

Sluˇcován´ı Voroného bunˇek Sn´ıˇzen´ı poˇctu Voroného bunˇek lze dosáhnou spojován´ım sousedn´ıch bunˇek do jedné, respektive vyˇrazen´ım sousedn´ıch stˇen z obou bunˇek. Spojovat lze pouze Voroného buˇnky, které soused´ı s bodem ze stejné pˇrekáˇzky a ze stejných ˇcást´ı hrany nebo stˇeny pˇrekáˇzky. Aproximaci vrcholu pˇredstavuje bod samotný, proto Voroného buˇnky vrchol˚u nen´ı nutné sluˇcovat.

V dalˇs´ı f´azi jsou vˇsechny buˇnky pˇrevedeny do struktury podobn´e DCEL(

”Doubly Con- nected Edge List“), která se skládá ze tˇr´ı seznam˚u: vrchol˚u, hran a stˇen. Pˇrevod do upravené struktury DCEL umoˇzn´ı efektivnˇe procházet stˇeny po hranách. Struktura pro jednotlivé vrcholy obsahuje souˇradnice jeho vrcholu, dále je rozˇs´ıˇrená o id vrcholu a seznam vˇsech hran buˇnky, které ve vrcholu zaˇc´ınaj´ı. Tato úprava umoˇzˇnuje pˇrecházen´ı z polygonu (stˇeny) do pˇrilehlého polygonu pˇres vrchol dané stˇeny.

Struktura pro stˇenu obsahuje ukazatel na prvn´ı hranu, která stˇenu ohraniˇcuje, id stˇeny, id sousedn´ı buˇnky, která pˇres danou stˇenu soused´ı a typ sousedn´ı buˇnky, tj. ke které ˇcást´ı pˇrekáˇzky patˇr´ı.

Struktura pro hranu obsahuje

”twin edge“, to je dvojice sousedn´ı hrany e

”half edge“.

Na obrázku 3 jsou graficky znázornˇené závislosti

”half edge“.

”Half edge“ má ukazatel na vrchol, kde má poˇcátek, dále ukazatel na dalˇs´ı, pˇredchoz´ı a protilehlou hranu a ukazatel na stˇenu, která je vlevo ve smˇeru hrany. Dále obsahuje id hrany a logický pˇr´ıznak pro urˇcen´ı, jestli hrana m´ıˇr´ı do pˇrekáˇzky v mapˇe. [4]

e

Prev(e) Next(e)

IncidentFace(e)

Origin(e) Twin(e)

Obr´azek 3: Z´avislosti hrany e ve struktuˇre DCEL, pˇrevzato z [4]

(18)

Na obrázku 4 je vidˇet, ˇze vzorkován´ı zp˚usobilo rozdˇelen´ı stˇen Voroného buˇnky i tam, kde má být stˇena souvislá. V následuj´ıc´ı ˇcásti je ˇreˇseno spojován´ı rozdˇelených stˇen.

Obrázek 4: Vypoˇctené Voroného buˇnky pro jednotlivé body z´ıskané z pˇrekáˇzek

Redukce nadbyteˇcných stˇen Redukce nadbyteˇcných stˇen spoˇc´ıvá v nahrazen´ı nˇekolika sousedn´ıch stˇen jednou novou stˇenou. Nejdˇr´ıve se urˇc´ı stˇeny, které je moˇzné nahrazovat.

Mezi nadbyteˇcn´e stˇeny se povaˇzuj´ı:

• Stˇeny, které tvoˇr´ı stˇenu se stejným okrajem, jsou z´ıskány ze stejné pˇrekáˇzky a soused´ı spolu alespoˇn jednou hranou.

• Stˇeny, které jsou mezi Voroného buˇnkami vytvoˇrenými z jiné ˇcásti stejné pˇrekáˇzky, napˇr´ıklad stˇena mezi buˇnkou hrany a pˇrilehlou buˇnkou stˇeny.

• Stˇeny mezi dvˇema hranami z jiné pˇrekáˇzky, které leˇz´ı v rovinˇe.

• Stˇeny mezi dvˇema Voroného buˇnkami, které vznikly ze stˇeny pˇrekáˇzky mezi dvˇema r˚uznými pˇrekáˇzkami.

Algoritmus hledán´ı okraje nové stˇeny Pˇri hledán´ı okraje nové stˇeny se na zaˇcátku urˇc´ı jeden vrchol leˇz´ıc´ı ohraniˇcen´ı nové stˇeny. Postup hledán´ı poˇcáteˇcn´ıho vrcholu nové stˇeny popisuje algoritmus 2. Cyklus na ˇrádku 1 vytváˇr´ı seznam vˇsech vrchol˚u, které definuj´ı stˇenu, která vytváˇr´ı hranici mezi dvˇema buˇnkami nebo buˇnkou a okrajem. Poˇcáteˇcn´ı vrchol nové stˇeny mus´ı být na jej´ım okraji. Poˇcáteˇcn´ı vrchol je vrchol, který je nejdále od prvn´ıho

(19)

vrcholu v seznamu vrchol˚u.

Vstup: Voron´eho buˇnka c, sousedn´ı buˇnky okraj V´ystup: vrchol v

1: for all stˇenu f buˇnky cdo

2: if stˇena f soused´ı s buˇnkouokraj then

3: Pˇridej vˇsechny vrcholy stˇeny f do pomocn´eho seznamu vrcholu psv.

4: end if

5: end for

6: Odstraˇn duplicity ze seznamu psv.

7: for all prvek v₁ seznamu psv\ {psv[1]} do

8: d=|v₁ psv[1]|

9: if d > maxd then

10: maxd=d av =v₁

11: end if

12: end for

Algoritmus 2: Algoritmus hled´an´ı prvn´ıho vrcholu nov´e stˇeny

V dalˇs´ım kroku se vytvoˇr´ı seznam vˇsech bod˚u, které leˇz´ı na okraji nové stˇeny. Opako- ván´ım algoritmu 3 pokaˇzdé pro naposledy nalezený vrchol v, dokud dalˇs´ı nalezený vrchol nen´ı prvn´ım vrcholem nové stˇeny, se z´ıská seznam vrchol˚u, které tvoˇr´ı okraj nové stˇeny.

Na ˇr´adku 2 algoritmu 3 se testuje, jestli hrana e ze seznamu hran vrcholu v₀ je hraniˇcn´ı.

Nalezený seznam vrchol˚u nové stˇeny vˇetˇsinou obsahuje hodnˇe vrchol˚u, které leˇz´ı na pˇr´ımce.

Tyto vrcholy lze odstranit, ale pouze pokud bude odstranˇen vrchol i v pˇrilehlé stˇenˇe, jak ukazuje obrázek 5. Jinak by ve struktuˇre pro Voroného buˇnky nesouhlasil poˇcet

”half edge“

a poˇcet

”twin edge“. Vrcholy nové stˇeny, která nelze odstranit, jsou pˇridány do seznamu neodstranitelných vrchol˚u.

Vstup: Voroného buˇnka c, sousedn´ı buˇnky okraj a poˇcáteˇcn´ı vrchol v₀ Výstup: vrchol v

1: for all Hrany e vrcholu v₀ do

2: if e.face.neighbor == okraj AND e.twin.face.neighbor !=okraj then

3: v = e.twin.vertex;

4: end if

5: end for

Algoritmus 3: Algoritmus hled´an´ı dalˇs´ıch krajn´ıch vrchol˚u nov´e stˇeny

(20)

Algoritmus 4 popisuje postup pˇri sn´ıˇzen´ı poˇctu vrchol˚u tvoˇr´ıc´ıch novou stˇenu, který je zaloˇzen na výpoˇctu jednotlivých smˇernic vrchol˚u jdouc´ıch za sebou. Pokud je testovaný vrchol vrcholem, který nem˚uˇze být odstranˇen, je pˇridán do filtrovaného seznamu hned.

V opaˇcném pˇr´ıpadˇe se vypoˇcte smˇernice hrany pˇred a za testovaným vrcholem. Testovaný vrchol je pˇridán, pokud je vzdálenost mezi souˇradnicemi smˇernic vˇetˇs´ı neˇz urˇcená mez.

Staré stˇeny se nahrad´ı novou stˇenou, odstran´ı se jiˇz nepouˇz´ıvané hrany a vrcholy, které leˇz´ı uvnitˇr nové stˇeny.

Obr´azek 5: Voron´eho buˇnka pˇred a po redukci stˇen

Vstup: seznam vrchol˚u v, seznam neodstranitelných vrchol˚unv, práh p Výstup: redukovaný seznam vrchol˚urv

1: Velikost seznamu vs =|v|. Na zaˇc´atek v pˇridej v[vs]. Na konec v pˇridej v[1].

2: for all i∈ h1, vs−1i do

3: if vrchol v[i] je v nv then

4: Pˇridej nakonec rv vrchol v[i]

5: else

6: Vypoˇc´ıtej smˇernici s₁ (v[i−1] a v[i] ).

7: Vypoˇc´ıtej smˇernici s₂ (v[i] av[i+ 1] ).

8: Normov´an´ı smˇernic s₁ a s₂.

9: if |s₁s₂|> p then

10: Pˇridej nakonec rv vrchol v[i]

11: end if

12: end if

13: end for

Algoritmus 4: Algoritmus redukce vrchol˚u nov´e stˇeny

Na obrázku 6 jsou zobrazeny Voroného buˇnky pro redukci poˇctu stˇen. Urˇcitou nevýhodou

(21)

je, ˇze okraje Voroného bunˇek zab´ıhaj´ı do pˇrekáˇzek, coˇz je zp˚usobeno pouˇzitým postupem aproximace pˇrekáˇzek mnoˇzinou bodu.

Obr´azek 6: Voron´eho buˇnky po redukci stˇen

Algoritmus, který je pouˇzit na výpoˇcet pr˚useˇc´ık˚u B-spline kˇrivky a Voroného buˇnky, procház´ı kaˇzdou stˇenu buˇnky, proto je vhodné, aby stˇen buˇnky bylo co nejménˇe. Na obrázku 7 jsou v popˇred´ı vpravo zobrazeny stˇeny mezi Voroného buˇnkami, které vznikly z hrany a stˇeny r˚uzných pˇrekáˇzek. Tato ˇcást buˇnky se skládá z malých a velkých stˇen. Slouˇcen´ı malých stˇen se sousedn´ımi stˇenami s velkým obsahem by vedlo ke sn´ıˇzen´ı poˇctu stˇen buˇnky.

2.3 Pl´ anov´ an´ı cesty

Na seznam struktury Voroného bunˇek lze nahl´ıˇzet jako na orientovaný graf s kladným ohodnocen´ım hran (délka hrany). Pro nalezen´ı nejkratˇs´ı cesty ze startovn´ıho do c´ılového vrcholu byl pouˇzit Dijkstr˚uv algoritmus [7]. Výsledná naplánovaná cesta je pak pˇrevedena na B-spline kˇrivku, která je vyuˇzita jako poˇcáteˇcn´ı trajektorie pro optimalizaci.

2.3.1 Pˇrevod Voron´eho bunˇek pro pl´anovac´ı algoritmus

Vstupem plánovac´ıho algoritmu je seznam vˇsech vrchol˚u, které definuj´ı stˇeny Voroného bunˇek ˇcást´ı pˇrekáˇzek. Prvky seznamu vˇsech vrchol˚u jsou popsány strukturou, která ob-

(22)

Obrázek 7: Nezpracované stˇeny Voroného buˇnky

sahuje id vrcholu v seznamu, souˇradnice vrcholu a seznam výskytu daného vrcholu v jed- notlivých buˇnkách a id tohoto vrcholu v konkrétn´ı Voroného buˇnce. Identifikátor vrcholu v konkrétn´ı buˇnce je kódován pomoc´ı vztahu

id=ic·1000000 +iv, (5)

kde ic je id Voroného buˇnky a iv je id vrcholu v rámci Voroného buˇnky. Vztah (5) klade omezen´ı na poˇcet vrchol˚u v jedné buˇnce, který nem˚uˇze být vetˇs´ı jak jeden milion, protoˇze by pak negeneroval jednoznaˇcné id.

Postup vytváˇren´ı seznamu vrchol˚u pro plánovaˇc popisuje algoritmus 5. Na ˇrádc´ıch 1 aˇz 6 se vytváˇr´ı seznam vˇsech vrchol˚u, které tvoˇr´ı Voroného buˇnky. Na ˇrádc´ıch 7 aˇz 13 se urˇc´ı výskyty jednotlivých vrchol˚u ve vˇsech buˇnkách. Na ˇrádku 16 se vytváˇr´ı poloˇzkan seznamu vˇsech vrchol˚u sv, do které se pˇridaj´ı údaje o souˇradnic´ıch vrcholu a seznam výskyt˚u z k.

Na ˇrádku 18 se ze seznamuspodstan´ı prvky, které jsou v seznamu výskyt˚u prvkun. T´ımto postupem je zajiˇstˇeno, ˇze v seznam sv budou vˇsechny vrcholy pouze jednou.

Naplánovaná cesta nesm´ı procházet pˇrekáˇzkami, k tomuto úˇcelu byly oznaˇceny vˇsechny hrany, které m´ıˇr´ı z pˇrekáˇzky nebo do pˇrekáˇzky. Pˇri hledán´ı tˇechto hran se vyuˇzilo faktu, ˇ

ze tyto hrany zaˇc´ınaj´ı, respektive konˇc´ı, bl´ızko vrchol˚u pˇrekáˇzek. V seznamu vˇsech vrchol˚u se najdou vrcholy, které jsou danému vrcholu pˇrekáˇzky nejbl´ıˇze. Pro tyto vrcholy se urˇc´ı hrany, kterým se nastav´ı logický pˇr´ıznak.

Logický pˇr´ıznak kolize je také nastaven hranám, které leˇz´ı v ohraniˇcen´ı celé mapy pˇrekáˇzek nebo do nˇeho smˇeˇruj´ı. Nejprve se urˇc´ı stˇeny Voroného bunˇek, které vytváˇr´ı hranici s ohraniˇcen´ım. Hranám, které zaˇc´ınaj´ı,respektive konˇc´ı, ve vrcholech tˇechto stˇen se opˇet

(23)

Vstup: seznam Voron´eho bunˇek sc

V´ystup: seznam vˇsech vrchol˚u sv ze seznamu Voron´eho bunˇek sc

1: for all Voron´eho buˇnkub zsc do

2: for all Vrchol v ze seznamu vrchol˚u buˇnky b do

3: Vytvoˇr poloˇzku seznamu sp jednotliv´ych vrchol˚u v.

4: Pˇridej poloˇzku do seznamu sp.

5: end for

6: end for

7: for all Poloˇzku s₁ seznamu sp do

8: for all Poloˇzkus₂ seznamusp do

9: if |s₁s₂|< mez then

10: Pˇridej poloˇzce s₁ do seznamu v´yskytu identifik´ator s₂.

11: end if

12: end for

13: end for

14: while sp nen´ı pr´azdn´y do

15: Vezmi posledn´ı prvek k z sp.

16: Vytvoˇr nov´y prvekn.

17: Do n pˇridej seznam v´yskyt˚u z k.

18: Odstraˇn z sp prvky, ve kter´ych se vyskytujen.

19: Pˇridejn do seznamu sv.

20: end while

Algoritmus 5: Algoritmus vytvoˇren´ı seznamu vrchol˚u pro pl´anovaˇc

nastav´ı pˇr´ıznak kolize. Tato úprava zajist´ı, ˇze naplánovaná cesta nevede po ohraniˇcen´ı celé mapy.

Algoritmus 6 popisuje Dijkstr˚uv algoritmus, kter´y slouˇz´ı k nalezen´ı nejkratˇs´ı cesty.

Na zaˇcátku algoritmu 6 se vytvoˇr´ı prioritn´ı halda, která má na zaˇcátku vrchol s nejmenˇs´ı vzdálenost´ı od startu. Pˇri inicializaci se vˇsem vrchol˚um nastav´ı vzdálenost na nekoneˇcno.

Seznam pˇredku prev slouˇz´ı ke zpˇetnému urˇcen´ı nalezené cesty. Startovn´ı vrchol má pˇred- ka −1, coˇz je vyuˇzito pro ukonˇcen´ı zpˇetného hledán´ı cesty. V kaˇzdém kroku se odebere jeden vrchol v prioritn´ı haldˇe, coˇz zajiˇst’uje koneˇcnost Dijkstrova algoritmu. Na ˇrádku 10 je poˇc´ıtána nová vzdálenost expandovaného vrcholu, která vznikne seˇcten´ım dosavadn´ı vzdálenosti expandovaného vrcholu a vzdálenost´ı mezi expandovaným vrcholem a jeho po- tomkem. Pokud je nová vzdálenost menˇs´ı jak vzdálenost, kterou má potomek, tak se po-

(24)

Vstup: startovn´ı vrchols, c´ılov´y vrcholc, seznam vrchol˚u sv, seznam Voron´eho bunˇek sc

V´ystup: seznam vrchol˚u cestysvc

1: Vytvoˇr prioritn´ı haldu h seznamu sv.

2: Inicializuj prioritn´ı fronty, seznam˚u vzd´alenost´ıdist a pˇredkuprev.

3: dist[start] = 0; h.update(start, 0)

4: repeat

5: vrcholv s nejmenˇs´ı vzd´alenost´ı z h

6: Odstraˇnv z h.

7: if v! =cthen

8: Expanduj vrchol v do seznamu ex.

9: for all Poloˇzku ex₀ seznamu ex do

10: vzd´alenostalt=dist[v] +|v ex₀|

11: if alt < dist[ex₀] then

12: dist[ex₀] =alt

13: prev[ex₀] =v

14: h.update(ex₀, alt)

15: end if

16: end for

17: else

18: Ukonˇci vyhled´av´an´ı.

19: end if

20: until h nen´ı pr´azdn´eor c==v

21: Do seznamu svc pˇridej na zaˇc´atek c´ılov´y vrcholc.

22: u=prev[c]

23: while u! =−1do

24: Pˇrideju na zaˇc´atek seznamusvc

25: u=prev[u]

26: end while

Algoritmus 6: Dijkstr˚uv algoritmus

tomkovi nastav´ı nová vzdálenost, expandovaný vrchol jako rodiˇc a aktualizuje se hodnota vzdálenosti potomka v prioritn´ı haldˇe.

Algoritmus 7 popisuje postup expanze vrcholu, kter´y vrac´ı seznam vrchol˚u potomk˚u.

Seznam Voron´eho bunˇek slouˇz´ı k filtraci pˇrechodu, kter´e by vedly po kolizn´ı hranˇe z ex-

(25)

pandovan´eho vrcholu do vrcholu potomka.

Vstup: expandovaný vrchol ev, seznam vrchol˚u sv, seznam Voroného bunˇeksc Výstup: seznam potomk˚u sp

1: Vezmi seznam v´yskytu vrchol˚usvv vrcholuev z sv.

2: for all poloˇzku ex₀ seznamu svv do

3: Vezmi seznam hran h vrcholuex₀.

4: for all Poloˇzkuh₀ seznamu h do

5: if h₀.twin nesmˇeˇruje do pˇrek´aˇzky then

6: Pˇridejh₀.twin.vrchol do pomocn´eho seznamu psv

7: end if

8: end for

9: end for

10: for all poloˇzku psv₀ seznamu psv do

11: Pˇridej do seznamusp vrchol z sv, kter´y odpov´ıd´a vrcholu psv₀.

12: end for

13: Odstraˇn duplicitn´ı vrcholy v sp

Algoritmus 7: Expanze vrcholu pro Dijkstr˚uv algoritmus

Na obrázku 8 je zobrazen výsledek plánován´ı pro mapu se dvˇema pˇrekáˇzkami na vrcholech Voroného bunˇek. Zelená kuliˇcka oznaˇcuje startovn´ı vrchol a ˇcervená kuliˇcka oznaˇcuje c´ılový vrchol.

V dalˇs´ı fázi se pˇrevede naplánovaná cesta do B-spline kˇrivky, která se následnˇe podle diskutovaných poˇzadavk˚u optimalizuje.

2.4 Popis trajektorie

Výsledná trajektorie je reprezentována pomoc´ı B-spline kˇrivky, jej´ıˇz ˇcásti tvoˇr´ı Coonsovy kubiky. Coonsova kubika je kˇrivka definovaná pˇredpisem

C(t) = C0(t)P0+C1(t)P1+C2(t)P2+C3(t)P3, t∈ h0,1i, (6)

(26)

Obrázek 8: Nejkratˇs´ı cesta vygenerovaná plánovaˇcem

kde P₀, P₁, P₂ a P₃ jsou ˇr´ıd´ıc´ı body a C₀, C₁,C₂ a C₃ jsou Coonsovy polynomy C₀(t) = 1

6(1−t)³ C₁(t) = 1

6(3t³−6t²+ 4) C₂(t) = 1

6(−3t³+ 3t²+ 3t+ 1) (7)

C₃(t) = 1 6t³.

Poloha poˇcáteˇcn´ıho bodu kˇrivky leˇz´ı v antitˇeˇziˇsti trojúheln´ıkuP₀P₁P₂ a poloha koncového bodu v antitˇeˇziˇsti trojúheln´ıku P₁P₂P₃.

Coons˚uv kubický B-spline je zadán ˇr´ıd´ıc´ımi bodyP₀,P₁, ...,P_n, které tvoˇr´ın−2 Coonsových kubik. Pˇr´ıklad B-spline kˇrivky, která je sloˇzená z nˇekolika kubik, je na obrázku 9. D˚uleˇzitou vlastnost´ı B-spline kˇrivky je, ˇze zmˇena souˇradnic jednoho ˇr´ıd´ıc´ıho bodu zmˇen´ı pouze kubiky, kde se ˇr´ıd´ıc´ı bod vyskytuje. Napojen´ı kubik a samotné kubiky maj´ı spojitou derivaci druhého ˇrádu. [8]

(27)

−15

−10

−5 0

5 10

15 20

25 −15

−10

−5 0

5

10 15 0

5 10

P5

P2

y [m]

P1

P₃

P4

x [m]

P0

z [m]

Obrázek 9: B-spline kˇrivka sloˇzená ze tˇr´ı Coonsových kubik

2.4.1 Pˇrevod napl´anovan´e cesty na B-spline kˇrivku

Poˇcet vrchol˚u cesty na obrázku 8 je 27, pokud by se pouˇzily vˇsechny vrcholy cesty, B-spline kˇrivka by zbyteˇcnˇe pˇresnˇe sledovala naplánovanou cestu. Algoritmus 8 popisuje postup pˇrevodu cesty na B-spline kˇrivku. Na ˇrádku 1 se generuj´ı souˇradnice ˇr´ıd´ıc´ıho bodu, který se mus´ı nalézat v bl´ızkém okol´ı daného vrcholu naplánované cesty. Toto okol´ı je definované konstantou. Malé náhodné posunut´ı je d˚uleˇzité pro hledán´ı poˇcáteˇcn´ı Voroného buˇnky v algoritmu pro výpoˇcet bezpeˇcnostn´ı funkce. Dalˇs´ı d˚uvod pro posunut´ı je, ˇze ge- nerovaná cesta by leˇzela v bl´ızkosti rozhran´ı mezi dvˇema Voroného buˇnkami, coˇz by vedlo na k velkému poˇctu pr˚useˇc´ık˚u B-spline kˇrivky s Voroného buˇnkami. Na ˇrádku 2 se pˇridává vygenerovaný ˇr´ıd´ıc´ı bod do seznamu ˇr´ıd´ıc´ıch bod˚u, které definuj´ı B-spline kˇrivku. Startovn´ı a c´ılový ˇr´ıd´ıc´ı bod se pˇridává tˇrikrát z d˚uvod˚u zachován´ı startovn´ı a c´ılové pozice. Sn´ıˇzen´ı poˇctu ˇr´ıd´ıc´ıch bod˚u oproti poˇctu vrchol˚u naplánované cesty se dˇeje tak, ˇze se pˇridává kaˇzdý N-tý náhodnˇe vygenerovaný bod v bl´ızkosti N-tého vrcholu cesty, viz ˇrádek 3 a 4.

Na obrázku 10 je fialovˇe zobrazena B-spline kˇrivka, která je zadána deseti ˇr´ıd´ıc´ımi body.

(28)

Vstup: napl´anovan´a cesta nc

V´ystup: ˇr´ıd´ıc´ı body B-spline kˇrivky bs

1: Vygeneruj n´ahodnˇe bodsb v epsilonov´em okol´ı startovn´ıho vrcholu.

2: Pˇridej do seznamu ˇr´ıd´ıc´ıch bod˚u bstˇrikr´at bodsb.

3: for all i∈ h1; |nc−1|ido

4: if mod (i, N) == 0 then

5: Vygeneruj n´ahodnˇe bod b v epsilonov´em okol´ınc[i] vrcholu.

6: Pˇridej do seznamu ˇr´ıd´ıc´ıch bod˚u bsbod b.

7: end if

8: end for

9: Vygeneruj náhodnˇe bodcb v epsilonovém okol´ı c´ılového vrcholu.

10: Pˇridej do seznamu ˇr´ıd´ıc´ıch bod˚u bstˇrikr´at bodcb.

Algoritmus 8: Pˇrevod napl´anovan´e cesty na B-spline kˇrivku

Obrázek 10: B-spline kˇrivka z naplánované cesty

(29)

2.5 Optimalizace trajektorie

Dalˇs´ım krokem je optimalizace vygenerované poˇcáteˇcn´ı trajektorie. Na výslednou trajektorii jsou poˇzadavky, aby vzdálenost mezi pˇrekáˇzkou a naplánovanou trajektori´ı byla maximáln´ı a energetická nároˇcnost byla minimáln´ı. Proces optimalizace je definován ve formˇe

x∈minRⁿ

f(x), (8)

kde x ∈ Rⁿ a f : Rⁿ → R. f(x) je nelineárn´ı multidimenzionáln´ı hodnot´ıc´ı funkce. Pro hledán´ı minima neomezené multidimenzionáln´ı hodnot´ıc´ı funkce je pouˇzita tzv. metoda pˇr´ımého vyhledáván´ı

”direct search method“ [9]. Metoda pˇr´ımého vyhledáván´ı pouze vyˇza- duje spojitou hodnot´ıc´ı funkci. Metoda nepotˇrebuje derivaci ani odhad derivace hodnot´ıc´ı funkce a snadno se implementuje.

V následuj´ıc´ıch podkapitolách je popsán postup výpoˇctu hodnot´ıc´ı funkce na základˇe rovnic z [3], které jsou rozˇs´ıˇreny pro 3D prostˇred´ı pˇrekáˇzek.

2.5.1 Bezpeˇcnostn´ı funkce

Bezpeˇcnostn´ı funkce zohledˇnuje poˇzadavek na maximáln´ı vzdálenost trajektorie a okol- n´ıch pˇrekáˇzek, viz rovnice (9), pˇrevzato z [3].

F_B =

N

X

i=1

X

b∈Cesta∩Bi

χ(d(b, B_i)) (9)

V rovnici (9) jeN poˇcet Voroného bunˇek,d(b, B_i) je vzdálenost bodu trajektorie od pˇre- káˇzky ve Voroného buˇnce. Funkceχhodnot´ı bl´ızkost trajektorie k pˇrekáˇzce. Funkce, podle které se poˇc´ıtá bezpeˇcnost ˇcásti trajektorie j, která je v buˇnce i, je definována

F_B^ij(P~) =

K

X

k=1

F_B^ijk(P~), (10)

kdeP~ je vektor ˇr´ıd´ıc´ıch bod˚u B-spline kˇrivky, K je poˇcet souvislých ˇcást´ı B-spline kˇrivky, které jsou uvnitˇr Voroného buˇnky a F_Bîjk(P~) je definováno rovnic´ı (11).

F_B^ijk(P~) = Z

Cj

χ(d(C_j(P , t), B~ _i))dt, t∈ hτ₁(P , i, j, k), τ~ ₂(P , i, j, k)i~ (11) V rovnici (11) je τ₁(P , i, j, k)~ k-tý poˇcátek ˇcásti souvislého pr˚uniku j-té ˇcásti cesty si-tou buˇnkou,τ₂(P , i, j, k)~ k-tý konec ˇcásti souvislého pr˚uniku j-té ˇcásti cesty s i-tou buˇnkou a

(30)

C_j(P , t) vyjadˇruje body B-spline kˇrivky v z´~ avislosti na ˇr´ıd´ıc´ıch bodech a promˇenné t. Pro dalˇs´ı výpoˇcty je vhodné rovnici (11) upravit do tvaru

F_B^ijk(P~) =

Z τ2(P ,i,j,k)~

τ1(P ,i,j,k)~

χ(d(C_j(P , t), B~ _i)) r

(∂C_jx

∂t (P , t))~ ²+ (∂C_jy

∂t (P , t))~ ²+ (∂C_jz

∂t (P , t))~ ²dt.

(12)

V následuj´ıc´ıch podkapitolách jsou popsány jednotlivé funkce z rovnice (12).

2.5.2 Funkce χ

Funkce χ má za úkol penalizovat bl´ızkost trajektorie od pˇrekáˇzek, na obrázku 11 je zobrazena tato funkce, která má funkˇcn´ı pˇredpis

χ(v) = 1000e^−20v, (13)

kde v je vzd´alenost.

0 0.5 1 1.5

0 200 400 600 800 1000

v [m]

χ

Obr´azek 11: Pr˚ubˇeh funkceχ

2.5.3 Funkce τ

Krajn´ı body jednotlivých ˇcást´ı funkce τ jsou definovány pr˚useˇc´ıky B-spline kˇrivky se stˇenami Voroného bunˇek. Pr˚useˇc´ıky jsou z´ıskány ˇreˇsen´ım soustavy

C_jx(P , t) =~ X+u_xs+v_xr t, s, r∈ h0,1i (14) C_jy(P , t) =~ Y +u_ys+v_yr (15) C_jz(P , t) =~ Z +u_zs+v_zr, (16)

(31)

kde ~u = (u_x, u_y, u_z) a ~v = (v_x, v_y, v_z) jsou lineárnˇe nezávislé smˇerové vektory stˇeny a (X, Y, Z) je bod stˇeny.

Dalˇs´ımi úpravami z´ıskáme kubickou rovnici, která se ˇreˇs´ı pomoc´ı Cardanových vzorc˚u [10]. Pˇr´ıpustná ˇreˇsen´ı jsou taková, ˇze t ∈ h0,1i a zároveˇn pr˚useˇc´ık mus´ı leˇzet uvnitˇr stˇeny Voroného buˇnky. K urˇcen´ı, zda pr˚useˇc´ık leˇz´ı uvnitˇr stˇeny, je pouˇzita metoda

”Ray casting“, která poˇc´ıtá, kolikrát protne polopˇr´ımka z testovaného bodu hranice polygonu. Pokud je poˇcet pr˚useˇc´ık˚u polopˇr´ımky a hranice polygonu lichý, pr˚useˇc´ık leˇz´ı uvnitˇr.

Na obrázku 12 je ukázka nalezených pr˚useˇc´ık˚u. Z tohoto obrázku je také vidˇet, ˇze v této Voroného buˇnce B-spline kˇrivka procház´ı tˇremi stˇenami a funkceτ má dvˇe souvislé ˇcásti.

t₁

t₂ t₃

t₄

Obr´azek 12: Pr˚useˇc´ıky Voron´eho buˇnky a B-spline kˇrivky

2.5.4 Funkce vzd´alenosti od vrcholu pˇrek´aˇzky

Kdyˇz B-spline kˇrivka procház´ı Voroného buˇnkou, která vznikla z vrcholu pˇrekáˇzky, poˇc´ıtá se vzdálenost dvou bod˚u, pˇriˇcemˇz prvn´ı bod je dán B-spline kˇrivkou a druhý bod je zadán souˇradnicemi vrcholu (X, Y, Z) v pˇrekáˇzce, viz rovnice (17).

d(Cj(P , t), B~ i) = q

(Cjx(P , t) +~ X)²+ (Cjy(P , t)~ −Y)²+ (Cjz(P , t)~ −Z)² (17) 2.5.5 Funkce vzd´alenosti od hrany pˇrek´aˇzky

Pokud B-spline kˇrivka procház´ı Voroného buˇnkou, která vznikla z hrany pˇrekáˇzky, poˇc´ıtá se vzdálenost bodu a pˇr´ımky, která je zadána poˇcáteˇcn´ım bodem A = (A_x, A_y, A_z) a

(32)

smˇernic´ı~u = (u_x, u_y, u_z), pˇriˇcemˇz bod je dán B-spline kˇrivkou. Pˇri výpoˇctu vzdálenosti se vyuˇzije faktu, ˇze normála roviny, na které leˇz´ı spojnice bod˚u hrany a B-spline kˇrivky, je stejná jako smˇerový vektor zadané hrany. Vzdálenost je vypoˇc´ıtána podle vztahu

d(C_j(P , t), B~ _i) = p

r_x+r_y +r_z, (18)

kderx,ry arz jsou kvadráty rozd´ıl˚u jednotlivých sloˇzek bodu na hranˇe a na B-spline kˇrivce, konkrétnˇe

r_x =

A_x− u_x(u_xA_X +u_yA_y +u_zA_z−u_xC_jx(t)−u_yC_jy(t)−u_zC_jz(t))

u²_x+u²_y+u²_z −C_jx(t) 2

r_y =

A_y− u_y(u_xA_X +u_yA_y+u_zA_z−u_xC_jx(t)−u_yC_jy(t)−u_zC_jz(t))

u²_x+u²_y+u²_z −C_jy(t) 2

(19) r_z =

A_z− u_z(u_xA_X +u_yA_y +u_zA_z−u_xC_jx(t)−u_yC_jy(t)−u_zC_jz(t))

u²_x+u²_y +u²_z −C_jz(t) 2

.

2.5.6 Funkce vzd´alenosti od stˇeny pˇrek´aˇzky

Pokud B-spline kˇrivka procház´ı Voroného buˇnkou, která vznikla ze stˇeny pˇrekáˇzky, poˇc´ıtá se vzdálenost bodu, který je dán B-spline kˇrivkou, a stˇeny zadané normálovým vektorem~n. Vzdálenost bodu B-spline kˇrivky a roviny lze poˇc´ıtat jako absolutn´ı hodnotu kolmého pr˚umˇetu vektoru bodu B-spline kˇrivky a libovolného bodu z roviny do normály roviny

d(C_j(P , t), B~ _i) =

C_j(P , t)~ −A

·~n

k~nk , (20)

kde~n je normálový vektor roviny a A je libovolný bod roviny [11].

2.5.7 Algoritmus v´ypoˇctu bezpeˇcnostn´ı funkce

Integrál v rovnici (12) je vypoˇc´ıtán numerickou metodou, konkrétnˇe Rombergovou metodou pro numerický výpoˇcet integrálu I =Rb

a f(x)dx [12].

Navrˇzený algoritmus 9 výpoˇctu bezpeˇcnostn´ı funkce je zaloˇzen na principu prohledáván´ı

(33)

do ˇs´ıˇrky.

Vstup: B-spline kˇrivka C, seznam Voron´eho bunˇek sc V´ystup: Hodnota bezpeˇcnostn´ı funkce F_B

1: Vymaˇz seznam open a close.

2: Urˇci prvn´ı buˇnkuf c, kde se nach´az´ı poˇc´atek B-spline C.

3: Pˇridej prvn´ı buˇnku f c na konec seznamu open.

4: F_B= 0

5: while open nen´ı pr´azdn´y do

6: Vezmi prvn´ı buˇnku k z open.

7: Odstraˇn prvn´ı buˇnku z open.

8: Odstraˇn vˇsechny prvky seznamu pr˚useˇc´ık˚up.

9: Odstraˇn vˇsechny prvky seznamu sousedn´ıch bunˇek soused.

10: for all stˇenuf z buˇnky k do

11: if stˇena f m´a pr˚useˇc´ık s B-spline C then

12: Pˇridej pr˚useˇc´ıky do seznamu pr˚useˇc´ık˚up.

13: Pˇridej sousedn´ı buˇnky pr˚useˇc´ık˚u do pomocn´eho seznamu sousedn´ıch bunˇek soused.

14: end if

15: end for

16: for all buˇnku cs ze seznamu soused do

17: if buˇnka cs nen´ı v close and buˇnka cs nen´ı v open then

18: Pˇridej buˇnkucs na konec seznamu open.

19: end if

20: end for

21: Vypoˇcti hodnotu bezpeˇcnostn´ı funkce F_k buˇnky k pro pr˚useˇc´ıky p.

22: F_B =F_B+F_k

23: end while

Algoritmus 9: Algoritmus v´ypoˇctu bezpeˇcnostn´ı funkce

Navrˇzený algoritmus poˇzaduje na ˇrádce 2, aby prvn´ı pˇridaná buˇnka do seznamu open mˇela alespoˇn jeden pr˚useˇc´ık s B-spline kˇrivkou. Pro vˇsechny buˇnky se na zaˇcátku spoˇc´ıtá vzdálenost jejich ˇcást´ı pˇrekáˇzek od poˇcáteˇcn´ıho bodu B-spline kˇrivky. Buˇnka s nejmenˇs´ı vzdálenost´ı se pˇridá do seznamu open jako prvn´ı. Tento postup lze vylepˇsit omezen´ım poˇctu testovaných bunˇek, protoˇze ze seznamu vˇsech vrchol˚u Voroného bunˇek je moˇzné zjistit, ve kterých buˇnkách se startovn´ı vrchol trajektorie nacházel pˇred malým náhodným posunut´ım.

(34)

Postup v´ypoˇctu pr˚useˇc´ıku stˇeny buˇnky a B-spline kˇrivky, viz ˇr´adek 11, popisuje algoritmus 10.

Vstup: B-spline kˇrivka C, Voron´eho buˇnka vc, stˇena f z vc, V´ystup: seznam pr˚useˇc´ık˚usp, seznam sousedn´ıch bunˇek soused

1: for all ˇc´astj zC do

2: Vypoˇcti koeficienty kubick´e rovnice r.

3: Vyˇreˇs kubickou rovnicir a ˇreˇsen´ı dej do seznamus.

4: for allˇreˇsen´ıs₀ z s do

5: if s₀ ∈ h0,1i and souˇradnice vrcholu leˇz´ı uvnitˇr stˇeny f then

6: Vytvoˇr pr˚useˇc´ık a pˇridej ho do seznamu pr˚useˇc´ık˚usp.

7: Vezmi sousedn´ı buˇnku, stˇeny f a pˇridej ji do seznamu soused.

8: end if

9: end for

10: end for

Algoritmus 10: Algoritmus výpoˇctu pr˚useˇc´ık˚u Voroného bunˇek a B-spline kˇrivky Na ˇrádku 21 algoritmu 9 se provád´ı výpoˇcet rovnice 12, seznam vˇsech pr˚useˇc´ıku je nejdˇr´ıve doplnˇen na sudý poˇcet pr˚useˇc´ık˚u. Pokud seznam pr˚useˇc´ıku obsahuje lichý poˇcet, pak se pˇridá pr˚useˇc´ık reprezentuj´ıc´ı zaˇcátek, respektive konec, B-spline kˇrivky, jestliˇze zaˇcátek, respektive konec, B-spline kˇrivky leˇz´ı v právˇe poˇc´ıtané Voroného buˇnce.

Takto doplnˇený seznam se sudým poˇctem pr˚useˇc´ık˚u je vzestupnˇe seˇrazen podle j a t, d´ıky tomu se integraˇcn´ı meze berou po dvojic´ıch. Dvojicet₁ t₂ na obrázku 12 urˇcuje jednu souvislou komponentu v buˇnce a dvojicet₃ t₄ dalˇs´ı. Z obrázku 12 je také zˇrejmé, ˇze souvislé komponenty nemus´ı konˇcit ve stejné ˇcásti B-spline kˇrivky v jaké zaˇcaly. V tomto pˇr´ıpadˇe je pak pro numerickou integraci souvislá komponenta rozdˇelena na menˇs´ı intervaly, které odpov´ıdaj´ı ˇcástem kˇrivky. Uvnitˇr pˇrekáˇzek je nastavena vzdálenost na nulovou hodnotu, coˇz zajiˇst’uje pˇr´ıpustnost optimalizované cesty.

2.5.8 Kombinace d´elky a bezpeˇcnosti

Pˇri optimalizaci trajektorie jsou kladeny poˇzadavky na bezpeˇcnost a na délku hladké trajektorie. Rovnice (21) váˇz´ı oba protich˚udné poˇzadavky,

F(P~) =αF_B(P~) + (1−α)F_D(P~), (21)

(35)

kde α∈ h0,1i je váha a F_D(P~) je délka B-spline kˇrivky, která se spoˇc´ıtá podle

F_D(P~) =

M

X

j=1

Z 1

0

s

∂C_jx

∂t (P , t)~ 2

+

∂C_jy

∂t (P , t)~ 2

+

∂C_jz

∂t (P , t)~ 2

dt, (22) kde M je poˇcet Coonsov´ych kubik tvoˇr´ıc´ıch B-spline kˇrivku.

2.6 Zhodnocen´ı navrˇ zen´ eho algoritmu

Metoda pˇr´ımého vyhledáván´ı obvykle konverguje pomaleji neˇz metody numerické optimalizace, které vyuˇz´ıvaj´ı znalost derivace hodnot´ıc´ı funkce. Pokud by se odvodil gradient bezpeˇcnostn´ı funkce podobnˇe, jak tomu je v [3], doba bˇehu optimalizace trajektorie by se zkrátila.

Definované ohraniˇcen´ı mapy pˇrekáˇzek pouze poskytuje ohraniˇcen´ı prostoru pro výpoˇcet Voroného bunˇek, aby se na nˇej dalo nahl´ıˇzet napˇr´ıklad jako na stˇenu m´ıstnosti, kde jsou pˇrekáˇzky um´ıstˇené uvnitˇr, musely by se jednotlivé stˇeny ohraniˇcen´ı zaˇradit do seznamu pˇrekáˇzek. Navrˇzený algoritmus pˇredpokládá pouze jednobodového robota. V [3] je uveden postup, jak pˇrevést polygonáln´ıho robota na jednobodové voz´ıtko. Metoda vyuˇz´ıvá k pˇrevodu nˇekolik bod˚u na okraji robota.

Popsaný pˇrevod naplánované cesty na B-spline kˇrivku m˚uˇze vytvoˇrit nepˇr´ıpustnou cestu pˇri pouˇzit´ı n´ızkého poˇctu ˇr´ıd´ıc´ıch bod˚u. Tento problém lze vyˇreˇsit pouˇzit´ım postupu, který nam´ısto pˇridán´ı kaˇzdéhoN-tého bodu bude sledovat smˇer cesty a ten respektovat, tj. rychlé zmˇeny naplánované cesty budou popsány v´ıce body neˇz rovné úseky.

Uvedená aproximace pˇrekáˇzek body funguje korektnˇe, pokud vzdálenost mezi nˇejakými dvˇema pˇrekáˇzkami nen´ı menˇs´ı neˇz nejmenˇs´ı hrana pˇrekáˇzky v mapˇe. D˚usledkem toho Voroného buˇnky zasahuj´ı do ˇcást´ı pˇrekáˇzek, kde má figurovat jiná buˇnka. Tento nedostatek by ˇslo odstranit pouˇzit´ım postupu, který by zohlednil velikost jednotlivých pˇrekáˇzek a jejich vzájemnou polohu.

(36)

3 Implementace

Navrˇzený program lze primárnˇe rozdˇelit na výpoˇcetn´ı a vizualizaˇcn´ı ˇcást. Implementace obou ˇcást´ı je provedena v programovac´ım jazyku C++.

Program je ˇclenˇen do jednotlivých tˇr´ıd, které reprezentuj´ı jednotlivé funkˇcn´ı bloky, jako jsou: reprezentace mapy, výpoˇcet Voroného bunˇek, plánovaˇc na vrcholech Voroného bunˇek, reprezentace B-spline kˇrivky a jej´ı optimalizace. Výpoˇcet Voroného bunˇek je realizován pomoc´ı knihovny Voro++, která je popsána v kapitole 3.2. Optimalizace trajektorie je provedena knihovnou OPT++, která je popsána v kapitole 3.3.

Jiˇz bˇehem vývoje se objevila nutnost zobrazit výsledky jednotlivých krok˚u. Pro vizualizaci byla zvolena knihovna VTK, která je popsána v kapitole 3.1. Tato knihovna byla vybrána, protoˇze je velmi dobˇre zdokumentovaná v podobˇe mnoha pˇr´ıklad˚u na webu a v knize [13]. Knihovna poskytuje struktury pro ukládán´ı dat a implementaci algoritm˚u poˇc´ıtaˇcové grafiky. Hlavn´ı struktura, kterou se pˇredává vˇetˇsina dat pro vizualizaci, je vtkP olyData, která obsahuje seznam bod˚u, vrchol˚u, úseˇcek, polygon˚u, trojúheln´ık˚u a pˇr´ıdavné informace o jednotlivých seznamech.

3.1 Knihovna VTK

Knihovna VTK Visualization Toolkit je objektovˇe orientovaná open-source knihovna pro 3D vizualizaci a zpracovan´ı dat, která je poskytovaná spoleˇcnost´ı Kitware. Knihovna VTK je implementována v C++, dále poskytuje rozhran´ı pro Tcl, Python a Javu. VTK podporuje znaˇcnou ˇskálu reprezentac´ı dat, jako jsou mnoˇziny bod˚u, polygony, obrázky a jiné. Zpracován´ı dat se provád´ı pomoc´ı filtr˚u, které obsahuj´ı implementace algoritm˚u pro poˇc´ıtaˇcovou grafiku. Pr˚ubˇeh vizualizace lze rozdˇelit do tˇr´ı krok˚u. Prvn´ı krok je naˇcten´ı dat, dalˇs´ı krok je zpracován´ı dat a posledn´ı krok je zobrazen´ı dat.

Vizualizaˇcn´ı okno je vytvoˇreno pomoc´ı tˇr´ıdy vtkRenderWindow. Tomuto objektu se pˇri- ˇrazuj´ı dalˇs´ı vlastnosti, nejd˚uleˇzitˇejˇs´ı jevtkRenderer obsahuj´ıc´ı zobrazovan´y objekt avtkRen- derWindowInteractor pro interakci vizualizaˇcn´ıho okna s uˇzivatelem [13].

3.2 Knihovna Voro++

Knihovna provád´ı výpoˇcet Voroného bunˇek pro kaˇzdý zadaný bod samostatnˇe. Vytvoˇr´ı kolem tohoto bodu Voroného buˇnku reprezentovanou jako mnohostˇen, který bod obklopuje.

Dále umoˇzˇnuje vytváˇret kontejnery, které seskupuj´ı jednotlivé body do podskupin, coˇz je

(37)

výhodné pro zvolený postup aproximace. Dalˇs´ı výhodou je, ˇze knihovna udrˇzuje informaci o okoln´ıch buˇnkách, která je vyuˇzita pro slouˇcen´ı jednotlivých bunˇek. [14]

3.3 Knihovna OPT++

Knihovna OPT++ je objektovˇe orientovaná knihovna, která poskytuje algoritmy pro ne- lineárn´ı optimalizaci. Základn´ı myˇslenka knihovny je m´ıt optimalizaˇcn´ı úlohu oddˇelenou od metody, která ji ˇreˇs´ı. Knihovna rozliˇsuje optimalizaˇcn´ı úlohy podle dostupnosti derivac´ı hodnot´ıc´ı funkce od nulté aˇz k druhé derivaci.

Metody pro hledán´ı minimáln´ı hodnoty hodnot´ıc´ı funkce, které jsou v knihovnˇe OPT++

implementované, lze rozdˇelit do ˇctyˇr skupin: 1) metody pˇr´ımého hledán´ı, 2) metody zobec- nˇených gradient˚u, 3) metody Newtonova typu a 4) metody vnitˇrn´ıho bodu. V´ıce informac´ı lze nalézt v [15] a v online dokumentaci [16].

Objekt reprezentuj´ıc´ı problém optimalizace trajektorie nemá dostupné derivace hodnot´ıc´ı funkce, proto je problém reprezentován instanc´ı tˇr´ıdy NLF0. Pˇri vytváˇren´ı instance tˇr´ıdy NLF0 nlp(ndim, optF unc, optInit) je ndim poˇcet reálných promˇenných ve vektoru x, optF uncje ukazatel na funkci, která poˇc´ıtá hodnotu hodnot´ıc´ı funkce, aoptInitinicializuje poˇcáteˇcn´ı hodnoty sloˇzek vektoru x. Takto sestavený problém se referenc´ı pˇredá optima- lizaˇcn´ı metodˇe. Optimalizaci je moˇzné ukonˇcit podle následuj´ıc´ıch podm´ınek: maximáln´ı poˇcet iterac´ı, maximáln´ı poˇcet vyhodnocen´ı hodnot´ıc´ı funkce a velikost´ı zmˇena hodnot´ıc´ı funkce v následuj´ıc´ı iteraci. Z knihovny OPT++ byla pro optimalizaci pouˇzita metoda

”Parallel Direct Search (PDS) Method“. Výsledné hodnoty promˇenných vektoruxse z´ıskaj´ı z definovaného problému. Optimalizace byla ukonˇcena, kdyˇz zmˇena hodnot´ıc´ı funkce v následuj´ıc´ı iteraci byla menˇs´ı neˇz 0,0001.

(38)

4 Experimenty

V této kapitole jsou uvedeny výsledky experiment˚u, které byly provedeny ke zjiˇstˇen´ı vlastnost´ı navrˇzeného algoritmu pro r˚uzné hodnoty parametruαz rovnice 21. Experimenty jsou provedeny na mapˇe pˇrekáˇzek, která je zobrazena na obrázku 13. Startovn´ı pozice je oznaˇcena zelenou koul´ı a c´ılová pozice je oznaˇcena ˇcervenou koul´ı. Na tomto obrázku 13 jsou také zobrazeny hrany Voroného bunˇek. Rozmˇery kvádru, který obklopuje mapu, jsou ve smˇeru osy x 6,45 m, ve smˇeru osy y 9,6 m a ve smˇeru osy z 4,1 m.

Obrázek 13: Mapa pˇrekáˇzek s naplánovanou cestou

Naplánovaná cesta pomoc´ı Dijkstrova algoritmu je pˇrevedena na B-spline kˇrivku, která je zobrazena na obrázku 14. Poˇcáteˇcn´ı bezpeˇcnost této trajektorie je 125,2 a délka je 10,0 m.

Tyto dvˇe hodnoty lze povaˇzovat jako výchoz´ı pro porovnán´ı optimalizované trajektorie.

Dále je na obrázku 14 vidˇet, ˇze B-spline kˇrivka procház´ı velmi bl´ızko rohu zelené pˇrekáˇzky, coˇz je nevýhodou popsaného algoritmu pˇrevodu naplánované cesty na B-spline kˇrivku, viz kapitola 2.4.1. Pro mapu na obrázku 13 bylo provedeno ˇsest simulac´ı, pˇri kterých se navzájem porovnaly výsledné trajektorie pro r˚uzné hodnoty parametru α. Výsledné hodnoty bezpeˇcnostn´ı funkce a délky trajektorie jsou uvedeny v levé ˇcásti tabulky 1. V pravé ˇ

cásti tabulky 1 jsou uvedeny výsledky experiment˚u pro mapu se dvˇema pˇrekáˇzkami.

(39)

Na obrázc´ıch 16, 17 a 18 jsou zaznamenané hodnoty bezpeˇcnostn´ı funkce, délky cesty a výsledné hodnot´ıc´ı funkce pro dané parametry α. Zaznamenané hodnoty jsou vˇzdy nej- menˇs´ı podle hodnot´ıc´ı funkce v pr˚ubˇehu optimalizace. Proces optimalizace byl ukonˇcen, kdyˇz zmˇena hodnot´ıc´ı funkce nebyla v následuj´ıc´ı iteraci vˇetˇs´ı jak 0,0001.

Na obr´azku 15a je zobrazena trajektorie pro hodnotu parametru α=1,0 , coˇz znamen´a, ˇ

ze na hodnot´ıc´ı funkci má vliv pouze bezpeˇcnostn´ı funkce. Výsledná trajektorie má pak nejvˇetˇs´ı vzdálenost od pˇrekáˇzek v mapˇe. Z pr˚ubˇehu grafu 16a je patrné, ˇze délka cesty se rychle dostane na hodnotu 15,8 m a hodnota funkce bezpeˇcnosti rychle klesne na hodnotu 3,4.

Na obrázku 15b je zobrazena trajektorie, pˇri které jsou obˇe kriteriáln´ı funkce váˇzené stejnˇeα=0,5. Z pr˚ubˇehu grafu 16b je vidˇet, ˇze nejdˇr´ıve strmˇe klesá hodnota bezpeˇcnostn´ı funkce, a kdyˇz se hodnoty bezpeˇcnostn´ı funkceF_Ba délky trajektorieF_D pˇribliˇznˇe rovnaj´ı, zaˇcne se postupnˇe zmenˇsovat i délka trajektorie. Pˇredchoz´ı tvrzen´ı dokládaj´ı obrázky 19. Na obrázku 19c je délka trajektorie nejdelˇs´ı, na dalˇs´ıch obrazc´ıch se zmenˇsuje. U experimentu 1 a 2 pˇrevládá vliv bezpeˇcnosti nad délkou cesty.

Obrázek 14: Poˇcáteˇcn´ı trajektorie v zadané mapˇe

Na obrázku 15c je zobrazena trajektorie pro α=0,05 . Výsledná hodnota bezpeˇcnostn´ı funkce je o nˇeco vetˇs´ı neˇz v pˇredchoz´ıch experimentech. Vliv délky cesty se projevuje,