42

Technické - cvut.cz

  • Upload
    others

  • View
    3

  • Download
    0

Embed Size (px)

Citation preview

Page 1: Technické - cvut.cz

eské Vysoké U£ení Te hni ké v PrazeFakulta Elektrote hni káBAKALÁSKÁ PRÁCE

Milan ProuzaPlánování pohybu forma e mobilní h robot·Katedra kybernetikyVedou í bakalá°ské prá e: Ing. Vojt¥ h Vonásek

Praha, 2012

Page 2: Technické - cvut.cz
Page 3: Technické - cvut.cz
Page 4: Technické - cvut.cz

AbstraktForma e mobilní h robot· jsou jedním z aktuální h sm¥r· výzkumu v mobilní roboti e. D·leºi-tou úlohou, kterou je nutné pro forma e mobilní h robot· °e²it, je plánování pohybu. V bakalá°sképrá i je popsáno plánování pohybu elé forma e s vyuºitím p°ístupu Leader-Follower, kdy je plánvytvo°en pouze pro vedou í robot, p°i£emº ostatní roboty si trajektorii odvozují na základ¥ p°e-depsaný h pozi ve forma i. Pro °ízení forma e je pouºita metoda Model Predi tive Control, jejíºhlavní a výpo£etn¥ nejnáro£n¥j²í £ástí je optimaliza e trajektorií. Vhodn¥ zvolená ini ializa£ní tra-jektorie m·ºe významn¥ ovlivnit jak kvalitu výsledného °e²ení, tak dobu optimaliza e. V této prá ibyl navrºen algoritmus pro nalezení po£áte£ní h trajektorií respektují í h kinemati ká omezení ro-bot·. Nalezení t¥ hto trajektorií je °e²eno geometri ky. Vliv metod plánování est na ry hlost akvalitu optimaliza e byl experimentáln¥ ov¥°en.Abstra tOne of the most attra tive areas in mobile roboti s are formations of mobile robots. In thisba helor thesis, we deal with motion planning for the formations. The Leader-Follower on ept isused to maintain the shape of the formation. In this on ept, the motion plan is reated only forthe leader robot, while the followers tra k the leader's traje tory in a predened distan e. Theformation is ontrolled along the given trajetories using the Model Predi tive Control approa h, inwhi h an optimization of the traje tories is performed. To speed up the optimization, a suitableinital solution should be provided to the optimization solver. We designed an algorithm to ndthe initial traje tories onsidering the kinemati onstraints of the robots. These traje tories are reated using several geometri path planning methods. The inuen e of the traje tory initializationto performan e of the optimization solver has been experimentally veried.

Page 5: Technické - cvut.cz

Obsah1 Úvod 12 Deni e úlohy 23 Metody hledání esty 43.1 Voroného diagram . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43.2 Nalezení esty ve Voroného diagramu . . . . . . . . . . . . . . . . . . . . . . . . . . . 43.3 Probabilisti Roadmap Method (PRM) . . . . . . . . . . . . . . . . . . . . . . . . . . 53.4 Nalezení esty v PRM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93.5 PRM smooth . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103.6 Srovnání metod VD a PRM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114 Nalezení trajektorie ar-like robotu 134.1 Prin ip hledání trajektorie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134.2 Algoritmus výpo£tu trajektorie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 154.3 P°íklady nalezený h trajektorií . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 175 ízení forma e ar-like robot· pouºitím Re eding Horizon Control 205.1 Kinemati ký model a zápis °ídi í h vstup· . . . . . . . . . . . . . . . . . . . . . . . . 205.2 Deni e forma e . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 215.3 Formula e úlohy plánování pohybu . . . . . . . . . . . . . . . . . . . . . . . . . . . . 225.4 Prin ip ustupují ího horizontu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 225.5 Implementa e pro °ízení forma e . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 225.6 Optimaliza e trajektorie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 246 Výsledky experiment· 266.1 Nastavení experiment· . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 266.2 Výsledky na map¥ empty . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 276.3 Výsledky na map¥ potholes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 296.4 Výsledky na map¥ test2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 316.5 Ov¥°ení nalezené trajektorie na systému SyRoTek . . . . . . . . . . . . . . . . . . . . 317 Záv¥r 35

Page 6: Technické - cvut.cz

1 ÚvodJedním z hlavní h úloh v mobilní roboti e je plánování pohybu robotu mezi dv¥ma místy na map¥.Poºadovaným výstupem této úlohy je trajektorie a °ídi í vstupy robotu, jeji hº aplika e zajistísledování nalezené trajektorie. Kritériemi pro ohodno ení kvality nalezené trajektorie jsou krom¥její délky na p°íklad i bezpe£ná vzdálenost od p°ekáºek nebo její k°ivost. V této prá i je °e²enaúloha plánování pohybu forma e mobilní h robot·. K °e²ení úlohy p°istupujeme tak, ºe hledámetrajektorie pouze pro jednoho robota. Pro nalezení trajektorií ostatní h robot· je vyuºit kon eptLeader-Follower. Nejprve se mezi dv¥ma zadanými polohami nalezne bezkolizní esta, následujevýpo£et trajektorie, která tuto estu sleduje, a zárove¬ je pro daný robot p°ípustná (robot jes hopen ji projet), a °ídi í h vstup·, které robot po této trajektorii povedou. Tato trajektorie jepotom optimalizována s pouºitím kvadrati kého programování a jsou vypo£ítány trajektorie v²e h£len· forma e. S héma postupu je znázorn¥né na Obr. 1.Tato prá e popisuje konkrétní implementa i plánování pohybu mobilního robotu typu ar-like iforma e t¥ hto robot·. Kone£nou optimaliza i a °ízení robot· obstarává kvadrati ký solver (SQP),ten ale pot°ebuje ini ializa£ní trajektorii, kterou následn¥ optimalizuje. Kvalita této vstupní tra-jektorie významn¥ ovliv¬uje výsledné °e²ení a stejn¥ tak dobu optimaliza e. Je tedy d·leºité zvolitvhodnou metodu k nalezení ini ializa£ní trajektorie. V této prá i jsou odzkou²eny t°i metody hle-dání esty: Probabilisti Roadmap, Probabilisti Roadmap s vyhlazenou estou a hledání estypomo í Voroného grafu.Pro zadanou po£áte£ní a ílovou kongura i robotu se tedy nejprve uºitím Voroného diagramu(VD) nebo metodou Probabilisti Roadmap (PRM) nalezne esta. Následn¥ se esta aproximujeúse£kami a £ástmi kruºni (Kapitola 4) tak, aby výsledná trajektorie byla pro ar-like robot p°í-pustná. Soub¥ºn¥ s aproxima í probíhá výpo£et °ídi í h vstup· robotu. Nalezená °ídi í sekven e sepak p°edá k optimaliza i kvadrati kému solveru, který je implementa í metody Re eding HorizonControl (Kapitola 5).Pro °ízení forma e mobilní h robot· je pouºita metoda Leader-Follower. Jejím vstupem je tra-jektorie leadera, která slouºí jako vzor pro trajektorie ostatní h £len· forma e. P°epo£et na tra-jektorii followera se provádí pomo í známý h vzájemný h pozi robot· ve forma i. Díky pouºitíkon eptu Leader-Follower není nutné p°i výpo£tu po£áte£ní trajektorie uvaºovat elou forma i, aproto se k hledání této trajektorie pouºívají metody ur£ené pouze pro jeden robot.start

cíl

start

cíl

nalezení cesty

1. 2. start

cíl

start

cíl

3. 4. start

cíl

5.

výpočet trajektorie

optimalizace řešení

řízení formaceObrázek 1: S héma plánování pohybu mobilního robotu pouºité v této prá i.1

Page 7: Technické - cvut.cz

2 Deni e úlohyPro pot°eby plánování pohybu nejprve denujeme nezbytné termíny. Car-like robot se zp·sobempohybu podobá automobilu, jeho s héma je uvedeno na Obr. 2. Pohyb ar-like robotu lze popsatrovni emi:x (t) = v (t) · cos (ϕ (t))

y (t) = v (t) · sin (ϕ (t)) (1)ϕ (t) = K (t) · v (t) ,kde kartézské sou°adni e robotu x, y a jeho azimut ϕ tvo°í kongura i robotu a k°ivost zatá£ky

K a ry hlost v jsou °ídi ími vstupy. Vztah mezi k°ivostí K a nato£ením kol θ je p°i vzdálenostip°ední h a zadní h kol l:K =

tan θ

l.K°ivost je oby£ejn¥ omezena hodnotami Kmin a Kmax, které plynou z konkrétní konstruk erobotu, stejn¥ tak ry hlost v je omezena hodnotami vmin a vmax.Tato prá e p°edpokládá následují í zjednodu²ení ar-like robotu:1. robot m·ºe zatá£et na ob¥ strany se stejnou maximální velikostí k°ivosti, tedy:

Kmax > 0 a Kmin = −Kmax,2. robot se smí pohybovat pouze sm¥rem dop°edu, tedy:vmin = 0 ≤ v ≤ vmax, a3. robot lze dob°e aproximovat kruhovým robotem s polom¥rem r (nazna£eno na obrázku 2).

φ

Φ

x

y

l

r

Obrázek 2: Car-like robot s vyzna£eným radiusem pro detek i kolizí.Mapa je v této bakalá°ské prá i dvourozm¥rná a je vymezena polygonálními p°ekáºkami. Prostoruvnit° kteréhokoli polygonu je nedostupný, roboty se mohou pohybovat pouze vn¥ polygon·. Jelikoº2

Page 8: Technické - cvut.cz

je pouºit CFSQP solver pro optimaliza i trajektorie, jsou p°ekáºky sloºeny z 2D box·, oº jsouobdélníky r·zný h rozm¥r· a rota í.Kongura e robotu je vektor sou°adni , které jednozna£n¥ popisují polohu robotu. Tato prá euvaºuje ar-like roboty, u který h je zapot°ebí krom¥ pozi e robotu v kartézský h sou°adni í h xa y , p°idat sou°adni i ϕ ur£ují í azimut (heading) robotu. V²e hny kongura£ní sou°adni e jsouznázorn¥ny na Obr. 2. Volná kongura e je taková, p°i které robot nekoliduje s ºádnou p°ekáºkou.Pro zjednodu²ení p°edpokládáme kruhový robot s polom¥rem r; sou°adni e x, y ur£ují polohust°edu robotu - díky tomuto p°edpokladu lze kongura i prohlásit za volnou, jestliºe vzdálenostst°edu robotu od p°ekáºky je v¥t²í neº r.Cestu denujme jako posloupnost volný h kongura í spojený h hranami. V této prá i se krom¥startovní kongura e p°i hledání esty neuvaºuje sou°adni e ϕ, takºe body esty jsou ur£ené kar-tézskými sou°adni emi, p°i£emº na spojni i mezi po sob¥ jdou ími kongura emi neleºí ºádnáp°ekáºka. Cesta za£íná startovní kongura í robotu a kon£í ílovou kongura í. Vybrané metodyhledání esty jsou popsány v kapitole 3. V této prá i se krom¥ startovní kongura e p°i hledání esty neuvaºuje sou°adni e ϕ, takºe body esty jsou ur£ené pouze kartézskými sou°adni emi x a y.Cílem hledání esty je nalezení volné esty z po£áte£ní do ílové kongura e.Trajektorie je k°ivka, po které se robot pohybuje, dopln¥ná o vektor °ídi í h vstup· a stejn¥velký £asový vektor s dobami trvání jednotlivý h vstup·. V tomto textu pouºijeme zjednodu²ují íp°edpoklad, ºe robot zatá£í pouze s k°ivostmi zatá£ky 0, Kmax nebo −Kmax. Z této podmínkyplyne, ºe se trajektorie bude vºdy skládat pouze z úse£ek a £ástí kruºni s polom¥rem r = 1Kmax .Hledání trajektorie z po£áte£ní do kon ové pozi e se nazývá plánování pohybu. Na rozdíl odmetod plánování esty je jeji h výstupem sekven e °ídi í h vstup·, kterými lze robota °ídit podlevypo£tené trajektorie.

3

Page 9: Technické - cvut.cz

3 Metody hledání estyÚkolem algoritm· popsaný h v této kapitole je nalézt bezkolizní estu pro mobilní robot. Uvedenyjsou dva £asto pouºívané p°ístupy: geometri ká metoda hledání esty ve Voroného diagramu (VD)a randomizovaná metoda Probabilisti Roadmap (PRM).3.1 Voroného diagramKdyº je p°i hledání esty jedním z hlavní h poºadavk· dostate£ná vzdálenost od p°ekáºek, nabízí se estu hledat ve Voroného diagramu. Bodový Voroného diagram [6 je datová struktura rozd¥lují írovinu na bu¬ky podle vzdáleností k diskrétní mnoºin¥ bod· B. Kaºdému bodu B je p°i°azenajedna bu¬ka V (B). Tato bu¬ka vymezuje podmnoºinu bod· roviny, které mají mezi body mnoºinyB nejmen²í vzdálenost k bodu B. Zobe n¥ním této struktury je segmentový Voroného diagram.Jsou-li p°ekáºky v map¥, ve které se robot pohybuje, popsány mnoºinou úse£ek S, segmentovýVoroného diagram této mapy rozd¥lí rovinu tak, aby kaºdé úse£ e u ∈ S byla p°id¥lena oblastV (u), pro kterou platí, ºe libovolný bod z této oblasti je nejblíºe práv¥ k úse£ e u. Pro hledání esty jsou zajímavé hrany t¥ hto oblastí, nebo´ ty v map¥ vytvá°ejí esty vedou í vºdy v nejv¥t²ímoºné vzdálenosti od p°ekáºek, které daný úsek esty vymezují. Robot pohybují í se po takové est¥ tak minimalizuje riziko kolize s p°ekáºkou. Dal²í výhoda pouºití Voroného diagramu je, ºeje touto metodou esta mezi dv¥ma body nalezena vºdy, kdyº existuje na rozdíl nap°. od níºepopsaného PRM.Problémem je itlivost na ²um v informa í h o map¥, protoºe ze zrnitý h dat je výpo£et grafuobtíºný, a dále netriviální implementa e algoritmu pro výpo£et Voroného diagramu. Pro pot°ebytéto prá e byl pouºit program Martina Heldse Vroni [10, který pouºívá algoritmus se sloºitostíO(n logn).Základní implementa e hledání esty ve Voroného grafu také oproti PRM neuvaºuje velikostrobotu, proto m·ºe nalezená esta vést zúºenou £ástí mapy, která je ov²em pro robot nepr·jezdná.Tento problém °e²í nafouknutí p°ekáºek o polom¥r robotu a vytvo°ení Voroného grafu pro upra-venou mapu. Místa, která by pro robot byla p°íli² úzká, se uzav°ou úpln¥ a hrany grafu uº skrz n¥nepovedou. V této prá i byl testován jen základní algoritmus bez nafukování p°ekáºek.3.2 Nalezení esty ve Voroného diagramuPro es hledání esty za£íná sestrojením Voroného diagramu dané mapy. Následuje jeho úprava protoºe hrany Voroného diagramu pro házejí kon ovými body úse£ek, je pot°eba kolidují í body zdiagramu odstranit, jinak by esta mohla vést skrz p°ekáºky. Takto upravený diagram lze v p°ípad¥nem¥nné mapy u hovat a pouºívat pro hledání esty mezi r·znými body opakovan¥.Druhou fází je p°idání po£áte£ní a ílové pozi e, jeji h napojení na Voroného diagram a nakone nalezení esty v grafu. Napojení p°idaný h bod· se provede k nejbliº²í £ásti diagramu, se kteroulze body spojit bez kolize s p°ekáºkou.Pokud startovní a ílová pozi e leºí ve stejné bu¬ e Voroného diagramu, mohou spolu býtokamºit¥ propojeny bez napojování na graf. V této bakalá°ské prá i je ale algoritmus implementovántak, aby vºdy esta vedla po hraná h Voroného diagramu. Z·stane tak totiº za hována jedna z výhodtéto metody, esta vºdy povede v maximální vzdálenosti od p°ekáºek.Tento jev v²ak není vºdy jen výhodou. V prostorný h £áste h mapy si robot zbyte£n¥ zajíºdík hranám VD. Extrémním p°ípadem je hledání esty v prázdné £tver ové map¥, kde povede esta4

Page 10: Technické - cvut.cz

vºdy po úhlop°í£ká h. Naopak nejv¥t²í uºitek p°íná²í tato vlastnost p°i plánování esty se zúºenými hodbami.Konkrétní p°íklady nalezený h est jsou na Obráz í h 3 a 4. Zatím o první obrázek ukazujesilnou stránku esty nalezené ve Voroného diagramu, kdy si metoda poradí s úzkými pr· hody, nadruhém obrázku ze stejné mapy je vid¥t slabina této metody hledání esty ve volném prostranství.

-10

-5

0

5

10

-10 -5 0 5 10

.Obrázek 3: Cesta z Voroného diagramu spoleh-liv¥ pro hází úzkými hodbami.-11

-10

-9

-8

-7

-6

-5

-4

-11 -10 -9 -8 -7 -6 -5 -4

Ořezaný Voroného diagramNalezená trajektorieNalezená cestaObrázek 4: Chování esty z Voroného diagramuve volném prostoru.3.3 Probabilisti Roadmap Method (PRM)Probabilisti Roadmap Method [14 umoº¬uje plánovat pohyb robotu s mnoha stupni volnosti a lzeji bez v¥t²í h komplika í pouºít i se za²um¥nými informa emi o map¥. Plánování probíhá ve dvoufází h. Ve fázi u£ení se vygeneruje graf, jehoº uzly jsou náhodn¥ generované volné kongura e robotua hranami jsou esty mezi t¥mito kongura emi dosaºitelné jednodu hým lokálním plánova£em(nap°. ve 2D to m·ºe být propojení úse£kou).Druhá fáze je dotazova í (query), ve které se do grafu p°idá po£áte£ní a poºadovaná kongura e,propojí se s p°ilehlými vygenerovanými body a následn¥ se mezi nimi pomo í libovolného algoritmuhledání esty v grafu nalezne esta.Pokud jsou body PRM generovány rovnom¥rn¥, je ve zúºený h £áste h mapy riziko, ºe do ni hnebude vygenerován dostatek bod·, které by zajistily vytvo°ení hran vedou í h skrz oblast (Obr.5). Tomuto neºádou ímu hování lze zabránit jiným neº rovnom¥rným generováním náhodný hbod·.

5

Page 11: Technické - cvut.cz

-10

-8

-6

-4

-2

0

2

-10 -8 -6 -4 -2 0 2

Hrany PRMBody PRM

Obrázek 5: P°íklad selhání PRM. Hrany nepropojily dv¥ místnosti odd¥lené úzkým pr· hodem,nebo´ se p°i u£í í fázi nepoda°ilo v tomto pr· hodu vygenerovat dostate£n¥ hustý náhodný vzorek.Vstup: Q mnoºina polygon· tvo°í í h p°ekáºkyVýstup: R = (P,E) graf PRMP := ;E := ; nt := poºadovaný po£et bod·;k := po£et nejbliº²í h soused·, se kterými se pokusí spojit;rmin := minimální vzdálenost od p°ekáºky / velikost robotu;// nalezení poºadovaného po£tu volný h konfigura íwhile |P | < nt do

p := náhodný bod v map¥;if vzdálenost p od libovolného polygonu z Q > r thenP := P ∪ p;endend// vytvo°ení hran mezi konfigura emiforea h p ∈ P dofor i = 1 to k don := i-tý nejbliº²í soused;if vzdálenost úse£ky |pn| od libovolného polygonu z Q > rmin then

E := E ∪ (p, n);endendend Algoritmus 1: PRM u£í í fáze6

Page 12: Technické - cvut.cz

Vstup: Q mnoºina polygon· tvo°í í h p°ekáºky, R = (P,E) graf vytvo°ený p°i u£í ífázi, Pstart, Pend startovní a ílová pozi eVýstup: P nalezená esta (sekven e bod·)k := po£et nejbliº²í h soused·, se kterými se pokusí spojit;rmin := minimální vzdálenost od p°ekáºky / velikost robotu;// zapojení startovního a ílového bodu do grafuP := P ∪ Pstart, Pend;forea h p ∈ Pstart, Pend dofor i = 1 to k do

n := i-tý nejbliº²í soused;if vzdálenost úse£ky |pn| od libovolného polygonu z Q > rmin thenE := E ∪ (p, n);endendend// nalezení estyDijkstr·v alg. na grafu R z bodu Pstart do bodu Pend;výsledek uloº do P ; Algoritmus 2: PRM dotazova í fázeJednou z moºný h strategií je gaussovské generování kolem okraj· [4. Postup dosaºení normál-ního rozd¥lení hustoty bod· je ukázán v Algoritmu 3. Nejprve se zvolí vzdálenost d, která ur£ujevelikost rozptylu výsledného rozd¥lení. V kaºdém yklu algoritmu je vygenerován náhodný bod p1a následn¥ náhodný bod p2 ve vzdálenosti d od p1. Pokud je kongura e v bod¥ p1 volná a kongu-ra e v bod¥ p2 není volná, je do grafu p°idán bod p1. Naopak pokud je volná pouze kongura e vbod¥ p2, je do grafu p°idán bod p2. Pokud jsou volné ob¥ kongura e, není do grafu p°idán ºádnýbod. D·sledkem tohoto postupu je zamezení generování bod· v £áste h mapy bez p°ekáºek, ov²emza enu del²í doby trvání u£í í fáze.Podobný prin ip generování bod· pro PRM je pouºit v metod¥ Bridge Test [12. Na rozdíl odgaussovského generování podél okraj· p°ekáºek je ale jeho ílem generovat body pouze v úzký h£áste h mapy. Pr·b¥h generování je zapsán v pseudokódu v Algoritmu 4 a vypadá následovn¥.Nejprve se vybere se náhodný bod p1. Pokud kongura e v tomto bod¥ není volná, vybere sedruhý bod p2 s pozi í generovanou podle Gaussova rozd¥lení pravd¥podobnosti se st°edem v p1.Pokud ani v tomto bod¥ kongura e není volná, otestuje se kongura e ve st°edu úse£ky |p1p2|, ajestliºe tato kongura e je volná, p°idá se do grafu PRM.Srovnání rozloºení bod· generovaný h v²emi uvedenými metodami (základní, gaussovské, sBridge Testem) je na Obrázku 6. V této prá i byl pouºit základní algoritmus PRM s rovnom¥rnýmgenerováním bod·, protoºe pouºité mapy neobsahují úzké pr· hody a základní strategie u£í í fázese pro n¥ ukázala jako dosta£ují í.

7

Page 13: Technické - cvut.cz

(a) (b) ( )Obrázek 6: Srovnání typi kého rozloºení bod· PRM p°i pouºití: a) rovnom¥rného generování bod·,b) gaussovského generování bod· kolem okraj· p°ekáºek a ) generování bod· s metodou BridgeTest.while |P | < nt dop1 := náhodný bod v map¥;d := vzdálenost zvolená podle poºadovaný h parametr· normálního rozd¥lení;p2 := náhodný bod ve vzdálenostidod bodup1;if p1 JE volná a p2 NENÍ volná then

P := P ∪ p1;endelse if p1 NENÍ volná a p2 JE volná thenP := P ∪ p2;endelse// ob¥ konfigura e jsou volnép1 ani p2 se nepouºije;endend Algoritmus 3: PRM Gaussovské generování

8

Page 14: Technické - cvut.cz

while |P | < nt dop1 := náhodný bod v map¥;if p1 NENÍ volná then

p2 := náhodný bod v okolí p1;if p2 NENÍ volná thenpc := st°ed úse£ky |p1p2|;if pc JE volná then

P := P ∪ pc;endendendend Algoritmus 4: PRM Bridge Test3.4 Nalezení esty v PRMPro hledání esty robotu v map¥ s polygonálními p°ekáºkami m·ºe být metoda implementovánanásledovn¥. Roboty v této prá i mají dvourozm¥rný kongura£ní prostor, takºe kongura e robotuje dána dv¥ma sou°adni emi x a y. Kongura i povaºujeme za volnou, pokud je vzdálenost bodu[x, y] od p°ekáºky v¥t²í nebo rovna rmin, kde rmin je dáno velikostí robotu (p°edpokládáme kruhovýrobot).Dále je pot°eba ur£it, zda jsou dv¥ zvolené kongura e propojitelné pomo í lokálního plánova£e.Lokální plánova£ to ov¥°í tak, ºe se pokusí spojit kongura e úse£kou. Pokud tato úse£ka nekoli-duje s ºádnou p°ekáºkou a její vzdálenost od libovolné p°ekáºky je nejmén¥ rmin, kongura e jsoupropojitelné. Odstup hran od p°ekáºek je dob°e pozorovatelný na Obr. 8.

0

1

2

3

4

5

-4 -2 0 2 4Obrázek 7: Ukázka esty získané metodou PRM.Pokou²et se vzájemn¥ propojit v²e h n vygenerovaný h kongura í je výpo£etn¥ náro£né (sloºi-tost je O(n2)), proto se kaºdá kongura e pokusí spojit s pevn¥ zvoleným po£tem nejbliº²í h sou-sed·, jeji hº hledání je ury hleno pouºitím KD-stromu. Nalezení nejbliº²ího souseda v KD-stromumá sloºitost O(log n). 9

Page 15: Technické - cvut.cz

P°i u£í í fázi bude tedy algoritmus postupovat takto: bude generovat náhodné kongura e,dokud nezíská n volný h kongura í. Kaºdou z ni h se pokusí propojit hranami s k nejbliº²ímisousedy. Potom zbývá p°idat do vygenerovaného grafu startovní a ílovou kongura i, ty op¥tpropojit s k nejbliº²ími sousedy a pouºít algoritmus hledání esty v grafu, nap°. Dijkstr·v.Výstupem algoritmu PRM je sekven e bod·, ve které pro kaºdé dva po sob¥ jdou í body platí,ºe se mezi nimi robot m·ºe bez kolizí pohybovat po jeji h spojni i. Pro °ízení ar-like robotu jepot°eba nalézt trajektorii, která se bude této esty drºet a zárove¬ bude respektovat manévrova ímoºnosti robotu.

0.5

1

1.5

2

2.5

-3.5 -3 -2.5 -2 -1.5

Hrany PRMBody PRMNalezená cesta

Obrázek 8: Bliº²í pohled na estu nalezenou algoritmem PRM.3.5 PRM smoothNalezená esta je zpravidla velmi kostrbatá a vyplatí se ji p°ed dal²ím zpra ováním vyhladit, na p°í-klad pouºitím iterativního odstra¬ování redundantní h bod· [9. Metoda je zapsaná v pseudokóduv Algoritmu 5. První bod esty P1 se pokusí propojit úse£kou se t°etím bodem P3, pokud úse£kanekoliduje s ºádnou p°ekáºkou, je odstran¥n bod P2 a bod P1 se pokusí propojit s následují ímbodem a pokra£uje aº k bodu Pn, u kterého dojde na spojni i bod· ke kolizi s p°ekáºkou. Stejnýmzp·sobem pak algoritmus pokra£uje z bodu Pn−1 atd., dokud nedojde na kone esty. Ukázka estyvyhlazené leh e obm¥n¥ným algoritmem je na Obr. 9. V rám i této prá e byly testovány jak nevy-hlazené esty p°ímo z PRM, tak i esty upravené tímto algoritmem. PRM dopln¥né o vyhlazova íalgoritmus bude níºe uvád¥no jako PRM smooth nebo PRMs.10

Page 16: Technické - cvut.cz

0

1

2

3

4

5

-4 -2 0 2 4

Hrany PRM

Nalezená trajektorie

Vyhlazená cesta

Obrázek 9: Cesta nalezená algoritmem PRM po vyhlazení.Vstup: Q mnoºina polygon· tvo°í í h p°ekáºky, P nalezená estaVýstup: Psmooth vyhlazená estaPsmooth := P ;i := 0;while i < |Psmooth| − 2 doif vzdálenost úse£ky |pipi+2| od libovolného polygonu z Q > rmin then

Psmooth := Psmooth \ pi+1;if i > 0 then i := i− 1endelsei := i+ 1;endend Algoritmus 5: PRM vyhlazení esty odtran¥ním p°ebyte£ný h bod·3.6 Srovnání metod VD a PRMVý²e byly popsány dv¥ jednodu hé metody hledání esty. První z ni h spo£ívá v nalezení estyv segmentovém Voroného diagramu. Její hlavní výhodou je, ºe nalezená esta vede v maximálnívzdálenosti od p°ekáºek, £ímº se pro robot pohybují í se po této est¥ minimalizuje riziko kolizes p°ekáºkou. Jde o metodu deterministi kou, která nalezne estu vºdy, kdyº existuje. Nevýhodouje, ºe v základní verzi metoda nepo£ítá s nenulovou velikostí robotu, proto nalezená esta m·ºevést úzkými pr· hody, kudy skute£ný robot nedokáºe projet. Dal²ími problémy jsou sloºitá imple-menta e algoritmu pro výpo£et Voroného diagramu a itlivost na ²um v date h popisují í h mapu,který komplikuje vytvo°ení VD.Druhou popsanou metodou bylo PRM. Na rozdíl od VD jde o metodu sto hasti kou, kteráprobíhá ve dvou fází h. Ve fázi u£ení se navzorkuje kongura£ní prostor náhodným¥ generovanýmivolnými kongura emi, které se následn¥ pokusí propojit lokálním plánova£em (nap°. ve 2D pro-pojení úse£kou) s n¥kolika nejbliº²ími sousedy. Výstupem u£í í fáze je graf, který na map¥ tvo°ísí´ est. P°i dotazova í fázi je do grafu p°idána startovní a ílová kongura e a algoritmem prohledání esty v grafu se získá výsledná esta. Výhodami této metody jsou velmi jednodu há imple-11

Page 17: Technické - cvut.cz

menta e, odolnost v·£i ²umu a moºnost p°idávat do grafu jen body a hrany, které jsou pro robots nenulovou velikostí bezkolizní. P°i rovnom¥rném generování bod· má ale algoritmus problémy sp°ekonáním zúºený h £ástí mapy, kde je malá pravd¥podobnost vygenerování bodu. Existují si e ijiné strategie generování bod· PRM, které tuto nevýhodu potla£ují, v rám i této prá e byl ale pou-ºit pouze základní algoritmus PRM. Cesta nalezená metodou PRM je kostrbatá, proto je výhodnéji odstran¥ním redundantní h bod· vyhladit a zjednodu²it.Ani jedna z metod neposkytuje p°ímo trajektorii ar-like robotu, ale pouze sekven i bod·, kterétvo°í bezkolizní (v p°ípad¥ VD ne nutn¥) estu ze startovní kongura e do ílové. V Kapitole 4 jepopsán algoritmus, který tuto estu geometri ky aproximuje trajektorií ar-like robotu a vypo£te°ídi í vstupy.

12

Page 18: Technické - cvut.cz

4 Nalezení trajektorie ar-like robotuP°ed hozí kapitoly se zabývaly metodami nalezení esty mezi dv¥ma body na map¥, které ov²em ne-poskytují pot°ebnou trajektorii robotu typu ar-like. K nalezení trajektorie slouºí tzv. path followingalgoritmy [20. Protoºe je v¥t²inou pot°eba po£ítat s od hylkami od vypo£tené trajektorie zp·sobe-nými prokluzováním kole£ek p°ípadn¥ jinými nep°esnostmi v °ízení, je vhodné realizovat °ízení jako losed-loop smy£ku, která má na vstupu krom¥ poºadované trajektorie i aktuální polohu robotu.Zp¥tnovazební regulátory ur£ené k °ízení b¥ºný h typ· robot· jsou popsány v [7, 2, 8, 17, 13.V této prá i je k °ízení forma e pouºit kon ept Leader-Follower a Re eding Horizon Control.Sta£í tedy pouºít open-loop metodu, která pro zadanou estu nalezne °ídi í vstupy ar-like robotu,které robot dokáºí podél této esty °ídit. Vypo£ítaná sekven e °ídi í h vstup· pak projde optima-liza£ním solverem a je pouºita k °ízení forma e metodou RHC, která obstará pot°ebnou zp¥tnouvazbu, a to jak na p°ípadné od hylky od trajektorie, tak na nov¥ detekované p°ekáºky.Vstupem pouºitého path following algoritmu je esta popsaná mnoºinou po sob¥ jdou í h bod·,výstupem je sekven e °ídi í h vstup· pro ar-like robot. Tyto vstupy zajistí pohyb robotu potrajektorii, která aproximuje vstupní estu. Kaºdý vstup se skládá z ry hlosti pohybu, k°ivostizatá£ení a doby svého trvání.Algoritmus p°edpokládá, ºe ry hlost pohybu robotu je konstantní a ºe robot p°i zatá£ení pouºívápouze t°i k°ivosti, a to bu¤ −Kmax, 0 nebo Kmax. Trajektorie pohybu takového ar-like robotu jetvo°ená úse£kami, které jsou propojené £ástmi kruºni s polom¥rem 1Kmax . Po aproximování zadanévstupní esty t¥mito geometri kými útvary lze snadno získat vstupy pro °ízení robotu.4.1 Prin ip hledání trajektorieBody vstupní esty ozna£me P = P1, P2, ... , PN. Pro kaºdou troji i bod· Pn, Pn+1, Pn+2 jet°eba nalézt st°ed Cn kruºni e s polom¥rem r = 1

Kmax , která je vepsaná do úhlu ∠Pn Pn+1 Pn+2, ajejí body dotyku X1n, X2n s úse£kami |Pn Pn+1| a |Pn+1 Pn+2| (Obr. 10).Výstup tvo°í sekven e vektor· °ídi í h vstup· U = u1, u2, ... , u2N−3. Kaºdý vektor umobsahuje ry hlost pohybu (konst.), k°ivost zatá£ky (nabývá hodnot −Kmax, 0 a Kmax) a dobutrvání daného °ídi ího vstupu.Je známo, ºe st°ed kruºni e leºí na ose úhlu ∠Pn Pn+1 Pn+2 a vzdálenost |CnX1n| = |CnX2n| =r. Dále je z°ejmé, ºe úhly ∠Pn+1X1n Cn a ∠Pn+1X2n Cn jsou pravé. Ozna£me:

α1 = atan2 (PnY − P(n+1)Y , PnX − P(n+1)X

)

,

α2 = atan2 (P(n+2)Y − P(n+1)Y , P(n+2)X − P(n+1)X

)

,

α =|∠Pn Pn+1 Pn+2|

2=α2 − α1

2a

αC = |∠Pn Pn+1 C| = α1 + α.Vzdálenost st°edu kruºni e od vr holu úhlu ozna£íme d = |Pn+1Cn|, vzdálenosti bod· dotykuod vr holu úhlu jsou pro oba body stejné a ozna£íme je x = |Pn+1X1n| = |Pn+1X2n|.Vzdálenost d lze spo£ítat z pravoúhlého trojúhelníka Pn+1CnX1n:d =|CnX1n|sinα

=r

sinα.13

Page 19: Technické - cvut.cz

r

r

d

Pn+1

Pn+2

x

x

Cn

α1

α2

α

αC

Pn

K=0

K= r-1

X1n

K=0

K= r-1

X2n

Obrázek 10: Aproxima e zatá£ky esty dv¥ma úse£kami a £astí kruºni e.Vzdálenost x vyjde z Pythagorovy v¥ty: x =√d2 − r2. Nyní sta£í pomo í úhl· a vzdálenostíspo£ítat sou°adni e v²e h t°í hledaný h bod·:

Cn =[

P(n+1)x + d · cosαC , P(n+1)y + d · sinαC

]

X1n =[

P(n+1)x + x · cosα1, P(n+1)y + x · sinα1

]

X2n =[

P(n+1)x + x · cosα2, P(n+1)y + x · sinα2

]

.Pokud je n > 1, skon£il minulý krok v bod¥ X2(n−1). Trajektorie mezi X2(n−1) a X1n je p°ímo-£ará, proto je p°idán °ídi í vstup s nulovou k°ivostí:v = konst., K = 0, ∆t =

∣X2(n−1)X1n

v,kde ∣∣X1nX2(n−1)

∣ je délka p°ímo£arého úseku. V p°ípad¥, ºe n = 1, je p°idán tento °ídi í vstup:v = konst., K = 0, ∆t =

|P1X11|v

.Délka i sm¥r zatá£ky jsou ur£eny rozdílem úhl· δ = atan2 (X2nY − CnY , X2nX − CnX) −atan2 (X1nY − CnY , X1nX − CnX). Úhel δ je t°eba normalizovat do interval· (0; π), resp. (0, −π).Potom znamená záporné znaménko zatá£ku doleva, proti sm¥ru ru£i£ek a kladné znaménko zatá£kudoprava, ve sm¥ru ru£i£ek. ídi í vstup pro vykonání zatá£ky vypadá následovn¥:v = konst., K = sign (δ) ·Kmax, ∆t = |δ|

v ·Kmax ,kde |δ|Kmax je délka oblouku zatá£ky.V posledním kroku algoritmu se p°idá záv¥re£ná p°ímo£ará £ást trajektorie:

v = konst., K = 0, ∆t =

∣X2(N−2)PN

v.14

Page 20: Technické - cvut.cz

4.2 Algoritmus výpo£tu trajektorieVý²e uvedený postup je nutné roz²í°it, nebo´ m·ºe selhat, pokud jsou body vstupní trajektorievelmi blízko u sebe nebo pokud jsou úhly mezi jednotlivými segmenty trajektorie p°íli² ostré. Obojízp·sobí, ºe se hledané body dotyku objeví mimo p°íslu²né úse£ky, tedy X1n /∈ |PnPn+1| ∨ X2n /∈|Pn+1Pn+2|, nebo jednodu²eji x > ‖PnPn+1‖ ∨ x > ‖Pn+1Pn+2‖ (Obr. 11), nebo ºe se zatá£kabude p°ekrývat se zatá£kou p°ed hozí, tedy ∥∥X2(n−1)Pn+1

∥ < ‖X1nPn+1‖, kde X2(n−1) je kone p°ed hozí zatá£ky (Obr. 12). V takovém p°ípad¥ by vypo£tené °ídi í vstupy byly hybné. Obaproblémy lze vy°e²it p°ímo£arým za ouváním robotu o pot°ebnou vzdálenost, ov²em tato prá euvaºuje pouze dop°edný pohyb robot·.Cn-1

rPn+2

X2n

Pn+1

X1n

Pn

Cn

Pn+3

Pn-1

X2(n-1)

Pn

Cn

X1n

Pn+1

X2n

Pn+2

Cn-1

X1(n-1) Pn-1

Cn-1

X2(n-1)

X1(n-1)Obrázek 11: Zjednodu²ení esty p°i hyb¥ X2n /∈ |Pn+1Pn+2| .e²ením za tohoto omezení je zjednodu²ování trajektorie aº do té míry, aby byla pro algoritmuspr· hozí bez hyb. Zjednodu²ování se provede odebíráním p°ebyte£ný h bod·. (Alg. 6).Nejd°ív je pot°eba zjistit, v jaký h kro í h do²lo k problému. Algoritmus hledá kruºni i vepsanoudo úhlu ∠Pn Pn+1 Pn+2 zp·sobem popsaným vý²e. Potom otestuje, jestli body dotyku kruºni e sevstupní estou X1n a X2n leºí na p°íslu²ný h úse£ká h esty, a také to, jestli bod X1n leºí na úse£ e|PnPn+1| blíº k vr holu úhlu Pn+1 neº kone p°e hozí zatá£ky X2(n−1).Jako první se kontroluje, zda platí nerovnost ∥∥X2(n−1)Pn+1

∥ > ‖X1nPn+1‖. Pokud ne, je odstra-n¥n bod Pn+1 a poslední dva °ídi í vstupy u2·n−2 a u2·n−3. Algoritmus v dal²ím kroku pokra£ujes hodnotou n := n− 1.Kdyº první hyba nenastane, zkontroluje se, jestli leºí bod X2(n−1) na úse£ e |Pn+1Pn+2|, oºje ekvivalentní s podmínkou x < ‖Pn+1Pn+2‖. Pokud ne, znamená to, ºe je nutné tuto hranuprodlouºit, £ehoº se dosáhne odstran¥ním bodu Pn+2. P°ed hozí °ídi í vstupy z·stanou beze zm¥nya v dal²ím kroku pouºije algoritmus stejnou hodnotu n, jako v tomto kroku.Situa i X1n /∈ |PnPn+1| je pot°eba kontrolovat pouze pro n = 1, protoºe ostatní p°ípady po-kryje první podmínka. Jelikoº odstran¥ní bodu P2 by znamenalo ztrátu informa e o startovnímazimutu robotu, je odebrán bod P3 a algoritmus v dal²ím kroku op¥t pouºije n := 1. Algoritmus lzezjednodu²it denováním X20 := P1 a drobým upravením bloku spou²t¥ném p°i poru²ení podmínky∥

∥X2(n−1)Pn+1

∥ > ‖X1nPn+1‖ jak je uvedeno v Alg. 6, a díky tomu je moºné poslední zmín¥noupodmínku X1n /∈ |PnPn+1| vyne hat.Ani tento postup nemusí vºdy vést ke zdárnému nalezení trajektorie. P°íklad vstupní esty,se kterou si algoritmus neporadí, je uveden v následují í kapitole. K dal²ímu zvý²ení spolehlivostiby bylo nutné pouºít komplikovan¥j²í metodu, neº je odstra¬ování bod· esty, p°ípadn¥ stávají ímetodu doplnit o pomo né kroky v situa í h, kde se algoritmus nedokáºe vstupní esty drºet.15

Page 21: Technické - cvut.cz

Pn+2

Pn+1

Pn

X2n

X1nX2(n-1)

Cn

Cn-1

Cn-2

Pn-1 Pn-2

X1(n-1)

X2(n-2)

Pn-1

Cn-2

Pn

Pn+1

X1(n-2)

Cn-1

Pn-2

X1(n-1)

X2(n-1)

X2(n-2)

X1(n-2)Obrázek 12: Zjednodu²ení esty p°i hyb¥ ∣

∣X2(n−1)Pn+1

∣ < |X1nPn+1|Vstup: body esty P = P1, P2, ... , PNVýstup: °ídi í vstupy U = u1, u2, ... , u2N−3N := po£et bod· vstupní esty;n := 1;X20 := P1; // aby algoritmus fungoval i pro n = 1while n < N doVýpo£et pozi e bod· X1n, X2n, Cn, vzdálenosti x a úhlu δ;if n < (N − 1) thenif ∥

∥X2(n−1)Pn+1

∥ > ‖X1Pn+1‖ thenif n > 1 then n := n− 1 odstra¬ Pn+2;odstra¬ u2n−2 a u2n−3;N := N − 1;jdi na za£átek while;else if x > ‖Pn+1Pn+2‖ thenodstra¬ Pn+2;N := N − 1;jdi na za£átek while;elseu2n−1 :=

(

v = konst., K = 0, ∆t =|X2(n−1)X1n|

v

);u2n :=

(

v = konst., K = sign (δ) ·Kmax, ∆t = |δ|v·Kmax);

n := n+ 1;endelseu2n−1 :=

(

v = konst., K = 0, ∆t =|X2(n−1)PN |

v

);endend Algoritmus 6: Algoritmus výpo£tu trajektorie16

Page 22: Technické - cvut.cz

2

3

4

5

6

3 4 5 6 7

P1P2

P3

P4

P5

(a) 2

3

4

5

6

3 4 5 6 7

P1P2

P3

P4

P5

(b) 2

3

4

5

6

3 4 5 6 7

Nalezená trajektorieUpravená cestaVstupní cestaZměny řídicích vstupů

P1P2

P3

P4

P5

( )Obrázek 13: a) Výstup algoritmu p°i Kmax = 1. Ze vstupní esty byl ve druhém kroku odebrán bodP4. b) Výstup algoritmu p°i Kmax = 2. Ze vstupní esty byl ve t°etím kroku odebrán bod P4. )Výstup algoritmu p°i Kmax = 3. K°ivost uº je dost velká, aby trajektorie mohla sledovat zadanou estu.4.3 P°íklady nalezený h trajektoriíNejprve ov¥°íme p°edpokládané hování algoritmu nazna£ené na Obráz í h 11 a 12. V obou p°ípa-de h jde o stejný úsek esty, který ov²em algoritmus zpra oval se t°emi r·znými hodnotami Kmax.Startovní pozi e robotu je v bod¥ P1, íl je v bod¥ P5. P°i zadané k°ivosti Kmax = 1 (polom¥rzatá£ky r = 1) dojde ve druhém kroku výpo£tu trajektorie k odebrání bodu P4 (Obr. 13a). P°í£inaodebrání tohoto bodu je stejná jako na Obr. 11 bod X22 leºel mimo úse£ku P3P4, ke které m¥lazatá£ka robot dovést.Nastavením v¥t²í k°ivosti Kmax = 2 (polom¥r zatá£ky r = 1

2 ) dojde ke druhému zmín¥némup°ípadu (Obr. 13b). Ve druhém kroku si e leºí bod X22 na úse£ e P3P4, ale v následují ím krokuleºí za£átek zatá£ky X13 dál od vr holu zatá£ky P4 neº X22. Robot by tedy musel po dosaºení boduX22 za ouvat do bodu X13 a pak teprve provést dal²í zatá£ku. V této prá i se ale uvaºuje pouzedop°edný pohyb robotu, proto algoritmus estu zjednodu²í odebráním bodu P4.Zvý²íme-li zadanou k°ivost aº na Kmax = 3, oº znamená polom¥r zatá£ek r = 1

3 , prob¥hnealgoritmus hlad e bez zásah· do zadané esty (Obr. 13 ).Zajímav¥j²í neº modelové p°íklady je výstup algoritmu p°i pouºití del²í a sloºit¥j²í esty. Prodemonstra i je zde pouºita £ást esty nalezené metodou PRM. Cesty generované metodou PRM sevyzna£ují velkým mnoºstvím r·znob¥ºný h segment·, takºe jsou ideálním materiálem k testovánítohoto algoritmu.ím je men²í povolená k°ivost zatá£ky, tím je pro algoritmus náro£n¥j²í nalézt trajektorii a tímví se p·vodní zadaná esta musí zjednodu²it. I s pom¥rn¥ malou k°ivostí (vzhledem k £lenitostivstupní esty) Kmax = 1 byla nalezena trajektorie (Obr. 14a).Vliv povolené k°ivosti zatá£ek na výslednou trajektorii lze sledovat na Obráz í h 14b a 14 ,které ukazují trajektorie pro k°ivosti Kmax = 2 a Kmax = 3.17

Page 23: Technické - cvut.cz

-8

-7

-6

-5

-4

-3

-2

-1

0

4 5 6 7 8 9 10 11 12

start

cíl (a) -8

-7

-6

-5

-4

-3

-2

-1

0

4 5 6 7 8 9 10 11 12

start

cíl (b) -8

-7

-6

-5

-4

-3

-2

-1

0

4 5 6 7 8 9 10 11 12

Nalezená trajektorieUpravená cestaVstupní cestaZměny řídicích vstupů

start

cíl ( )Obrázek 14: Trajektorie nalezené podle esty získané metodou PRM: a) p°i Kmax = 1, b) p°iKmax = 2, ) p°i Kmax = 3.Problém m·ºe nastat, na p°íklad kdyº je hned na za£átku esty p°íli² ostrá zatá£ka a zjed-nodu²ování esty nevede ke zlep²ení. P°edpokladem je, ºe startovní úhel robotu je pevn¥ daný anelze ho zm¥nit. Nakone dojde ke stavu za hy eném na Obr. 15. ervené body ukazují, kde bymusela za£ínat a kon£it zatá£ka se zadanou k°ivostí, aby vyto£ila takový úhel. Do²lo tak k situ-a i X1 /∈ |PnPn+1| (stejn¥ jako v p°ed házejí í h kro í h), v dal²ím kroku algoritmus odstraní ílový bod P5 a nalezená trajektorie povede ze startovního bodu P1 do bodu P2, oº je samoz°ejm¥nedostate£né °e²ení.

18

Page 24: Technické - cvut.cz

2

3

4

5

6

3 4 5 6 7

Upravená cestaVstupní cestaZměny řídicích vstupů

P1

P2

P3

P4

P5

Obrázek 15: Selhání algoritmu. Hned první zatá£ka byla p°íli² ostrá a postupné zjednodu²ovánínakone vede k odstran¥ní v²e h bod· krom¥ P1 a P2. Na obrázku je stav p°ed odstran¥ním boduP5.

19

Page 25: Technické - cvut.cz

5 ízení forma e ar-like robot· pouºitím Re eding HorizonControlMetody °ízení forma e mobilní h robot· se lze podle p°ístupu d¥lit do t°í skupin. Virtuální struktury[18 pohlíºejí na forma i jako na jedno virtuální t¥leso. Plánování pohybu se provádí pro elouvirtuální strukturu, poté se pohyb rozd¥lí na pohyby jednotlivý h £len· forma e a nakone sevypo£ítají °ídi í vstupy, které tyto pohyby zajistí. Udrºení tvaru forma e zaji²´uje zp¥tná vazba svhodn¥ nastavenou váhou. Výhodou tohoto p°ístupu je dobrá predikovatelnost hování elé forma e,nevýhodou pak omezené moºnosti aplika e. V [18 a [3 je rozebráno pouºití této metody pro °ízeníforma e vesmírný h plavidel. Pro °ízení ar-like robot· tuto metodu nelze pouºít, protoºe ar-likeroboty nemohou na míst¥ zm¥nit sm¥r pohybu.Druhým moºným p°ístupem je pouºití behaviorální h te hnik [16, 18, které fungují díky sou-£innosti jednodu hý h vzor · hování jednotlivý h robot·, °ízení je tedy distribuované mezi £lenyforma e. Základními pudy robotu mohou být na p°íklad udrºení forma e, zamezení kolize s p°e-káºkou nebo jiným robotem a sm¥r pohybu odpovídají í sm¥ru pohybu ostatní h robot·, p°i£emº°ídi í vstupy jsou r·zn¥ váºené. Behaviorální te hniky jsou inspirovány p°írodními spole£enstvími,které fungují bez jakéhokoli entralizovaného °ízení. Distribuovanost umoº¬uje na p°íklad bez ob-tíºí m¥nit po£et £len· forma e, na druhou stranu lze obtíºn¥ nebo v·be p°edpovídat konkrétní hování forma e. Realiza e tohoto p°ístupu pro ar-like roboty by byla obtíºná vzhledem k jeji hkinemati kým omezením.V této prá i byl pro °ízení forma e ar-like robot· vybrán p°ístup Leader-Follower [5, 19, kde jejeden nebo ví e robot· ve funk i leadera a ostatní jsou followery. Follower má denovanou pozi i v·£isvému leaderovi a snaºí se kopírovat jeho trajektorii, takºe sta£í naplánovat pohyb jednoho robotua ostatní £lenové forma e jeho trajektorii p°evezmou. Ve funk i leadera m·ºe být ví neº jedenrobot, p°ípadn¥ mohou roboty vytvo°it °et¥zovou forma i, kde robot i sleduje trajektorii robotui−1. Prakti kou aplika í Leader-Follower p°ístupu m·ºe být na p°íklad skupina autonomní h pluh·shrabují í h sníh [11.Tato kapitola popisuje Leader-Follower metodu °ízení forma e ar-like robot· zaloºenou naprin ipu Re eding Horizon [21, která zaji²´uje jak optimaliza i trajektorie leadera, tak stabilituforma e a její d·leºitou vlastností je s hopnost reagovat na dynami ké zm¥ny v prost°edí.V následují í h odstav í h je terminologie pouºitá k popsání metody Re eding Horizon Controlp°evzatá z [21.5.1 Kinemati ký model a zápis °ídi í h vstup·Kinemati ký model ar-like robotu je popsán soustavou rovni (1). Kongura e robotu je dánavektorem ψ(t) = (x(t), y(t), θ(t)), ry hlost a k°ivost jsou °ídi ími vstupy u(t) = (v(t), K(t)). í-zení bude realizováno posloupností M konstantní h vstup·. Denujme vektor neklesají í h £as·T (t0, tM ) = (t0, t1, ... , tM ), který bude obsahovat £asy p°epínání mezi vstupy. Celá °ídi í sek-ven e potom bude U(t0, tM , T ) = u(t0; t1 − t0), ... , u(tM−1; tM − tM−1), p°i£emº kaºdý vstupu(tk−1; tk − tk−1), k ∈ 0, ... , M − 1, je konstantní a doba jeho trvání je tk − tk−1. Tato dobam·ºe být u r·zný h vstup· odli²ná.Pro zjednodu²ení zápisu zahrneme £as tM do vektoru T a poloºíme rovnosti U(t0) ≡ U(t0, tM , T )a T (t0) ≡ T (t0, tM ).Z integra e modelu (1) p°es interval [t0, tf ] p°i za hování konstatní h °ídi í h vstup· na kaºdémintervalu [tk, tk+1], kde tk, tk+1 ∈ T (t0) a k ∈ 0, 1, ... , M − 1 lze odvodit následují í model pro20

Page 26: Technické - cvut.cz

L

1 2 3

4 5 6

p

q

L

1

3

2

45

6

q

pObrázek 16: Forma e pomo í k°ivkový h sou°adni .p°e hodové body, ve který h se m¥ní °ídi í vstupy:x(k + 1) = x(k) +

1

K(k + 1)[sin (θ(k) +K(k + 1)v(k + 1)∆t(k + 1))− sin (θ(k))],ifK(k + 1) 6= 0;

x(k) + v(k + 1) cos (θ(k))∆t(k + 1),ifK(k + 1) = 0

y(k + 1) = y(k)− 1

K(k + 1)[cos (θ(k) +K(k + 1)v(k + 1)∆t(k + 1))− cos (θ(k))], (2)ifK(k + 1) 6= 0;

y(k) + v(k + 1) sin (θ(k))∆t(k + 1),ifK(k + 1) = 0

θ(k + 1) = θ(k) +K(k + 1)v(k + 1)∆t(k + 1),kde x(k), y(k) a θ(k) tvo°í kongura i robotu ψ(k) v p°e hodovém bod¥ s indexem k, v(k + 1) aK(k + 1) jsou °ídi í vstupy z vektoru U(k + 1) = u(tk, tk+1 − tk), které jsou pouºity v £ase tk podobu ∆t(k + 1) = tk+1 − tk.Tento zápis nám umoº¬uje úsporn¥ popsat trajektorii po£áte£ní kongura í robotu ψ(t0), mno-ºinou °ídi í h vektor· U a vektorem p°epína í h £as· T .ídi í vstupy jsou omezeny následují ími nerovni emi:

vmin ≤ v ≤ vmaxKmin ≤ K ≤ Kmax,kde vmin a vmax jsou minimální a maximální ry hlosti robotu, Kmin a Kmax jsou minimální amaximální k°ivosti zatá£ky.5.2 Deni e forma eAby bylo moºné m¥nit b¥hem plánování tvar forma e, je pot°eba pouºít vhodný typ sou°adni ,21

Page 27: Technické - cvut.cz

kterými je forma e denována. Hlavním kinemati kým omezením ar-like robotu je nemoºnostzm¥ny azimutu na míst¥, tvar forma e se tedy musí nutn¥ zm¥nit p°i kaºdé zatᣠe. Kv·li této pod-mín e nelze forma i vymezit jednodu²e relativními pozi emi follower· v·£i leaderovi v kartézský hsou°adni í h. Vyhovují í jsou sou°adni e k°ivkové denované následovn¥.Ne h´ ΨL(t) je kompletní trajektorie leadera. Tuto trajektrorii rozd¥líme na £ást ←−ΨL, kterouuº leader urazil, a £ást −→ΨL, po které se leader teprve bude pohybovat. Pouºité k°ivkové sou°adni ejsou ur£eny dv¥ma osami p a q. Osa p kopíruje trajektorii ΨL(t) a osa q je na ni kolmá. Na ose p jepozitivní sm¥r opa£ný ke sm¥ru pohybu leadera a na ose q je pozitivní sm¥r nalevo od p ve sm¥ru(dop°edného) pohybu.Pozi e i-tého followera je tak ur£ena stavem ψL(tpi(t)) a sou°adni í qi(tpi(t)). pi(t) je zpoºd¥nífollowera v·£i leaderovi, tedy vzdálenost na ose p, tpi(t) je £as, ve kterém se leader na házel práv¥ napozi i ur£ené posunem na ose p o pi(t). qi(tpi(t)) je posun na ose q, tedy vzdálenost mezi ψL(tpi(t))a ψi(t) ve sm¥ru kolmém k trajektorii ←−ΨL.P°evod z k°ivkový h sou°adni do kartézský h lze provést následovn¥:xi(t) = xL(tpi(t))− qi(tpi(t)) sin(θL(tpi(t)))

yi(t) = yL(tpi(t)) + qi(tpi(t)) cos(θL(tpi(t))) (3)θi(t) = θL(tpi(t)),kde ψL(tpi(t)) = (xL(tpi(t)), yL(tpi(t)), θL(tpi(t))) je stav leadera v £ase tpi(t).5.3 Formula e úlohy plánování pohybuPrimárním úkolem je °ídit forma i do ílové oblasti a minimalizovat p°itom dobu pohybu. Problémse d¥lí na hledání optimální trajektorie leadera a °ízení zbytku forma e tak, aby kopírovala tra-jektorii leadera. Cílová oblast je konvexní plo ha, pro kterou platí, ºe kongura e kaºdého robotu,který se v oblasti na hází, je volná. P°edpokládáme, ºe je ílová oblast dosaºitelná z po£áte£níkongura e leadera ψL(t0).5.4 Prin ip ustupují ího horizontuNejprve se °e²í optimaliza£ní úloha pro £asový interval [t0, tf ], p°i£emº tf − t0 je ²í°ka horizontu a

δt, 0 < δt < tf , je vzorkova í £as. Následn¥ se pouºijí nalezené °ídi í vstupy na intervalu [t0, t0+δt]a pro es se opakuje pro interval [t0 + δt, tf + δt]. Díky pr·b¥ºnému aplikování vypo£tený h vstup·m·ºe algoritmus reagovat na nov¥ detekované p°ekáºky a poskytovat zp¥tnou vazbu na od hylkyod vypo£ítané trajektorie.5.5 Implementa e pro °ízení forma eÚloha se skládá ze dvou podproblém·. Prvním je nalezení £asov¥ optimální a p°ípustné trajektorieleadera a druhým s hopnost reagovat na zm¥ny v prost°edí (nové nebo pohyblivé p°ekáºky). V tétoimplementa i je problém vy°e²en rozd¥lením optimalizované °ídi í sekven e leadera UL(k) na dva£asové intervaly, TN pro k = 1, ..., N a TM pro k = N + 1, ..., N +M.ídi í vstupy z intervalu TN zaji²´ují dynami ké zm¥ny trajetorie podle zm¥n prost°edí v blíz-kosti forma e, zp·sobené nov¥ detekovanými nebo pohyblivými p°ekáºkami. V tomto intervalu je22

Page 28: Technické - cvut.cz

pevn¥ daná velikost £asového kroku ∆t(k + 1) = t(k + 1) − t(k), která odpovídá £asovému krokuRe eding Horizon Control a tato £ást UL je p°ímo pouºita k °ízení robotu.Interval TM bere v potaz elou mapu a slouºí k £asové optimální naviga e leadera do íle. asovýkrok ∆t je v tomto intervalu prom¥nný. Díky tomu se po£et p°e hodový h bod· m·ºe m¥nit podlepot°eby, ve sloºit¥j²í h £áste h mapy se tak vytvo°í ví e bod·.Optimální plánování pohybu leadera forma e lze popsat jako minimaliza i objektivní funk eJL(·) s omezeními rovnosti hTN

(·), hTM(·) a omezeními nerovnosti gTN

(·), gTM(·), gSF

(·), gra,L(·):min JL(ΩL,Oobs)s.t. hTN

(k − 1) = 0, gTN(k) ≤ 0, ∀k ∈ 1, ..., N

hTM(k − 1) = 0, gTM

(k) ≤ 0, ∀k ∈ N + 1, ..., N +M (4)gSF

(ψL(N +M)) ≤ 0, gra,L(ΩL,Oobs) ≤ 0.P°i£emº objektivní funk e je:

JL(ΩL,Oobs) =N+M∑

k=N+1

∆t(k)

+ α

no∑

j=1

(min0, distj(ΩL,Oobs)− rs,Ldistj(ΩL,Oobs)− ra,L

)2 . (5)První °ádek vyjad°uje poºadavek minimaliza e doby pohybu, ve druhém °ádku je zahrnut vlivprost°edí. Funk e ve druhém °ádku závisí na vzdálenosti leadera od j-té p°ekáºky (distj(ΩL,Oobs))a nabývá hodnoty 0 pro distj(ΩL,Oobs) > rs,L, takºe p°ekáºky ve vzdálenosti v¥t²í neº rs,L nemajína objektivní funk i vliv. Vzdálenost rs,L tak lze ozna£it jako detek£ní radius. Nenulové hodnotynabývá funk e pro rs,L > distj(ΩL,Oobs) > ra,L a nekone£na pro distj(ΩL,Oobs) < ra,L, oº mázajistit, ºe výsledná trajektorie bude bezkolizní; ra,L je tedy minimální dovolená vzdálenost odp°ekáºky. Hodnota no v sum¥ vyjad°uje po£et p°ekáºek v map¥.Omezují í rovnosti hTN(k), ∀k ∈ 0, ..., N − 1 a hTM

(k), ∀k ∈ N, ..., N +M − 1 odpoví-dají kinemati kému modelu (1), omezují í nerovnosti gTN(k), ∀k ∈ 0, ..., N − 1 a gTM

(k), ∀k ∈N, ..., N +M − 1 vyjad°ují zejména omezení ry hlosti a k°ivosti nebo také podmínku kladného£asového kroku∆t ≥ 0. Omezení gra,L

(ΩL,Oobs) má tvar:r2a,L − distj(ΩL,Oobs)

2 ≤ 0, j = 1, ..., n0a zaji²´uje trajektorii bez kolizí. Omezení gSF(ψL(N+M)) zaji²´uje, ºe trajektorie povede do ílovéoblasti SF . Pokud je tato oblast kruhová s polom¥rem rSF

a se st°edem v CSF, bude mít omezenítvar:

‖pL(N +M)− CSF‖ − rSF

≤ 0,kde pL je pozi e leadera.Optimaliza£ní s héma pro i-tého followera lze zapsat takto:23

Page 29: Technické - cvut.cz

min Ji(Ωi,Oobs), i = 1, ..., nrs.t. hi(k − 1) = 0, gi(k) ≤ 0, ∀k ∈ 1, ..., N (6)gra(Ωi,Oobs) ≤ 0, gra,i

(Ωi, plast,j) ≤ 0P°i£emº objektivní funk e je:Ji(Ωi,Oobs) =

N∑

k=1

‖(pd,i(k)− pi(k))‖2

+ α

no∑

j=1

(min0, distj(Ωi,Oobs)− rsdistj(Ωi,Oobs)− ra

)2 , (7)+ β

j∈nn

(min0, di,j(Ωi,Ωj )− rs,i

di,j(Ωi,Ωj )− ra,i

)2

.První £ást funk e slouºí ke sledování trajektorie leadera a p°edstavuje vzdálenost naplánovanépozi e robotu pi(k) od p°edpokládané pozi e pd,i(k). Druhý °ádek odpovídá druhému °ádku veobjektivní funk i JL(·), u follower· v²ak mohou být pouºity men²í povolené vzdálenosti od p°ekáºek.Poslední °ádek zaji²´uje zabrán¥ní kolizí s po²kozenými roboty. Funk e di,j(Ωi,Ωj ) je minimálnívzdálenost mezi naplánovanou trajektorií ψi(t) a skute£n¥ provád¥ným plánem j-tého robotu ψj(t),

j ∈ nn, kde nn jsou indexy v²e h robot· forma e krom¥ i-tého.Omezení rovnosti a nerovnosti odpovídají t¥m v optimaliza£ním s hématu leadera. P°ibyloomezení nerovnosti gra,i(Ωi, plast,j), které zabra¬uje kolizi s jiným robotem:

r2a,i − di,j(Ωi, plast,j)2 ≤ 0, j ∈ nn.5.6 Optimaliza e trajektorieD·leºitou sou£ástí RHC je optimaliza e trajektorií jednotlivý h £len· forma e. V p°ípad¥ pouºitíkon eptu Leader-Follower se nejprve optimalizuje trajektorie leadera od jeho aktuální pozi e do íle. Optimaliza£ní s héma (4) je popsáno v p°ed hozí podkapitole, krom¥ doby pohybu obsahujeobjektivní funk e (5) i £len penalizují í p°íli² malou vzdálenost od p°ekáºek. Optimalizovaná tra-jektorie leadera je následn¥ p°edáne follower·m, jeji hº poºadované trajektorie se vypo£tou podletvaru forma e pouºitím soustavy rovni (3) . Trajektorie follower· je rovn¥º optimalizována podles hématu (6). V objektivní funk i (7) je v²ak místo doby pohybu penalizováno od hýlení od p·vodnítrajektorie získané od leadera a p°ibyl £len kontrolují í vzdálenost od ostatní h robot· forma e.Samotnou optimaliza i lze stru£n¥ popsat takto: vstupní trajektorie délky n je p°evedena naoptimaliza£ní vektor x = (v1, k1, t1, ..., vn, kn, tn), kde vi a ki jsou °ídi í vstupy robotu a ti jsou dobyjeji h trvání. Pokud je ini ializa£ní trajektorie p°ípustná, tedy jsou spln¥ny omezují í podmínky,je vypo£ten gradient objektivní funk e a vektor x je zm¥n¥n v p°íslu²ném sm¥ru. Jestliºe b¥hemn¥kolik po sob¥ jdou í h itera í h nedojde k nalezení lep²ího °e²ení, optimaliza e kon£í.Pro výpo£et objektivní funk e (5) je t°eba spo£ítat elkový £as trajektorie (sloºitost O(n)) ahlavn¥ vzdálenosti k p°ekáºkám. To m·ºe být výpo£etn¥ náro£né sloºitost aº O(nno), kde no24

Page 30: Technické - cvut.cz

je po£et p°ekáºek v map¥. Tento výpo£et lze ury hlit pouºitím hiear hi ký h datový h strukturpro uloºení objekt· v map¥, ov²em v solveru pouºitém v této prá i zatím tento postup není imple-mentován a výpo£et objektivní funk e tak m·ºe být zdlouhavý. Vzdálenosti k p°ekáºkám se naví po£ítají i p°i vyhodno ování omezují í h podmínek.Experimenty popsané v dal²í kapitole ukázaly, ºe optimaliza e trajektorií je £asov¥ velmi náro£náa kvalitní ini ializa£ní trajektorie m·ºe °e²ení úlohy kvadrati kého programování výrazn¥ ury hlit.

25

Page 31: Technické - cvut.cz

6 Výsledky experiment·Výpo£etn¥ nejnáro£n¥j²í £ástí systému pro plánování pohybu forma í mobilní h robot· je optimali-za e trajektorie, která je sou£ástí metody Re eding Horizon Control (Kapitola 5). V kaºdém krokuRHC se trajektorie optimalizuje z aktuální pozi e robotu aº do íle a zárove¬ se malý úsek tétotrajektorie okamºit¥ pouºije pro °ízení. Kdyº je tato £ást trajektorie roboty kompletn¥ vykonána,optimaliza£ní úloha je znovu spu²t¥na na zbytku trajektorie. Protoºe se trajektorie optimalizujevºdy z aktuální pozi e robotu, je zaji²t¥na zp¥tná vazba na p°ípadné od hylky robotu od p·vodnítrajektorie. Metoda RHC díky této opakované optimaliza i také dokáºe reagovat na zm¥ny v pro-st°edí, nap°. pohybují í se nebo nov¥ objevené p°ekáºky.K plynulému °ízení forma e je zapot°ebí ry hlá implementa e kvadrati kého solveru. V této ba-kalá°ské prá i je pouºit solver CFSQP [1, který lze získat zadarmo pro akademi ké ú£ely. KnihovnaCFSQP umoº¬uje krom¥ °e²ení úloh kvadrati kého programování i °e²ení úloh sekven£ního kvad-rati kého programování, a dokon e i obe nou nelineární optimaliza i. Pouºitá implementa e v²ak ztéto knihovny vyuºívá pouze SQP solver.Vzhledem ke sloºitosti optimaliza£ní úlohy m·ºe být doba optimaliza e p°íli² velká na to, abymohla být forma e °ízena v reálném £ase. Jedním ze zp·sob·, jak optimaliza i ury hlit, je pouºítvhodné po£áte£ní °e²ení, tedy trajektorii, která uº sama o sob¥ o nejlépe vyhovuje objektivnífunk i.V této prá i byly odzkou²eny t°i ini ializa£ní metody: VD (3.1), PRM (3.3), PRM smooth (3.5)a následují í experimenty ukazují, jak se li²í ry hlost optimaliza e, pokud je SQP solver ini ializovántrajektoriemi nalezenými t¥mito metodami.6.1 Nastavení experiment·Experimenty byly provedeny na t°e h mapá h (Obr. 17): a) empty, která krom¥ obdélníkovéhoohrani£ení neobsahuje ºádné p°ekáºky, b) potholes, která obsahuje mnoho konvexní h p°ekáºek rov-nom¥rn¥ rozmíst¥ný h v map¥, a ) test2, která obsahuje £ty°i p°ekáºky typu kvádr. Pro ury hlenívýpo£tu vzdálenosti trajektorií k p°ekáºkám jsou p°ekáºky reprezentovány jako boxy, oº umoº¬ujeanalyti ký výpo£et vzdálenosti segmentu k p°ekẠe. Tato reprezenta e je vhodná pro pravoúhléprost°edí, ale k vytvo°ení nepravoúhlý h p°ekáºek m·ºe být pot°eba je aproximovat ví e 2D boxy, oº zvy²uje výpo£etní náro£nost. Po£ty box· pouºitý h v jednotlivý h testovaný h mapá h jsouuvedeny v Tabul e 1. Z elkového po£tu tvo°í vºdy £ty°i boxy okraj mapy.V kaºdé map¥ bylo vygenerováno 20 náhodný h poloh robot·, mezi kterými byly následn¥hledány trajektorie ( elkem tedy 380 trajektorií). Sto hasti ké metody PRM a PRMs byly na kaºdétrajektorii spu²t¥ny 10Ö a výsledky t¥ hto b¥h· byly zpr·m¥rovány. Metoda VD je detereministi ká,proto byla pro kaºdou dvoji i poloh spu²t¥na jen jednou.M¥°ené hodnoty byly: délka ini ializa£ní trajektorie, délka optimalizované trajektorie, po£etsegment· ve vstupní trajektorii, doba optimaliza e a vzdálenost trajektorie k íli. Doba výpo£t·po£áte£ní h trajektorií není do výsledku zahrnuta, ov²em jedinou fází výpo£tu, která trvala déleneº jednu sekundu, bylo generování roadmapy v metod¥ PRM a PRMs. asy generování roadmapypro jednotlivé mapy jsou uvedeny v Tabul e 1.Výsledky jsou uvedeny gra ky s uºitím boxplotu, kde spodní úse£ka p°edstavuje minimum,horní úse£ka maximum a obdélník vymezuje oblast, ve které se na hází 90 % hodnot jeho spodnía horní hrana tedy p°edstavují 5% a 95% kvantil. Poloha medián je vyzna£ena vodorovnou £arou.26

Page 32: Technické - cvut.cz

-15

-10

-5

0

5

10

15

-15 -10 -5 0 5 10 15(a) empty -5

-4

-3

-2

-1

0

1

2

3

4

5

-5 -4 -3 -2 -1 0 1 2 3 4 5(b) potholes -6

-4

-2

0

2

4

6

-6 -4 -2 0 2 4 6( ) test2Obrázek 17: Mapy, na který h byly provedeny simula£ní experimenty.Mapa Po£et p°ekáºek Generování PRM roadmapyempty 4 1.22 spotholes 142 4.62 stest2 8 2.21 sTabulka 1: Po£et objekt· a trvání u£í í fáze PRM v pouºitý h mapá h.Vzhledem k tomu, ºe je pohyb robotu omezen maximální k°ivostí, m·ºe se stát, ºe metodyplánování est PRM, PRMs a VD naleznou takovou estu, podél které nelze vytvo°it trajektoriialgoritmem popsaným v 4. K n¥kolika takovým p°ípad·m do²lo pouze u metody VD. Dvoji e, prokteré trajektorie nebyla nalezena, byly vyjmuty z dal²ího porovnání, p°i kterém jsme pozorovali,jak ry hle SQP solver trajektorie optimalizuje a jak kvalitní ini ializa£ní trajektorie jsou.Experimenty byly spou²t¥ny na PC Intel Core2Duo 2.8 GHz s 4 GB RAM pod opera£ním sys-témem FreeBSD 8.2. Parametry SQP solveru byly nastaveny následovn¥: α = 1000, rs = 0, 25,ra =0, 2, K ∈ 〈−2; 2〉, v ∈ 〈0, 01; 1〉, £as hrany∈ 〈0, 01; 1〉.6.2 Výsledky na map¥ emptyNa prázdné map¥ empty bylo nejlep²í h výsledk· dosaºeno s ini ializa í algoritmem PRMs. Pom¥rdélky optimalizované a p·vodní trajektorie je znázorn¥n na Obr. 18 . Zde je vid¥t, ºe pro metoduPRMs se tato hodnota u v²e h trajektorií pohybuje okolo 100 %, oº znamená, ºe optimalizovanátrajektorie byla u v²e h testovaný h dvoji tém¥° totoºná se vstupní trajektorií. Naopak nejmar-kantn¥j²í ú£inek m¥la optimaliza e trajektorií nalezený h s pouºitím metody VD, kde u n¥který hdvoji solver zkrátil trajektorii tém¥° na polovinu p·vodní délky. U metody PRM byl tento pom¥rpr·m¥rn¥ a. 80 %.Délky optimalizovaný h trajektorií jsou d·leºitým m¥°ítkem jeji h kvality. Z Obrázku 18b lzevypozorovat, ºe metody PRM a PRMs poskytly p°ibliºn¥ stejné výsledky. Metoda VD je na tom vtomto kritériu o n¥ o h·°e.Dal²ími m¥°enými veli£inami byly po£et segment· ini ializa£ní trajektorie a doba optimaliza eSQP solverem. Tyto dv¥ hodnoty spolu souvisí, nebo´ velké mnoºství segment· zna£n¥ komplikuje azpomaluje optimaliza i. Korela i je moºné pozorovat na Obr. 19. Nejví e £asu zabrala optimaliza etrajektorií nalezený h metodou PRM, které obsahovaly mnoho segment· (pr·m¥rn¥ 78, u del²í htrajektorií aº 500). Doba u vzdálen¥j²í h dvoji spojovaný h bod· dosahovala aº 8 minut, oº je27

Page 33: Technické - cvut.cz

2

4

6

8

10

12

14

16

18

20

22

PRM PRMs VD

Initial lengths

(a) 2

4

6

8

10

12

14

16

PRM PRMs VD

Optimized lengths

(b) 50

55

60

65

70

75

80

85

90

95

100

105

PRM PRMs VD

Ratios

( )Obrázek 18: Výsledky experiment· na map¥ empty : a) délka ini ializa£ní trajektorie, b) délkaoptimalizované trajektrorie, ) pom¥r délky optimalizované a ini ializa£ní trajektorie.

0

20

40

60

80

100

120

PRM PRMs VD

Number of segments in paths

(a) 0

100

200

300

400

500

600

PRM PRMs VD

Solver times

(b)Obrázek 19: Výsledky experiment· na map¥ empty : a) po£et segment· ini ializa£ní trajektorie, b)doba optimaliza e. 28

Page 34: Technické - cvut.cz

pro reálné aplika e nevhodné. Optimaliza e trajektorií nalezený h metodou VD, sloºené v 90 %p°ípad· z 27 aº 66 segment·, v¥t²inou netrvaly déle neº minutu a nejry hleji byly optimalizoványtrajektorie získané metodou PRMs, u který h nejdel²í optimaliza e trvala 34 s, pr·m¥rná doba v²akbyla ne elý h 6 s. Po£et segment· vstupní trajektorie p°i pouºití PRM smooth se pohyboval mezi6 a 16, nebo´ v prázdné map¥ bez p°ekáºek lze estu snadno vyhladit.

-10

-5

0

5

10

-10 -5 0 5 10(a) -10

-5

0

5

10

-10 -5 0 5 10(b) -10

-5

0

5

10

-10 -5 0 5 10

Inicializační trajektorieNalezená cestaOptimalizovaná trajektorie

( )Obrázek 20: P°íklady nalezený h trajektorií na map¥ empty : a) PRM, b) PRMs. ) VD.6.3 Výsledky na map¥ potholesS výpo£ty trajektorií v map¥ potholes m¥l v n¥který h p°ípade h solver problémy, velké mnoºstvíp°ekáºek a nep°íli² volného prostoru mezi nimi totiº z°ejm¥ zvý²ilo náro£nost optimaliza e. Utrajektorií získaný h metodou PRM, kde naví optimaliza i komplikuje vysoký po£et segment·vstupní trajektorie, byl pom¥r délky p·vodní a optimalizované trajektorie pr·m¥rn¥ 97 %. O n¥ oniº²í byl tento pom¥r u metody PRMs, kde m¥la nejví e zkrá ená trajektorie po optimaliza i 74 %p·vodní délky. V¥t²ina trajektorií v²ak byla zkrá ena o mén¥ neº 10 %. P°i optimaliza i trajektoriínalezený h metodou VD do²lo v mnoha p°ípade h k neo£ekávanému hování solveru, který nejspí²zatím není p°ipraven hledat °e²ení v natolik komplikovaném prost°edí.Z graf· na Obrázku 21 lze usoudit, ºe optimaliza e na této map¥ byla velmi neú£inná a výslednádélka trajektorie záleºela výhradn¥ na pouºité metod¥ hledání esty. Nejkrat²í trajektorie op¥tposkytovala metoda PRMs.Jelikoº SQP solver v¥t²inou nedokázal najít o mo lep²í °e²ení, byla doba optimaliza e u metodPRM a PRMs velmi krátká (v¥t²inou do 5 sekund), u metody VD trvala optimaliza e aº dv¥ minuty,v extrémním p°ípad¥ dokon e p¥t minut. Graf je uveden na Obr. 22.Ukázky nalezený h trajektorií p°i ini ializa i ze v²e h t°í metod jsou na Obr. 23. Na sním í hje vid¥t, ºe optimalizovaná trajektorie tém¥° kopíruje p·vodní trajektorii.29

Page 35: Technické - cvut.cz

0

2

4

6

8

10

12

14

PRM PRMs VD

Initial lengths

(a) 0

2

4

6

8

10

12

14

16

18

PRM PRMs VD

Optimized lengths

(b) 0

50

100

150

200

250

300

350

400

PRM PRMs VD

Ratios

( )Obrázek 21: Výsledky experiment· na map¥ potholes : a) délka ini ializa£ní trajektorie, b) délkaoptimalizované trajektrorie, ) pom¥r délky optimalizované a ini ializa£ní trajektorie.

0

50

100

150

200

250

300

350

PRM PRMs VD

Number of segments in paths

(a) 0

50

100

150

200

250

300

350

PRM PRMs VD

Solver times

(b)Obrázek 22: Výsledky experiment· na map¥ potholes : a) po£et segment· ini ializa£ní trajektorie,b) doba optimaliza e. 30

Page 36: Technické - cvut.cz

-5

-4

-3

-2

-1

0

1

2

3

4

5

-5 -4 -3 -2 -1 0 1 2 3 4 5(a) -5

-4

-3

-2

-1

0

1

2

3

4

5

-5 -4 -3 -2 -1 0 1 2 3 4 5(b) -5

-4

-3

-2

-1

0

1

2

3

4

5

-5 -4 -3 -2 -1 0 1 2 3 4 5

Inicializační trajektorieNalezená cestaOptimalizovaná trajektorie

( )Obrázek 23: P°íklady nalezený h trajektorií na map¥ potholes : a) PRM, b) PRMs. ) VD.6.4 Výsledky na map¥ test2Poslední mapou, na které byly experimenty spou²t¥ny, byla jednodu há mapa test2 se £ty°mi p°e-káºkami. Na Obrázku 24 je vid¥t, ºe solver tentokrát ve v¥t²in¥ p°ípad· pra oval správn¥ a poda°ilose mu optimaliza í zmen²it rozdíly mezi trajektoriemi získanými r·znými metodami. Nejlep²í ini- ializa£ní trajektorie produkovala op¥t metoda PRMs. I po optimaliza i je na tom tato metodanejlépe, ov²em rozdíl mezi metodami (pokud odhlédneme od extrémní h hodnot) uº není tak velký.Kvalita optimalizovaný h trajektorií získaný h metodami PRM a VD ov²em vzrostla na úkorry hlosti optimaliza e. Zatím o u metody PRMs je horní kvantil doby optimaliza e 7,5 s, u PRMje to 521 s a u metody VD dokon e 1040 s. Nejdel²í optimaliza e trajektorie získané z VD trvalabezmála 37 minut. P°íslu²ný graf je na Obr. 25. Na Obrázku 26 jsou p°íklady trajektorií ini ializo-vaný h v této map¥ v²emi metodami.Na map¥ test2 bylo rovn¥º otestováno plánování pohybu forma e t°í ar-like robot·. Výsledekjedné z úsp¥²n¥ vy°e²ený h optimaliza í je na Obrázku 27. Ini ializa£ní trajektorie byla získánametodou PRMs, která se p°i p°ed hozí h experimente h nejlépe osv¥d£ila.6.5 Ov¥°ení nalezené trajektorie na systému SyRoTekSyrotek [15 je systém primárn¥ ur£en pro vzdálenou výuku mobilní robotiky, lze jej v²ak pouºít i provýzkumné ú£ely. Systém je tvo°en arénou o rozm¥re h 3,5Ö3,8m, ve které je umíst¥no 50 pevný h a37 posuvný h p°ekáºek. Posuvné p°ekáºky je moºné kongurovat softwarov¥, oº umoº¬uje jednakzvolit elou °adu kongura í p°ekáºek, jednak tím lze v arén¥ vytvo°it dynami ké prost°edí. Systémnabízí aº 12 mobilní h robot· vybavený h nej£ast¥ji pouºívanými senzory, jako jsou sonar, laserrange nder, kamera, kompas, ak elerometr a oor sensor. Opera£ní doba robotu je aº 8 hodin.Roboty jsou dobíjeny v dokova í h stani í h, do který h automati ky zajíºd¥jí po ukon£ení sezení.31

Page 37: Technické - cvut.cz

0

2

4

6

8

10

12

14

16

18

20

PRM PRMs VD

Initial lengths

(a) 0

2

4

6

8

10

12

14

16

18

20

PRM PRMs VD

Optimized lengths

(b) 40

60

80

100

120

140

160

180

PRM PRMs VD

Ratios

( )Obrázek 24: Výsledky experiment· na map¥ test2 : a) délka ini ializa£ní trajektorie, b) délka opti-malizované trajektrorie, ) pom¥r délky optimalizované a ini ializa£ní trajektorie.

0

50

100

150

200

250

PRM PRMs VD

Number of segments in paths

(a) 0

500

1000

1500

2000

2500

PRM PRMs VD

Solver times

(b)Obrázek 25: Výsledky experiment· na map¥ test2 : a) po£et segment· ini ializa£ní trajektorie, b)doba optimaliza e. 32

Page 38: Technické - cvut.cz

-6

-4

-2

0

2

4

6

-6 -4 -2 0 2 4 6(a) -6

-4

-2

0

2

4

6

-6 -4 -2 0 2 4 6(b) -6

-4

-2

0

2

4

6

-6 -4 -2 0 2 4 6

Inicializační trajektorieNalezená cestaOptimalizovaná trajektorie

( )Obrázek 26: P°íklady nalezený h trajektorií na map¥ test2 : a) PRM, b) PRMs. ) VD.

-6

-4

-2

0

2

4

6

-6 -4 -2 0 2 4 6

Inicializační trajektorieNalezená cestaOptimalizovaná trajektorieTrajektorie followerů

Obrázek 27: P°íklad optimalizované trajektorie forma e t°í ar-like robot· na map¥ test2. Proini ializa i byla pouºita metoda hledání esty PRMs.33

Page 39: Technické - cvut.cz

Init trajLeader

Followers

Obrázek 28: Vypo£ítané trajektorie forma e t°í ar-like robot· na map¥ Syrotku.Obrázek 29: Reálné roboty se pohybují po trajektorii získané metodou PRMs a optimalizovanouSQP solverem. Posuvná p°ekáºka uprost°ed by m¥la být vysunutá, jinak pohyb odpovídá napláno-vané trajektorii.Roboty lze ovládat p°es ²iro e pouºívané frameworky Player a ROS. Ty poskytují základníAPI k jednotlivým senzor·m robot· a dále pak nabízejí dal²í moduly, jakou je modul mapování£i lokaliza e. Systém je moºné ovládat na dálku: roboty lze °ídit p°es Player £i ROS s tím, ºeuºivatelský kód b¥ºí na dedikovaném serveru. Aktuální situa i v arén¥ lze sledovat online ze t°íkamer (jedna horní a dv¥ bo£ní). Z t¥ hto kamer je zárove¬ po°izován záznam, který je po skon£enísezení nabídnut uºivateli ke staºení. Aréna je v provozu nep°etrºit¥, b¥hem aktivního sezení jeosv¥tlena. Se systémem tak lze po p°ed hozí rezerva i pra ovat kdykoliv z pohodlí domova.V této prá i bylo k ovládání robot· pouºito Rozhraní Player. Jeho výhodou je, ºe je úz epropojeno s roboti ký h simulátorem Stage. ízení robot· bylo tedy nejprve vylad¥no práv¥ vtomto simulátoru, a poté p°eneseno na reálné roboty.Protoºe p°edm¥tem této bakalá°ské prá e nebylo vlastní propojení SQP solveru se systémemSyRoTek, nebyla forma e °ízena za b¥hu ve zp¥tnovazební smy£ e RHC, ale byly jí pouze p°edányp°edem vypo£ítané °ídi í vstupy. Nalezená trajektorie forma e je na Obr. 28 a snímky skute£ný hrobot· vykonávají í h tuto trajektorii v arén¥ Syrotku jsou na Obr. 29.

34

Page 40: Technické - cvut.cz

7 Záv¥rTato prá e pojednává o problému plánování pohybu forma e mobilní h robot· typu ar-like. Úlohabyla rozd¥lena na t°i podproblémy: 1) nalezení volné esty mezi zadanými kongura emi, 2) výpo£ettrajektorie, která nalezenou estu sleduje a °ídi í h vstup·, které zajistí pohyb robota po tétotrajektorii a 3) optimaliza e vypo£tené trajektorie a výpo£et trajektorií v²e h £len· forma e.Byla implementována geometri ká metoda hledání esty ve Voroného diagramu, která pouºíváhrany VD nalezené programem Vroni. Ke grafu je p°ipojena startovní a ílová kongura e a knalezení esty mezi nimi je pouºit Dijkstr·v algoritmus.Dal²í implementovanou metodou bylo PRM. Tato sto hasti ká metoda p°i u£í í fázi náhodn¥vzorkuje stavový prostor a vytvá°í tzv. roadmapu, oº je graf, jehoº hrany tvo°í úseky esty v bez-pe£né vzdálenosti od p°ekáºek. P°i dostate£ném po£tu vygenerovaný h uzl· tak vzniká sí´ est, kekteré se p°i query fázi p°ipojí startovní a ílová kongura e a op¥t se v grafu hledá Dijkstrovýmalgoritmem nejkrat²í esta. Protoºe jsou esty poskytované metodou PRM velmi klikaté, byl naim-plementován jednodu hý algoritmus, který odstraní redundantní body, a tím estu vyhladí. PRMs p°idaným vyhlazova ím algoritmem bylo s velmi dobrými výsledky testováno jako samostatnámetoda PRM smooth.Dále byl naimplementován path following algoritmus, tedy metoda, která estu nalezenou vp°ed hozím kroku aproximuje trajektorií ar-like robotu a vypo£ítá °ídi í vstupy k jejímu uskute£-n¥ní. Nalezení této trajektorie je °e²eno jako geometri ká úloha.Vektor °ídi í h vstup· je pak p°edán kvadrati kému solveru, který trajektorii optimalizuje podlenastavený h kritérií. Doba i kvalita optimaliza e siln¥ závisí na pouºité ini ializa£ní trajektorii.K °ízení forma e byl pouºit kon ept Leader-Follower. Díky tomu sta£í nalézt trajektorii leaderaforma e, ze které jsou následn¥ vypo£ítány trajektorie follower· podle vzájemné pozi e robot· veforma i.Samotné °ízení forma e obstarává metoda Re eding Horizon Control, která provádí optimaliza itrajektorie leadera od aktuální pozi e robotu do íle. ást nalezené trajektorie je okamºit¥ robotemvykonána a potom se elý pro es opakuje z nové pozi e robotu. Díky tomuto krokování umoº¬ujemetoda reagovat na zm¥ny v prost°edí nebo od hylku robotu od naplánované trajektorie. Obdobn¥metoda plánuje pohyb follower· a udrºuje tvar forma e.V¥t²ina experiment· se v¥novala ov¥°ení vlivu pouºité metody hledání esty na ry hlost optima-liza e a kvalitu výsledné trajektorie. Experimenty ukázaly, ºe vhodný tvar ini ializa£ní trajektoriep°edané solveru m·ºe velmi výrazn¥ zkrátit dobu optimaliza e a naopak ²patn¥ naplánovaná tra-jektorie vede ke zdlouhavému optimalizovaní a nakone i k hor²ímu výsledku.

35

Page 41: Technické - cvut.cz

Literatura[1 http://www.aemdesign. om/downloadfsqp.html. AEM Design, In .[2 A. Ballu hi, A. Bi hi, A. Balestrino, and G. Casalino. Path tra king ontrol for dubin's ars. In Roboti s and Automation, 1996. Pro eedings., 1996 IEEE International Conferen eon, volume 4, pages 31233128. IEEE, 1996.[3 Al W. Beard, Jonathan Lawton, Fred Y. Hadaegh, and Senior Member. A oordination ar hi-te ture for spa e raft formation ontrol. IEEE Transa tions on Control Systems Te hnology,9:777790, 2001.[4 V. Boor, M.H. Overmars, and A.F. van der Stappen. The gaussian sampling strategy forprobabilisti roadmap planners. In Roboti s and Automation, 1999. Pro eedings. 1999 IEEEInternational Conferen e on, volume 2, pages 10181023. Ieee, 1999.[5 Aveek K. Das, Rafael Fierro, Vijay Kumar, James P. Ostrowski, John Spletzer, and Camillo J.Taylor. A vision-based formation ontrol framework. IEEE TRANSACTIONS ON ROBOTICSAND AUTOMATION, 18(5):813825, 2002.[6 M. De Berg, O. Cheong, and M. Van Kreveld. Computational geometry: algorithms and appli- ations. Springer-Verlag New York In , 2008.[7 C.C. De Wit, AD NDoudi-Likoho, and A. Mi aelli. Feedba k ontrol for a train-like vehi le. InIEEE International Conferen e on Roboti s and Automation, pages 1414. INSTITUTE OFELECTRICAL ENGINEERS INC (IEEE), 1994.[8 M. Egerstedt, X. Hu, and A. Stotsky. Control of mobile platforms using a virtual vehi leapproa h. Automati Control, IEEE Transa tions on, 46(11):17771782, 2001.[9 R. Geraerts and M.H. Overmars. Creating high-quality paths for motion planning. TheInternational Journal of Roboti s Resear h, 26(8):845863, 2007.[10 M. Held. Vroni and ar vroni: Software for and appli ations of voronoi diagrams in s ien e andengineering. In Voronoi Diagrams in S ien e and Engineering (ISVD), 2011 Eighth Internati-onal Symposium on, pages 3 12, june 2011.[11 Martin Hess, Martin Saska, and Klaus S hilling. Appli ation of oordinated multi-vehi leformations for snow shoveling on airports. Intelligent Servi e Roboti s, 2:205217, 2009.10.1007/s11370-009-0048-5.[12 D. Hsu, T. Jiang, J. Reif, and Z. Sun. The bridge test for sampling narrow passages withprobabilisti roadmap planners. In Roboti s and Automation, 2003. Pro eedings. ICRA'03.IEEE International Conferen e on, volume 3, pages 44204426. IEEE, 2003.[13 G. Indiveri, A. Nu hter, and K. Lingemann. High speed dierential drive mobile robot pathfollowing ontrol with bounded wheel speed ommands. In Roboti s and Automation, 2007IEEE International Conferen e on, pages 22022207. IEEE, 2007.[14 L.E. Kavraki, P. Svestka, J.-C. Latombe, and M.H. Overmars. Probabilisti roadmaps for pathplanning in high-dimensional onguration spa es. Roboti s and Automation, IEEE Transa ti-ons on, 12(4):566 580, aug 1996. 36

Page 42: Technické - cvut.cz

[15 M. Kuli h, K. Ko²nar, J. Chudoba, O. Fi²er, and L. P°eu£il. User&#8217;s A ess to theRoboti E-Learning System SyRoTek. In Pro eedings of the 4th International Conferen e onComputer Supported Edu ation, volume 1, pages 206211, Porto, 2012. S iTePress S ien e andTe hnology Publi ations.[16 N.E. Leonard and E. Fiorelli. Virtual leaders, arti ial potentials and oordinated ontrol ofgroups. In De ision and Control, 2001. Pro eedings of the 40th IEEE Conferen e on, volume 3,pages 2968 2973 vol.3, 2001.[17 K. Ma ek, I. Petrovi , and R. Siegwart. A ontrol method for stable and smooth path followingof mobile robots. In Pro eedings of the European Conferen e on Mobile Robots, 2005.[18 Wei Ren and Randal W. Beard. Virtual stru ture based spa e raft formation ontrol withformation feedba k. In in AIAA Guidan e, Navigation, and Control Conferen e, (Monterey,CA), Ameri an Institute of Aeronauti s and Astronauti s, pages 20024963, 2002.[19 J. Rodrigues, D. Figueira, C. Neves, and M.I. Ribeiro. Leader-following graph-based distributedformation ontrol. In Pro . of Roboti a 2008-8th Conferen e on Autonomous Robot Systemsand Competitions, volume 95, 2008.[20 N. Sarkar, X. Yun, and V. Kumar. Dynami path following: A new ontrol algorithm formobile robots. In De ision and Control, 1993., Pro eedings of the 32nd IEEE Conferen e on,pages 26702675. IEEE, 1993.[21 M. Saska, JS Mejia, D.M. Stipanovi , and K. S hilling. Control and navigation of formati-ons of ar-like robots on a re eding horizon. In Control Appli ations,(CCA) & IntelligentControl,(ISIC), 2009 IEEE, pages 17611766. IEEE, 2009.

37