AMBER. Automated Mapping of robot EnviRonment. Berecz Szabolcs Kovács Róbert Kúthy Előd Zoltán

Átírás

1 AMBER Automated Mapping of robot EnviRonment Berecz Szabolcs Kovács Róbert Kúthy Előd Zoltán Budapesti Műszaki Főiskola Neumann János Informatikai Főiskolai Kar Intelligens Automatizált Rendszerek szakirány Konzulens: Vámossy Zoltán, főiskolai docens november 8.

2 Tartalomjegyzék 1. A projekt célja 5 2. Irodalomkutatás Képfeldolgozás, modellalkotás (Berecz Szabolcs, Kúthy Előd Zoltán) Képjellemzők kiemelése A SUSAN módszer A Monogenic signal alapú módszer Felület-regisztráció A spin-image módszer Az Iterative Closest Point algoritmus A spin-image módszerre specializált ICP algoritmus Legközelebbi pont meghatározása Abszolút orientáció meghatározása Normális eloszlású zaj Hibás pontpárosítások Navigáció (Kovács Róbert) A közelítő cellákra bontás módszerén alapuló útkeresési technikák A quadtree algoritmus A D* keresés A parti-game algoritmus Egyéb módszerek Rendszerterv A projekttagok feladatai

3 3.2. Képfeldolgozó modul (Kúthy Előd Zoltán) Modellkezelő modul (Berecz Szabolcs) Pozíció meghatározás Modellépítés Hibás adatok kezelése Mozgás érzékelése Irányítás modul (Kovács Róbert) A robot irányítása A generált modell hiányosságainak meghatározása A következő elérendő pozíció meghatározása Út generálása a választott pozícióhoz A hordozójármű vezérlése A robot mozgásának kalibrálása Tesztelés Megvalósítás Képfeldolgozó modul (Kúthy Előd Zoltán) Kamerakezelés Kamerakalibráció A megfelelő pontok kiválasztása Pontpárok képzése A mélység meghatározása Modellkezelő modul (Berecz Szabolcs) Modellek ábrázolása A mesh osztály A vertex és face osztályok A breadth_first_vertex_iterator osztály A marker és markable osztályok A indexed_v3d osztály A spin_stack osztály Teszteredmények A transzformációk ábrázolása Irányítás modul (Kovács Róbert) A modell térképpé konvertálása

4 Az út generálása A parti-game algoritmus Inicializáció A fő eljárás A cellák céltól való távolságának meghatározása Tesztek A generált út átalakítása Eredmények, célkitűzések 48 3

5 Ábrák jegyzéke 2.1. Egy spin-image által hordozott információ Kd-tree felosztás két dimenzió esetén Párhuzamos optikai tengelyek Összetartó optikai tengelyek Föltérképezett terület A kamerakalibráció eredményeként kapott görbék Token-detektálás különböző paraméter értékek esetén A pontpárok ábrázolása a két kép átlagán Megtalált pontpárok összekötve A szoba modellje Kacsa modell, egy és két százalék zaj Szoba modell, egy és két százalék zaj Várhatóan a valódihoz hasonló térkép Nagy bonyolultságú térkép Szűk folyosókat tartalmazó térkép Vontató és vontatandó összekapcsolása Átalakított út Az algoritmus hibájából adódó ütközések

6 1. fejezet A projekt célja A projekt célja olyan rendszer megtervezése és elkészítése, mely két kamerából nyert információ alapján a környezetéről egy háromdimenziós modellt készít. Mivel ennek felépítéséhez szükséges, hogy a kamera több helyről legyen képes felvételeket készíteni, a rendszernek rendelkeznie kell egy minél több szabadsági fokú hordozóeszközzel, melyet irányítania kell. A feladat megoldása három fő részből áll, a kamerák által közvetített képekből a környezeti információk kinyerése, ezek alapján a környezet modelljének megalkotása, majd a modell alapján a robot irányítása. 5

7 2. fejezet Irodalomkutatás 2.1. Képfeldolgozás, modellalkotás (Berecz Szabolcs, Kúthy Előd Zoltán) Mivel a következőkben felvázolt terület nem bontható két közel azonos nehézségű részre, úgy döntöttünk, hogy egymást kiegészítve ismerkedünk meg a mások által használt algoritmusokkal, és közösen írjuk meg az általunk használható algoritmusok áttekintését. Megismerkedtünk az alapvető képjellemzők kiemelésének tipikus módszereivel (élkiemelés), mélységinformációk kinyerésével két kép különbségeinek alapján, illetve két ponthalmaz összeillesztésével[3]. Az élkiemelő algoritmusok három osztályra bonthatóak: a gradiens és Laplace operátort közelítő módszerek, a template matching módszert alkalmazó algoritmusok, és a parametrikus élmodellekkel működő algoritmusok. A gradiens operátort közelítő algoritmusok (Robert s cross operator, Söbel, Prewitt, Kirsch, Robinson) a differenciáló természetük miatt a zajra nagyon érzékenyek, ami fokozottan jellemző a Laplace operátort alkalmazó algoritmusokra, ráadásul az előző módszerekkel ellentétben az intenzitásváltozás irányát sem adják meg. A template matching módszert alkalmazó algoritmusok csak előre meghatározott struktúrájú élek nagy biztonságú meghatározására alkalmasak. A parametrikus modelleket használó algoritmusok részletesebb élstruktúrák meghatározására alkalmasak, de számításigénye az előzőekben felvázolt módszereknél jelentősen nagyobb. Mélységinformációk kinyerésére két módszert ismertet Ballard és Brown[3], melyek a terület alapú, és a képjellemzőket felhasználó eljárások. A terület alapú módszerek alapfeltevése az, hogy a két kép közötti különbség minimális ezért csak kis kamera bázistávolság esetén alkalmazható, és a hasonló területek képen elfoglalt helyzete alapján számítja a távolságot. Ezen algoritmusok egy kissé 6

8 pontatlan, sűrű mélységtérképet eredményeznek, melyek további feldolgozást igényelnek a számunkra hasznos információk kinyeréséhez (egy mesh felépítéséhez). A képjellemzők alapján működő algoritmusok ritka mélységtérképet állítanak elő, melyek lokálisan pontosabbak, és globálisan megbízhatóbbak, mint a terület alapú módszerek eredménye, valamint a már meghatározott képjellemzők felhasználhatóak temporal matching eljárások bemeneteként, melyek tovább pontosíthatják a pontok helyzetének meghatározását. Megoldásként a két ponthalmaz összeillesztésének problémájára a gráf izomorfizmusok meghatározását mutatja be. Ezen gráf izomorfizmusok meghatározása az NP-teljes problémák csoportjába tartozik, ezért általunk nem hasznosítható. A további irodalomkutatás során újabb algoritmusokat ismertünk meg, melyek közül az általunk használhatóak áttekintése következik Képjellemzők kiemelése A következő algoritmusok közös jellemzője, hogy nem csak éldetektálást végeznek, hanem a kép néhány jellegzetes pontjának (pl. élcsomópontok) kiemelésére is használhatóak, melyekkel egyszerűbben megoldható a képjellemzők párosítása a mélységinformációk kinyerése érdekében A SUSAN módszer Ez az algoritmus[20] integráló jellegű, ezért kevésbé érzékeny a zajra, valamint pontosabban meghatározza a csomópontok helyzetét, mint a gradiens alapú módszerek. A módszer elméletileg is jól alátámasztott, és csak egy empirikus paramétert használ, amely a zajszűrés mértékét határozza meg A Monogenic signal alapú módszer Az egydimenziós jelfeldolgozásban jól ismert módszer az analitikus jel, mely komplex számsíkon ábrázolja az adott jelet. Alapvető tulajdonsága a jellemzők felosztása, polár koordinátarendszerben a komplex jel kitevője és alapja szerint különíthetőek el a jel jellemzői. A kitevő a jel lokális kvantitatív jellemzőit írja le, melyre a lokális amplitúdó elnevezés használatos. Az alap pedig a jel lokális kvalitatív jellemzőit írja le, amit lokális fázisnak hívunk. A lokális amplitúdó és fázis tulajdonsága az invariancia és az ekvivariancia, azaz a lokális amplitúdó független a lokális struktúrára, de leírja a lokális energiát. A lokális fázisra pedig pont a fordítottja igaz. Az analitikus jel a komplex számsíkon alkalmazott Hilbert transzformáció segítségével állítható elő. 7

9 Sajnos ez a leírás közvetlenül nem alkalmazható kétdimenziós jelekre, mivel nem rendelkezik kellő szabadsági fokkal, vagyis a módszer kiterjesztésekor a jel elveszti az invariancia és ekvivalencia tulajdonságait, így egy rendszeres hiba keletkezik, mely a jel irányától függ. Ezen probléma feloldására Felsberg egy harmadik komponens bevezetését javasolta, a lokális orientációt, mely a jel geometriai jellemzőit írja le. Az analitikus jel ezen kiterjesztését nevezzük monogenikus jelnek. Ezen jel előállításához a kvaterniókat alkalmazó Reisz transzformáció használatos. Mivel ez a módszer a kép jellegzetes pontjait számos paraméterrel jellemzi (irány, fázis, két meghatározó szín, energia), melyek nagymértékben pontosítják két jellegzetes pont párosítását a képeken[6][11] Felület-regisztráció A pontok kameráktól mért távolságának meghatározása után következő lépés a már meglévő modellhez viszonyított kamerahelyzet meghatározása. Ennek megvalósítására két általunk használható algoritmust találtunk, mindkettő magába foglalja az abszolút orientáció, valamint a legközelebbi pont meghatározásának problémáját A spin-image módszer Az algoritmus[10] azokban az esetekben alkalmazható, amikor a regisztrálandó ponthalmaz minden pontjához orientáció számítható. Egy felület egyértelműen kielégíti ezt a feltételt, ugyanis a felület egy adott pontjához tartozó normálvektor amennyiben úgy határozzuk meg, hogy a pontból a kamera felé mutató vektorral 0 α 90 szöget zár be egy egyértelmű irányt határoz meg. A regisztráció elvégzéséhez létre kell hoznunk a modell spin-image reprezentációját, ami a felület minden pontjához egy kétdimenziós tömböt (képet) rendel, mely megmutatja, hogy az adott pont és vektor által meghatározott egyenestől (x koordináta) és síktól (y koordináta) milyen távolságra vannak a felület pontjai (2.1. ábra). Ez a reprezentáció független az eltolástól és forgatástól, így a regisztrációhoz nincs szükség egy kezdeti, kellően pontos transzformációra. A modell spin-image-einek létrehozása után a regisztrálandó felületből véletlenszerűen kiválasztott néhány pontnak is elkészítjük a spin-image reprezentációját, majd ezek és a modell spin-image-einek korrelációját kiszámítjuk. A hasonló spin-image-ek alapján létrehozunk pont-megfeleltetéseket a modell és a regisztrálandó ponthalmaz között. Mivel hibás megfeleltetéseket is kaphatunk, érdemes csoportosítanunk: egy csoportba egymástól távol eső pont-megfeleltetéseket teszünk. A kapott csoportok mindegyike alapján meghatározunk egy transzformációt (lásd pont), 8

10 ezt pontosítjuk a spin-image módszerre specializált ICP algoritmussal ( pont), majd kiválasztjuk azt, amellyel elvégezve a transzformációt a legtöbb pontegyezést találjuk a két ponthalmaz között (másképpen: hány olyan pontpárt találunk, melyek távolsága egy bizonyos értéknél kisebb). Azzal, hogy távoli pontokat helyezünk egy csoportba azt is biztosítjuk, hogy a pontok pozíciójában jelen lévő zajnak kis hatása legyen a kiszámított transzformációra (annál nagyobb a transzformáció hibája, minél nagyobb a hibák összegének és a pontok távolságának hányadosa) ábra. Egy spin-image által hordozott információ (forrás: Johnson[10]) Az Iterative Closest Point algoritmus Az ICP algoritmus[4] iterált jellegű, és három alapvető lépésből áll: 1. Megkeressük a regisztrálandó ponthalmaz egyes pontjaihoz legközelebb eső pontokat a modell ponthalmazban (ld pont). 2. Transzformáció meghatározása, amely minimalizálja a pontpárok közötti távolság négyzetét (ld pont). 3. A regisztrálandó ponthalmazon elvégezzük az előző lépésben meghatározott transzformációt. A módszer leggyengébb pontja az első lépés, melynek célja az, hogy a regisztrálandó és a modell ponthalmaz pontjai között létrehozzon egy megfeleltetést. Ez a megfeleltetés csak abban az esetben lesz kielégítően jó, ha ismerünk egy megközelítőleg helyes kiindulási transzformációt. Ha ez a transzformáció nagy eltérést 9

11 mutat a tényleges transzformációhoz képest, akkor nagy valószínűséggel lokális minimumhoz közelít az algoritmus, ezért néhány iteráció után mikor már csak kismértékben változik a második lépés során kiszámított transzformáció érdemes ellenőriznünk, hogy megfelelő-e a kapott transzformáció: összeszámoljuk, hogy a regisztrálandó ponthalmazban lévő pontok hány százalékához találunk egy előre meghatározott távolságnál közelebb lévő pontot a modell ponthalmazban. Ha ez az érték kisebb, mint egy előre meghatározott küszöb, akkor a transzformációt hibásnak tekintjük. Ha a pontokhoz a pozíción kívül más tulajdonságokat is rendelhetünk (pl. felületi normálvektor, szín), akkor ezt is felhasználhatjuk az első lépésben, így pontosabb összerendeléseket kaphatunk. Ebben az esetben több mint három dimenziót kell használnunk a legközelebb eső pont meghatározásakor: minden tulajdonsághoz egy-egy dimenzió például pozíció és normálvektor esetén 3+3 dimenzió. Ha különböző mértékű maximális eltérést engedélyezünk az egyes tulajdonságokra, akkor a távolságszámítás során ennek megfelelően súlyoznunk kell az összetevőket. Az iteráció leállási feltételét meghatározhatjuk az iterációk maximális számával, a megengedett legnagyobb hibával, vagy a konvergencia sebességével A spin-image módszerre specializált ICP algoritmus Az egyetlen különbség az általános ICP-hez képest a pontpárok meghatározásában van. Mivel a spin-image algoritmus már meghatározott néhány pontpárt, így érdemes ezekből kiindulni: az egyes pontpároktól elindulva, azoktól egyre távolodva végighaladunk a regisztrálandó felület egyes pontjain, és megvizsgáljuk, hogy találunk-e ehhez egy előre meghatározott távolságnál közelebb lévő pontpárt a modell ponthalmazban. Ha találtunk, akkor feljegyezzük ezt a pontpárt, és továbbhaladunk a felületen. Ha nem találtunk, akkor ezt az irányt zsákutcának tekintve visszafordulunk. Ezzel a módszerrel a kiindulási pontpárok környezetében terjesztjük szét a pont-megfeleltetéseket, ezáltal kiszűrve a hibás pontpárokat, melyeket egy egyszerű, minden ponton végighaladó kereséssel találtunk volna Legközelebbi pont meghatározása A triviális megoldásnál (minden pontra meghatározzuk az adott koordinátától számított távolságot) hatékonyabban határozhatjuk meg a legközelebbi pontot a Kd-tree adatstruktúrával. A Kd-tree a Binary Search Tree általánosításaként is felfogható, ami a BST egy kulcsával szemben k darab kulcs alapján keres (k a dimenziók száma). Az adatstruktúra a ponthalmazt a fa egyes szintjein egy-egy 10

12 2.2. ábra. Kd-tree felosztás két dimenzió esetén dimenzió alapján osztja két részre (2.2. ábra). Annak meghatározására, hogy az egyes szinteken melyik dimenziót használjuk több módszert is alkalmaznak: A legegyszerűbb megoldás, ha az egyes dimenziókat sorrendben választjuk (a fa i-edik szintjén a (i mod k)-adik dimenziót) Optimálisabb struktúrát eredményez, ha a ponthalmaz vizsgálata alapján határozzuk meg a szétválasztó dimenziót. Egy ilyen struktúra felépítése számításigényesebb, és pontok módosításakor/törlésekor is érdemes figyelembe venni az optimalitást, ezert csak kismértékben változó adatok esetén érdemes használnunk. A szétválasztás pozíciójának meghatározására valószínűleg az a legjobb módszer, ha a ponthalmaz adott dimenzió szerinti mediánjának koordinátáját választjuk, így kismértékben változó ponthalmaz esetén gyakorlatilag O(log n) idő alatt kereshetünk a struktúrában. Gyorsan változó ponthalmaz esetén annak érdekében, hogy hasonló hatékonysággal kereshessünk, folyamatosan az optimalitás közelében kell tartanunk a struktúrát. Ezt elérhetjük úgy, hogy a módosítások során optimális állapotban tartjuk a struktúrát, vagy időnként egy teljes optimalizációt hajtunk végre. A Kd-tree adatstruktúra segítségével viszonylag gyorsan kaphatjuk meg egy adott koordinátától adott távolságon belül lévő pontokat. Ezt használhatjuk fel a legközelebbi pont meghatározásához: egy az alkalmazástól függő maximális távolságot meghatározva keresünk a struktúrában, majd az így kiválasztott pontok közül lineáris kereséssel választjuk ki a legközelebbi pontot. Ezzel a módszerrel kezelhetjük a pontatlan adatokat is, ebben az esetben a várható maximális bizonytalanság kétszeresével meg kell növelnünk a megengedett maximális távolságot, majd egy utólagos szűrés során eldobjuk a keresett tartományon kívül eső pontokat. 11

13 Abszolút orientáció meghatározása Az abszolút orientáció probléma megoldásán két ponthalmaz közötti transzformáció meghatározását értjük. A keresett transzformáció meghatározására két megközelítés tűnik megfelelőnek: a Horn[8][9] által kidolgozott módszer normális eloszlású zaj esetén ad optimális eredményt; Micheals[15] módszere pedig abban az esetben alkalmazható eredményesen, ha a ponthalmazok közötti megfeleltetések nem tökéletesek Normális eloszlású zaj [8][9]. Horn a megoldás levezetésekor abból indult ki, hogy a legmegfelelőbb eredményt a n w i r r,i sm(θ,u)r l,i r 0 2 i=1 minimalizálásával kapjuk (ahol n a pontok száma; w i az i-edik pont-pozíció pontosságának mérőszáma; r l,i a bal, r r,i a jobb ponthalmaz i-edik pontjának a ponthalmaz tömegközéppontjához viszonyított helyzete; s a skálázás mértéke; M(θ, u) az u vektor által meghatározott tengely körüli θ mértékű forgatást leíró mátrix). A keresett értékek: s, θ, u, r 0. Ebből a kifejezésből látszik, hogy minden pontot figyelembe vesz esetleg különböző súllyal a transzformáció kiszámításához, így nem kezeli a hibás pontpárosításokat. Horn levezetéséből kiderül, hogy a legmegfelelőbb eltolást a r 0 = r r sm(θ,u)r l képlettel kaphatjuk meg (r l, r r a bal, illetve a jobb ponthalmaz tömegközéppontja), tehát egyszerűen kiszámítható, ha már ismerjük a szükséges forgatást valamint skálázást. Mivel esetünkben nincs szükség skálázásra, ezt konstans egynek véve már csak a forgatás meghatározása van hátra. Ehhez a pontok koordinátáit felhasználva egy 4 4-es mátrixot építünk, melynek segítségével megkaphatjuk a keresett forgatást. A mátrix felépítéséhez először kiszámítjuk az S xx, S xy, S xz, S yx, S yy, S yz, S zx, S zy, S zz értékeket, ahol n S xx = w i x l,i x r,i n S xy = w i x l,i y r,i... i=1 i=1 (x l,i és y r,i a megfelelő pontok x illetve y koordinátái), majd ezen értékek összegeiből és különbségeiből felépítjük a 4 4-es mátrixot: S xx + S yy + S zz S yz S zy S zx S xz S xy S yx S N = yz S zy S xx S yy S zz S xy + S yx S zx + S xz S zx S xz S xy + S yx S xx + S yy S zz S yz + S zy S xy S yx S zx + S xz S yz + S zy S xx S yy + S zz Az N mátrix legpozitívabb sajátértékéhez tartozó sajátvektorát kvaternióként értelmezve megkapjuk a keresett forgatást. 12

14 Hibás pontpárosítások[15]. Micheals módszere egy olyan Hornétól különböző zárt formájú megoldáson alapszik, mely 4 pontpár alapján számítja ki a szükséges transzformációt. A megoldás azt aknázza ki, hogy a forgatást reprezentáló kvaternió komponenseinek páronkénti szorzatát kifejezhetjük a pontok lineáris kombinációival, tehát a megoldáshoz egy lineáris egyenletrendszert kell megoldanunk (a konkrét egyenletekre a méretei miatt itt nem térek ki). A megoldás során a q sx, q sy, q sz, q xy, q xz, q yz, q s 2, q x 2 q y 2, q z 2 szorzatok értékeit számítjuk ki (q sx = q s q x, q x 2 = q x q x,... ). Micheals megfigyelései szerint a komponensek négyzete kevésbé érzékeny a zajra, mint a többi komponens-szorzat. A számítások rövidebb idő alatt elvégezhetők mint a sajátérték/sajátvektor számítás, viszont csak 4 pontpár közötti transzformáció meghatározására használható. Ha több pont áll rendelkezésre, alkalmazhatunk egy RANSAC[7]-hoz hasonló megközelítést: a ponthalmaz pontjait sorrendben bejárva az egymás utáni pont-négyesekre elvégezzük a számítást, majd a részeredményeket azok jósága szerint súlyozva átlagoljuk. Az egyes részeredmények súlyának számításához felhasználhatjuk az egyes pontok, és ezek transzformált párjainak páronkénti távolságának négyzetösszegét (d érték), valamint a különböző zajérzékenység miatt a kvaternió-komponensek szorzatait: p = q s 2q x 2 q 2 sx + q s 2q y 2 q 2 sy + q s 2q z 2 q 2 sz + q x 2q y 2 q 2 xy + q x 2q z 2 q 2 xz + q y 2q z 2 q 2 yz (zajt tartalmazó adatok esetén a q s 2q x 2 = q 2 sx egyenlőség a zajt nem tartalmazó esettel ellentétben nem teljesül). Nagy d és p érték esetén a részeredmény a súlyozás miatt csak kismértékben esetleg egyáltalán nem változtat a végeredményen Navigáció (Kovács Róbert) A különleges esetektől eltekintve a robot környezetének teljes föltérképezéséhez szükséges a kamera célzott mozgatása, ezt hardveres megvalósítás esetén egy modellautó segítségével tervezzük megoldani. Szükséges, hogy a robot meg tudja határozni a jelenlegi helyzetét, esetleges elmozdulását az előző helyzetéhez képest, az elérendő pozíció(ka)t és a célhoz vezető utat. A célhoz vezető út meghatározására két, ugyanazon alaptechnikán alapuló módszert találtam megfontolásra méltónak: az egyik az un. pontos cellákra bontás (exact cell decomposition), a másik a közelítő cellákra bontás (approximate cell decomposition)[12]. Mindkét módszer lényege, hogy a robot környezetét egymást nem fedő régiókra cellákra bontjuk, majd a szerint, hogy a cella szabad teret 13

15 vagy tereptárgyat tartalmaz, osztályozzuk azokat. A különbség a két módszer között, hogy az első esetben a cellák formája nem meghatározott, így lehetőség van olyan felbontásra, hogy kizárólag kétféle cella képződjön: olyan, amely területén nincs tereptárgy és olyan, amelynek területe csak tereptárgyat tartalmaz (olyan sokszögeket képezünk, amelyek határai az egyes tereptárgyak csúcsaihoz és éleihez igazodnak). Második esetben előre meghatározott méretű és formájú cellákra bontjuk a környezetet, így általában létrejön egy vegyes cellatípus is, amelynek területén szabad tér és tereptárgy is található. Ezek után létrehozzuk a kapcsolat gráfot (connectivity graph), mely az egyes cellák közötti szomszédságot jelöli oly módon, hogy a cellákat a gráf csúcsai jelölik és két csúcs között akkor és csak akkor van él, ha a megfelelő két cella szomszédos egymással, vagyis a celláknak van közös határolószakaszuk. Következő lépésként megpróbálunk e gráfban valamilyen módszerrel egy utat keresni a kiindulási és a célcella között, vagy csak üres, vagy üres és vegyes cellákon végighaladva. A pontos cellákra bontás előnye, hogy a gráf felépítése után egyszerű gráf kereséssé válik a probléma, hátránya viszont, hogy a gráf elkészítésének bonyolultsága több nagyságrendet változhat a környezet komplexitásától függően. A közelítő cellákra bontás előnye, hogy az állapotteret egyszerű módszerrel, hatékonyan lehet fölbontani cellákra, az elkészített gráf mérete könnyen becsülhető, az előző megközelítéshez képest ezen algoritmus általában gyorsabb, egyetlen hátránya a vegyes cellák okozta esetleges problémák például változó cellamérettel orvosolható A közelítő cellákra bontás módszerén alapuló útkeresési technikák Ezeknél a módszereknél lényeges, hogy mekkora cellákkal dolgozunk: ha túl nagyok, akkor sok utat hagyunk figyelmen kívül, túl kicsi esetén pedig nagyon számításigényessé válik az eljárás. Általánosan alkalmazott megoldás a problémára, hogy kiválasztunk egy kiindulási cellaméretet, felépítjük az ennek megfelelő kapcsolódási gráfot, és ha ebben a gráfban nem sikerült utat találni, akkor a valamilyen heurisztika szerint kiválasztott cellákat felosztjuk kisebb részekre, beillesztjük a gráfba és az új gráfban is megpróbálunk utat találni. Az eljárást addig folytatjuk, amíg el nem érjük a célt vagy valamilyen megállási feltételt. Többféle eljárás alapul a fenti alapelven, ebből kettőt találtam megfontolásra méltónak, mivel mindkettő alkalmazható dinamikusan változó térképekre is amely a mi esetünkben alapfeltétel: A quadtree fölbontáson (quadtree decomposition) alapuló D* keresés[21] és a parti-game algoritmus[16][17]. 14

16 A quadtree algoritmus A quadtree algoritmus a robot környezetét háromféle cellára bontja a szerint, hogy a cellák mekkora részét foglalja el valamilyen tereptárgy: egy cella üres, ha egyáltalán nincs benne tereptárgy, vegyes, ha egy részében van, és teli, ha a cella egy tárgy belsejében fekszik. Amennyiben létezik az elkészített kapcsolódási gráfban olyan út a célig, amely csak üres cellákat tartalmaz, úgy az algoritmus ezt az utat használja. Ha nem létezik, akkor ezeket a cellákat további négy cellára bontja föl, majd ezeket is beilleszti a kapcsolati gráfba és ezen a gráfon hajt végre egy keresést. Ha még így sem talált üres cellákból álló utat, akkor tovább bontja a cellákat egészen egy minimum mértékig A D* keresés A D* keresés nulladik lépéseként meg kell határozni az indulási és a cél cellákat a gráfban. Minden cellának kivéve a célcellát van egy mutatója amely a cél eléréséhez vezető úton a következő cellára mutat, van egy tulajdonsága, amely NYÍLT, ZÁRT és ÚJ értékeket vehet fel, létezik továbbá minden szomszédos cella között egy költség, amely a cellák közti áthaladás költségét reprezentálja. Az algoritmus karban tart egy NYÍLT listát, amely arra szolgál, hogy az állapottérben bekövetkezett változásokat tovaterjessze a cellák között. Egy cella NYÍLT tulajdonságú, ha ezen a listán van, ZÁRT, ha már kikerült belőle és ÚJ, ha még soha nem volt rajta. Az eljárás minden cellára nyilvántart még egy céltól való becsült távolságot és egy kulcs tulajdonságot, amely a módosítás utáni céltól való távolságok és a módosítás előtti céltól való távolság minimuma. A NYÍLT listán lévő cellák e kulcs szerint vannak növekvően rendezve. A kulcs funkció lehetővé tesz tovább egy osztályozást a szerint, hogy a listán lévő cella céltól való távolsága egyenlőre ezzel az értékkel ebben az esetben LOWER, egyébként RAISE besorolást kap. Első lépésként minden cella ÚJ tulajdonságot kap, a célcella céltól való távolságát nullára állítjuk majd ráhelyezzük a NYÍLT listára. Ezek után levesszük a listáról az első elemet itt a cél cellát és megvizsgáljuk a szomszédos elemek céltól való távolságát, LOWER és RAISE tulajdonságait, és ezek alapján döntünk arról, hogy a szomszédos cella fölkerüljön-e a listára az kiválasztó algoritmus részletesebb leírása megtalálható a [21]-ben. Az eljárást addig folytatjuk, amíg a kiindulási cella át nem megy a nyílt listán. A környezetben bekövetkezett változás esetén az algoritmus a NYÍLT listára teszi az érintett cellákat és így végzi el a fent vázolt eljárást. Lényeges, hogy a kiválasztási metódus és a megállási feltétel miatt csak azon cellák adatai kerülnek egy változáskor frissítésre, amelyek frissítése esetén esély van egy rövidebb 15

17 út felfedezésére. Az algoritmus így lényegesen gyorsabb, mintha az egyébként kiindulásként használt A* algoritmust alkalmaznánk mivel az minden változáskor igényli a teljes újraszámolást ugyanakkor funkcionálisan ekvivalens azzal. A változások esetén történő frissítések további lokalizálására Stentz[22] bevezette a Focussed D* algoritmust, amely további sebességnövekedést ígér azokban az esetekben, amikor a környezetfrissítések mennyisége nem halad meg egy bizonyos mértéket A parti-game algoritmus A parti-game algoritmus az állapotteret négyszögletű cellákra bontja, majd egy virtuális ügynököt a startpozícióból elindítva megpróbál a célcellába jutni, közben esetlegesen tovább finomítva a cellákra bontást. A cellák költségének meghatározásához egy játékelméleti megközelítést alkalmaz. Az ágens minden pillanatban meg tudja állapítani, hogy mely pozícióban és mely cellában van. Minden cellából lehetősége van az összes szomszédos cellába továbbhaladni, megcélozva a cellák közepét. Minden cella nyilvántartja az ágens mozgásának eredményeit, ezeket hármasokban eltárolja a tapasztalati adatbázisba, mely hármas első eleme, hogy mely cellából indult az ügynök, második, hogy melyik cellát célozta meg, harmadik pedig, hogy melyik cellába érkezett. A cellák költségének kiszámítása ezen adatbázis szerint, minimax algoritmussal történik: Vegyünk egy ellenfelet, aki látván az ágens megcélzott celláját, az eddig tapasztaltak közül bármely hármast kiválaszthatja eredményként. A cellákat úgy pontozzuk, hogy számítunk arra, hogy az ellenfél mindig a számunkra legrosszabb lehetőséget fogja választani, így növelve az út hosszát, mi pedig próbáljuk ezt minél kisebbre venni. Nulladik lépésként egy cella reprezentálja az állapotteret és egy másik az elsőn belül pedig a cél cellát tartalmazza. Helyezzük az ügynököt a start pozícióba, majd utasítsuk, hogy haladjon a szomszédos cellák közül annak a közepe felé, amelyik a legrosszabb eshetőségeket feltételezve a legközelebb van a célhoz. A mozgás tapasztalatát jegyezzük föl az adatbázisba, esetlegesen osztályozzuk újra a cellákat, majd haladjunk a következő cella közepe felé, és így tovább. Ha az ügynök vesztes cellában tartózkodik vagyis a jelenlegi diszkretizáció mellett nem létezik út a célba akkor vágjuk mindazon vesztes cellákat ketté a hosszabbik oldaluknál, amelyek szomszédosak egy nyerő cellával és minden olyan nyerő cellát is, amelyek egy vesztessel szomszédosak, majd folytassuk az ügynök útkeresését (vesztes cella az, amelyből a legrosszabb eshetőségeket feltételezve nem létezik út a célba, nyertes pedig amelyikből létezik). 16

18 Egyéb módszerek Mindkét algoritmus problémája, hogy nagy cellaméret esetén a generált út bár gyorsan elkészül de kevésbé lesz optimális, mint kis cellaméret esetén. Megoldást nyújthat a Framed Quadtree[2] használata, amely minden, minimálisnál nagyobb cella köré egy minimális cellákból álló határt képez melyek között speciális szomszédsági viszony áll fenn és így hajtja végre a fenti eljárások valamelyikét. Az itt felsorolt algoritmusok alkalmazása főleg kétdimenziós környezetekben megszokott, ugyanakkor az általunk készítendő robotnak háromdimenziós térben kell mozognia. Bár mindkét módszernek van olyan változata, amely alkalmazható többdimenziós környezetben, nyilvánvaló, hogy lényegesen kisebb számítási kapacitást igényel a síkbeli változat, ezért próbáltam egy olyan megoldást találni, amellyel alkalmazható a kétdimenziós változat háromdimenziós környezetben olyan eseteket is beleértve, amikor a robotnak például egy spirális rámpát kell használnia a közlekedéshez. Szerencsére a közelítő cella dekompozíciós algoritmusokat viszonylag egyszerűen át lehet alakítani úgy, hogy a mi esetünkben is használhatóak legyenek. Amennyiben a robot egy olyan, tetszőleges domborzatú térben tud csak közlekedni, ahol nem létezik egymás fölé nyúló, elérhető útvonal, az algoritmusok a következő megszorításokkal használhatóak: két cellát csak abban az esetben veszünk szomszédosnak, ha a találkozási pontjukon a robot biztonságosan tud közlekedni, nem dől föl, stb. Amennyiben a két cella találkozásánál egyes részek biztonságosak, mások nem, úgy a cellák közül az egyik lehetőség szerint az, amelyikben a probléma oka található kevert cellának lesz jelölve. egy cella csak abban az esetben teljesen üres, ha a teljes területén, bármely irányban biztonságosan lehet közlekedni, ha csak részben, akkor kevert. E megkötések alkalmazásához természetesen szükséges az, hogy képesek legyünk tesztelni az adott terület átjárhatóságát, de ez első közelítésben nem tűnik bonyolult problémának. Amennyiben léteznek egymás fölé nyúló, járható felületek, úgy belátható, hogy a fönti algoritmus minden változtatás nélkül alkalmazható itt is, ugyanis az eljárás nem veszi figyelembe a cellák térbeli elhelyezkedését, csak és kizárólag az egyes cellák közötti átjárhatóságot. Például ha egy A cella egy másik, B cella C szomszédja fölött helyezkedik el, úgy B és C szomszédosak lesznek, A -ból viszont nem lesz él a többibe, így olyan út sem készülhet, amely lehetetlen függőleges irányú mozgást kényszerítene a robotra. 17

19 3. fejezet Rendszerterv 3.1. A projekttagok feladatai Berecz Szabolcs A projektmunkálatok felügyelete, a modellkezelő modul létrehozása, tesztelése, valamint a modulok összehangolása. Kovács Róbert A navigációs modul létrehozása, tesztelése. Kúthy Előd Zoltán A képfeldolgozó modul létrehozása, tesztelése Képfeldolgozó modul (Kúthy Előd Zoltán) A képfeldolgozó modul feladata a kamerák által szolgáltatott képi információ átalakítása a modellalkotáshoz szükséges adathalmazzá. Első körben az Intel által kiadott OpenCV csomag segítségével valósítjuk meg az élpontkiemelést[18]. A kapott pontokat a pont és környezetének intenzitása alapján párosítjuk össze. Amennyiben az így kapott pontpárok nem adnak megfelelő eredményt, akkor a képet transzformáljuk a monogenikus jellé[6] a Felsbergféle algoritmus[11] segítségével, majd ezen jelből választjuk ki a képpár legjellegzetesebb, azaz a legtöbb információt tartalmazó pontjait. Ekkor a kiválasztás energiamaximum kereséssel fog történni. A kiválasztott pontokat úgy párosítjuk össze, hogy a jellemzőkben lehetőleg kis eltérések legyenek. Ezen kritérium alól kivételt képez az irány jellemző, mivel az irányok eltérése függhet a távolságtól. Kérdéses a fenti adatok alapján a felületeket leíró háromszögek meghatározásának módja, mely a modellkezelő modul számára alapvető információ, lásd bővebben a modul leírásánál. Felmerült, hogy a háromszögeket valamely éldetektáló algoritmus alapján meghatározott egyenesek segítségével alkotjuk meg, 18

20 esetleg Hough transzformációt is alkalmazva. Alternatív megoldást jelenthet, ha a háromszögeket közvetlenül a monogenikus jel jellegzetes pontjaiból határoznánk meg, kérdéses a módszer pontossága. Fontos kérdés a kamerák egymáshoz viszonyított helyzete, és a kamerák távolsága. Párhuzamos kamera-elhelyezkedés és nagy kameratávolság alkalmazása nagy látótávolságot és pontos helymeghatározást eredményez, viszont jelentős holttérrel kell számolnunk (3.1. ábra). A kameratávolság illetve az optikai tengelyek metszéspontjának kameráktól vett távolságának csökkentésével a holttérrel együtt a látótávolság is csökken (3.2. ábra). A kamerák számunkra optimális helyzetének meghatározása a későbbiek során, gyakorlati tapasztalatok alapján történik ábra. Párhuzamos optikai tengelyek 3.2. ábra. Összetartó optikai tengelyek 3.3. Modellkezelő modul (Berecz Szabolcs) A modul több háromszögekkel ábrázolt felületmodell kezelését végzi, és két fő részre bontható: aktuális pozíció meghatározása, valamint a modell építése Pozíció meghatározás A képfeldolgozó modultól kapott lokális koordinátarendszerben lévő pontok, valamint a navigációs résztől kapott várható elmozdulásra vonatkozó információk alapján az ICP[4] módszer alkalmazásával megpróbálja meghatározni 19

21 az aktuális pozíciót. Ha a pozíció meghatározása sikertelen (pl. egy nagymértékű, váratlan elmozdulás, vagy bekapcsolás utáni ismeretlen aktuális pozíció miatt), akkor a spin-image módszer[10] alkalmazásával közelítő pontossággal kiszámítja az aktuális pozíciót, majd az ICP módszerrel tovább pontosítja azt Modellépítés Az aktuális pozíció meghatározása után a modellt kiegészíti az új pontokkal, valamint a modellben már szereplő pontokat tovább pontosítja. A koordináták pontosítása viszonylag egyszerű feladat, csak az összetartozó pontokat kell meghatároznunk, majd a két pont hibája és pozíciója alapján módosítjuk a modellben lévő pontot. Nem ennyire triviális a modell kiegészítése, mivel számítanunk kell arra, hogy a különböző kamerapozíciókból kismértékben eltérő szerkezetű felületet kapunk (más jellegzetességeket emel ki a képfeldolgozó modul). Mivel kiegészítés közben sok speciális esettel kellene foglalkozni (pl. egymást fedő, egy síkban lévő és különböző méretű háromszögek), melyek a modell térképként való használatát nem befolyásolják, az implementálás első fázisában a javítással nem foglalkozom. Az egyetlen (nem elhanyagolható) hatása a javítás mellőzésének az, hogy a modellben lévő pontok és felületek száma gyorsabban nő, mint szükséges lenne, ezt legegyszerűbben (és valószínűleg leghatásosabban) úgy orvosolhatjuk, ha időnként letisztítjuk a modellt. A tisztítás egyetlen nagy hátránya, hogy időigényes, így nem kerülhető el a kiegészítés közben történő ellenőrzés és javítás Hibás adatok kezelése A modell építése közben számítani kell hibás adatokra is, melyek valójában nem létező felületrészletek érzékelésében nyilvánul meg. Ezen hibák kiszűrésének egyik lehetséges módja egyfajta megerősítéses tanulás alkalmazása. A tanulást egy-egy felületrészletre csak addig alkalmazzuk, amíg meg nem bizonyosodtunk a felületrészlet valódiságáról, melynek feltétele, hogy adott idő alatt különböző pozíciókból is érzékeljük a kérdéses felületrészletet. A különböző pozíciókból való érzékelésre azért van szükség, mert ha a robot egy időre megáll, akkor a kameraképek változatlansága miatt minden felületet valóban létezőnek fogadnánk el. Azzal, hogy csak korlátozott ideig vizsgáljuk egy felületrészlet tényleges létezését, kizárjuk ugyan a környezet változásának folyamatos érzékelését, viszont lehetővé tesszük a hatékony működést, mivel csak néhány felületrészlet esetében kell figyelnünk az elévülésre. 20

22 Mozgás érzékelése A környezet változásának érzékelésére használhatjuk a hibás felületrészletek kiszűrésének módszerét azzal a módosítással, hogy csak azoknak a felületrészleteknek a valódiságát vizsgáljuk, amelyek a kamerák látószögén belül, az éppen látható felület és a kamerák között helyezkedik el. Ezen felületek meghatározásának gyorsítására is felhasználható a már említett kd-tree adatstruktúra ( pont). Ez a módszer nem követi elég gyorsan a változásokat a folyamatos mozgás érzékeléséhez, viszont lehetővé teszi, hogy a rendszer észrevegye egy akadály eltűnését Irányítás modul (Kovács Róbert) A robot irányítása A robot szoftverének e modulja felelős a robot irányításának minden mozzanatáért. A modul a következő részekre bontható: A generált modell hiányosságainak, vagyis a felderítendő területeknek a meghatározása A következő elérendő pozíció kiválasztása Út generálása a választott pozícióhoz A hordozójármű vezérlése A generált modell hiányosságainak meghatározása A Modellkezelő modultól kapott, háromdimenziós modellt a rendszer első lépésben egy felülnézeti, kétdimenziós térképpé alakítja, majd meghatározza azokat a területeket, amelyeket belát és azokat, amelyek takarásban, vagy túl távol vannak (3.3. ábra). Következő lépésként ezt cellákra bontja (a felderített-felderítetlen részek határainál nagyobb felbontással, lásd quadtree algoritmus[12]), és az újonnan felderített és a felderíthetetlen területeket feltünteti a tárolt térképben amennyiben nem létezik még ez a térkép, úgy a jelenlegit menti el. Felderíthetetlen terület például egy objektum belseje lehet. 21

23 3.3. ábra. Föltérképezett terület A következő elérendő pozíció meghatározása A tárolt térkép fehér foltjai közül ez az algoritmus határozza meg azt, amelyiket a legközelebbi mozgás alkalmával el kell érni. Az algoritmusnak figyelembe kell vennie többek között a robot irányultságát, sebességét, helyzetét és esetleges további körülményeket is Út generálása a választott pozícióhoz A robot jelenlegi terveim szerint az út létrehozásához a parti-game algoritmust fogja használni. A cél meghatározása után a parti-game algoritmusban leírt, ágenses módszerrel kerül az oda vezető út megtervezésre (lásd az irodalomkutatás navigációs részét). Amennyiben nem sikerül utat találni a célig, úgy új cél meghatározását kéri a rendszer. Amennyiben a belső heurisztika szerint ez a cél nem csak ideiglenesen elérhetetlen a környezet felderítetlen mivolta miatt, úgy bejelölődik a térképen, mint elérhetetlen cella. 22

24 A hordozójármű vezérlése Egyenlőre nem áll rendelkezésünkre információ arról, hogy milyen módszerrel lehet majd irányítani a hordozójárművet, így kénytelen vagyok azt írni, hogy az algoritmus csak a későbbiekben lesz kidolgozva. Annyit mindenesetre feltételezek, hogy valamilyen, a játékautók irányításához hasonló, esetleg sebességszabályozást is lehetővé tevő kezelőfelülete lesz a járműnek, amely ugyanakkor nem lesz túl pontos A robot mozgásának kalibrálása Amennyiben rendelkezésünkre állna a robot sebességéhez viszonyítva gyors helymeghatározási rendszer, úgy a navigációs rendszer képes lenne a hordozójárművet oly módon irányítani, hogy miután egy pontatlanul megadott irányba, pontatlan sebességgel elindította a járművet, a helymeghatározási rendszer segítségével folyamatosan korrigálná a mozgást a szerint, hogy mennyivel tér el a számítottól. Ha viszont e helymeghatározási rendszer nem elég gyors, úgy túl nagy lesz az eltérés az előre tervezettől, esetleg akkora, hogy a robot nekiütközik valaminek, vagy képtelen visszatalálni a tervezett pozícióba. Amennyiben egyáltalán nem állna rendelkezésünkre helymeghatározási rendszer, úgy csak abban az esetben lennénk képesek megfelelően irányítani a járművet, ha teljesen pontos adatokkal rendelkeznénk az egyes irányítási utasítások hatásairól, vagyis hogy mi lesz a pozíció azok végrehajtása után. Ez pedig csak speciális körülmények között valósítható meg: teljesen sík domborzat, egyenletes talajviszonyok, egyenletesen járó motor, vagyis komoly lehetőségként nem jöhet szóba. A problémát az előző két megoldás ötvözésével tervezzük megoldani: egy kezdeti, a vezérlő utasítások hatásainak durva beállítása után a robot folyamatosan korrigálná ezen adatokat. E kalibráció a navigációs és a modellkezelő rendszer közötti, folyamatos kommunikáción alapul. A navigációs rendszer a tárolt kalibrációs adatok alapján meghatározza ama járművezérlő utasítások sorozatát, amelyekkel belátása szerint a kívánt pozícióba jut a robot, majd a mozgás során folyamatosan lekérdezi az alatta lévő, modellkezelő rendszert a robot általa ítélt helyzetéről, majd ezen információk alapján korrigál a kalibrációs adatokon továbbá természetesen a jelenlegi mozgáson is, így a robot képes lesz lejtőn vagy nehéz terepen (pl.: szőnyeg) is közlekedni. Abban az esetben, ha lehetőség lesz a jármű sebességének szabályozására, úgy a robot képes lesz nagy bizonytalanságot produkáló talajon is közlekedni: ha túl nagy az eltérés a tervezett és a tényleges pozíció között, akkor egyszerűen lelassítja a robotot. 23

25 3.5. Tesztelés Első körben a képfeldolgozó és modellkezelő modulok tesztelése a navigációs modultól elkülönülve történne. Hiteles bemeneti információt a kamera rángatásával állítanánk elő. Az ebből előállított modellből levont következtetések hozzásegíthetnek számos függőben maradt kérdés megválaszolásához. A navigációs modul tesztelése kezdetben rajzolt térképekkel fog történni. Miután az alrendszerek önmagukban már elfogadható működést mutatnak, következhet az integrált rendszer tesztje, kezdetben szintén rángatós módszerrel. Végső szintet a navigációs modul által vezérelt mobil robot in vitro tesztje képezné. 24

26 4. fejezet Megvalósítás 4.1. Képfeldolgozó modul (Kúthy Előd Zoltán) A képfeldolgozó modul feladata a sztereó képpárok feldolgozása olyan módon, hogy meghatározza a környezetet reprezentáló pontok koordinátáit a robothoz rögzített koordinátarendszerben. A mélység meghatározásához hasonlóan az emberi szemhez szükséges a két kép eltéréseinek kiértékelése, illetve a kamerák külső és belső paramétereinek ismerete. A modul C++ nyelven, került implementálásra, az Intel cég által fejlesztett OpenCV csomag felhasználásával. A képeket két darab Logitech QuickCam Zoom USB csatlakozású webkamera szolgáltatja Kamerakezelés Először a két kamerát az OpenCV csomag nyújtotta szolgáltatással kezeltük. Sajnos, ez az eljárás csak egy kamera esetén járható út, mivel a két kamera párhuzamos alkalmazása esetén a képfrissítés 3-4 frame/sec-es értékre esett. Jelenleg a kamerakezelés DirectShow segítségével történik, így már két kamera párhuzamos alkalmazása esetén is elfogadható a képfrissítés sebessége[19] Kamerakalibráció A képpontok háromdimenziós koordinátáinak rekonstrukciójához a vízszintes diszparitások meghatározásán túl ismernünk kell az effektív pixelméretet a kamerától vett távolság függvényében, illetve a pixeldiszparitások pixelértékeihez rendelhető távolságot. Utóbbi segítségével határozhatjuk meg adott pont pár diszparitásértéke alapján a pont pár mélység koordinátáját, az előbbi szükséges a pixel 25

27 koordinátákról távolság koordinátákra való áttéréshez. Mindkét függvényt egy kalibrációs képsorozattal határoztuk meg, mely egy sakktábla mintázatot tartalmaz, és ismert a kiemelt pontok valós távolsága, illetve a kamera és tábla távolsága. Az 4.1. ábrán láthatóak a kamerakalibráció eredményei, az első pontsorozatra lineáris illesztést végeztünk, a másik két méréssort exponenciális függvénnyel közelítettük. (a) Effektív pixelméret a távolság függvényében (b) Képponttávolság a diszparitás függvényében (c) Effektív pixelméret a diszparitás függvényében 4.1. ábra. A kamerakalibráció eredményeként kapott görbék 26

28 A megfelelő pontok kiválasztása Először ki kell választanunk a képek azon jellemző pontjait(tokeneket), melyeket később össze tudunk párosítani a két képen. Ezen feladat megoldására az OpenCV csomag GoodFeaturesToTrack függvényét alkalmaztuk. Az algoritmus a következő lépésekből áll[18]: Minden képponthoz meghatározzuk a következő 2 2-es kovariancia mátrixot: [ (di/dx) 2 ] (di/dx)(di/dy) K = (di/dx)(di/dy) (di/dy) 2, (4.1) melynek elemeit az adott pont 3 3-as környezetében lévő pontok x illetve y irányú deriváltjainak összegeként képezzük. Kiszámítjuk a mátrix sajátértékeit, a következő egyenlet alapján: det(k λe) = 0, (4.2) ahol λ a sajátértéket, E pedig az egységmátrixot jelöli, ebből λ-t kifejezve: λ 1,2 = D 2 x + Dy 2± ( Dx 2+ Dy 2)2 4( Dx 2 D 2 y ( D x Dy) 2 ), (4.3) 2 ahol D x az x irányú, D y az y irányú deriváltat jelöli. Minden képponthoz hozzárendeljük a két sajátértéke közül a kisebbet. A képet 3 3-as ablakokra osztjuk, és a továbbiakban csak azon pontokat tarjuk meg, melyhez tartozó sajátérték lokális maximum az adott ablakra (Non-maxima suppression). Azon pontokat is eldobjuk, melyekhez tartozó sajátérték nem éri el a legnagyobb sajátérték bizonyos hányadát. Ez a százalékérték a függvény egyik paramétere(t). Végül a függvény másik paramétereként (d min ) megadott pixeltávolságnál egymáshoz közelebb lévő pontok közül csak a legnagyobb sajátértékkel rendelkezőt tartjuk meg. A két képből a fenti algoritmussal kiválasztott pontokat kell összepárosítani. Az eljárás hatékonyságát természetesen döntően a két paraméter értékének megfelelő megválasztásán múlik. A vizsgált tesztképek esetében t = 5 10%, d min = 5 10 paraméter értékeknél sikerül a legjobb eredményeket elérni. 27

29 (a) t = 2%,d min = 10 (b) t = 10%,d min = 5 (c) t = 10%,d min = 10 (d) t = 10%,d min = ábra. Token-detektálás különböző paraméter értékek esetén Pontpárok képzése A két képen nagy valószínűséggel azon pontok egyeznek meg, melyeknek intenzitáskülönbsége illetve környezetük intenzitáskülönbsége minimális. Ezért minden lehetséges párosításhoz meghatározzuk a következő értéket: D = 1 1 i= 1 j= 1 a x+i,y+j b u+i,v+j, (4.4) ahol a n,m a baloldali kép (n,m) koordinátájú pontjának, b n,m a jobboldali kép (n, m) koordinátájú pontjának intenzitását jelöli, (x, y) a baloldali kép tokenjének koordinátája, (u,v) a jobboldali kép tokenjének koordinátája. Ideális esetben a minimumértékekhez tartozó párosítások a helyesek. Mivel a lehetséges párosítások száma a tokenszámmal exponenciálisan növekszik, illetve nem biztos, hogy 28

30 adott tokennek van párja pl. takarásban van csak az egyik képen a pont -, ezért a következő megszorításokat alkalmazzuk a párosítás során: Mivel sztereó képek esetén a pontpárok y irányú diszparitása nem túl nagy, csak olyan pontpárokat vizsgálunk, melyek y koordinátájának eltérése egy meghatározott értéknél kisebb ( es felbontású kameraképekhez 5 pixeles eltérést használtam). Szintén korlátozzuk a maximális x irányú eltérést is, természetesen itt jóval nagyobb értéket alkalmazunk. Ezzel az algoritmus gyorsítása mellett a párosítás hibáját is csökkentjük, mivel elképzelhető, hogy több hasonló intenzitáskörnyezetű token is található a képen, melyek közül a triviális rossz (nagy távolságra lévő) párosításokat kiszűrjük ezzel a korlátozással. A programban ez a korlát 50 pixel, es felbontású képpárok esetén. Az algoritmus a baloldali tokenekhez keresi a jobboldali párjukat meghatározva az intenzitásbeli eltérésüket (4.4 egyenlet), de mielőtt párként regisztrálná őket, megvizsgálja, hogy az adott jobboldali tokenhez nem létezik-e kisebb eltérésű baloldali token. Amennyiben talál ilyet a pontpárt csak lehetséges párként jegyzi fel, ellenkező esetben biztos párként. A lehetséges párok feljegyzésére azért van szükség, mivel az előbbiekben megemlített második párkeresés alkalmával talált baloldali tokenhez tartozhat kisebb differenciájú jobboldali token, így jó párosítások is elveszhetnek. Miután az összes baloldali tokenen végigment az algoritmus, megvizsgáljuk, hogy a lehetséges párok közül melyek maradtak párosítatlanok, ezeket szintén biztos párként jegyezzük fel. A vizsgált képek esetén a program a lehetséges párok 80 90%-át találta meg, a párok 3 5%-a volt hibás A mélység meghatározása A pontpárokból az alábbi egyenletekkel határozzuk meg a háromdimenziós koordinátákat, ahol az origó a két lencse középpontját összekötő szakasz felezőpontjában helyezkedik el: X = ( x b + x j 2 o x )s x (4.5) Y = ( y b + y j o y )s y (4.6) 2 Z = f(x b x j ), (4.7) 29

31 4.3. ábra. A pontpárok ábrázolása a két kép átlagán ahol x b,y b,x j,y j a bal- illetve jobboldali token képkoordinátája, o x,o y optikai középpont képkoordinátája, s x,s y pixel x és y irányú mérete, f a kamerakalibráció során a mérési pontokra illesztett exponenciális függvény. Az így előállt háromdimenziós koordinátákat tartalmazó tömböt adjuk át a modellkezelő modulnak ábra. Megtalált pontpárok összekötve 4.2. Modellkezelő modul (Berecz Szabolcs) A modul egyszerű részeit C++ nyelven, a bonyolultabb részeket Python nyelven implementálom, mivel ez utóbbi nyelv jobban megfelel az algoritmus prototípusok gyors elkészítésére és tesztelésére. A későbbiekben, mikor már fontos lesz a kis számításigény, a problémásabb programrészek viszonylag egyszerűen átültethetőek C++ nyelvre. 30

32 Ahhoz, hogy a két nyelv együttműködhessen, szükség van egy interfészre, melynek létrehozása elméletileg kézzel is lehetséges, gyakorlatilag viszont csak stabil C++ objektum-interfészek esetén kivitelezhető (fáradságos és felesleges munka árán). Ehelyett használhatunk olyan programokat/library-ket, melyek bizonyos szintig automatizálják ezt a feladatot. Az általam használt library, a Boost.Python[5] egy viszonylag egyszerű interfész definíció alapján hozza létre a két nyelv közötti átjárást biztosító interfészt. Az interfész definíció egy egyszerű C++ file, ami a Pythonból használni kívánt osztályok, függvények, stb. neveit, és néhány kiegészítő információt tartalmaz (pl. abban az esetben, ha egy függvény visszatérési értékének típusa pointer vagy referencia, meg kell adnunk, hogy a Python milyen módon kezelje ezt az objektumot, mert a deklarációk alapján nem dönthető el hogy a hivatkozott objektum felszabadítása kinek a feladata). Ezt a C++ file-t lefordítva kapunk egy object file-t, amit már használni tud a Python interpreter. Az interfész definíció létrehozását is kikerülhetjük, ha használjuk a Pyste nevű programot, mely egy C++ header file alapján létrehozza helyettünk az interfészt definiáló C++ file-t, de így sem úszhatjuk meg a pointerek, referenciák kezelési módjának megadását. Ezek után a C++-ban implementált algoritmusok és típusok egyszerűen elérhetők Pythonból is, sőt teljesen úgy viselkednek, mintha Pythonban implementáltuk volna: működik az öröklés, a C++-ban megírt osztályokhoz hozzáadhatunk újabb metódusokat, vagy egy létezőt lecserélhetünk, stb Modellek ábrázolása A modellek ábrázolása háromszögekből álló felületekkel történik. Egy teljes felület leírásához négy osztály szükséges: v3d, vertex, face és mesh osztályok. A négy osztályból három szorosan összekapcsolódik, gyakorlatilag egyik sem használható a másik kettőtől függetlenül. mesh Ez egy a vertex és face osztályok példányait aggregáló osztály. A példányokat egy-egy tömbben tárolja, így egyszerűen hivatkozhatunk az egyes vertex-ekre és face-ekre, valamint lehetővé teszi néhány művelet egyszerű implementálását. v3d Egy egyszerű háromdimenziós vektort valósít meg. A v3d osztályon értelmezett műveletek a szokásos vektorműveletek: összeg, különbség, skaláris és vektoriális szorzat, skalárral való szorzás, hossz, normalizálás, és két vektor különbsége. vertex A felület egy pontját írja le. Ennek két bázisosztálya van: a v3d és a markable osztály. Számon tartja azoknak a face-eknek az indexét, melyek egyik csúcsa ez a pont. 31

33 face A felület egy részletét ábrázolja három vertex segítségével. A vertex-ekre azok mesh-beli indexével hivatkozik. Az osztálynak egyetlen bázisosztálya van, a markable osztály. További osztályok egyszerűsítik a felületek kezelését, ezek, valamint az előzőekben röviden bemutatott osztályok bővebb bemutatása következik A mesh osztály A pozíció meghatározása, valamint a már meglévő modell kiegészítése szükségessé teszi a Kd-tree struktúra használatát, melynek Martin F. Krafft által implementált változatát használom[13] A vertex és face osztályok Ez a két osztály lehetővé teszi, hogy kiszámítsuk a felület egy vertex-éhez, vagy face-éhez tartozó normálvektort. Mivel egy face-hez tartozó normálvektor meghatározásához ismernünk kell a face-t alkotó vertex-ek pozícióit, egy vertex normálvektora pedig a környező face-ekből számítható, könnyen elérhetővé kell tenni az egyik osztályból a másikat. Ennek legegyszerűbb módja, ha minden vertex és face osztály számon tartja egy pointer segítségével, hogy melyik mesh tartalmazza őket, majd ezen a mesh-en keresztül kérdezi le a szükséges információkat. Ez egy nagyméretű mesh esetén nagy memóriaveszteséggel jár, viszont lehetővé tesz néhány konzisztencia-ellenőrzést, valamint szükség esetén viszonylag kis módosításokkal eltávolítható ez a hivatkozás. A számításigény is csökkenthető, ha eltároljuk a már kiszámított normálvektorokat, viszont ez tovább bonyolítja egy mesh módosítását, így csak akkor érdemes megvalósítani, ha a kód nagy része már viszonylag keveset változik A breadth_first_vertex_iterator osztály Néhány esetben (pl. a pontban bemutatott spin-image módszerre specializált ICP algoritmusban) szükséges a felület egy adott vertex-étől egyre távolodva bejárni a felület vertex-eit. Ezt egy szélességi kereséssel lehet megoldani, ahol az aktuális vertex-hez tartozó face-ek vertex-eit egy FIFO-ba helyezzük, a következő vertex-et pedig a FIFO-ból egy elemet kivéve kapjuk. Ezzel a módszerrel egy vertex többször is bekerülhet a FIFO-ba, így fel kell jegyeznünk, hogy melyik vertex-et használtuk már fel. Ezt a feladatot a marker és a markable osztályok oldják meg ( pont). 32

34 Az osztály két fontos metódussal rendelkezik: next() Ez a Python nyelv iterátor interfészéhez tartozik, a feladata meghatározni a következő elemet, vagy ha nincs több, egy StopIteration kivételt dobni. turn_back() Ha az aktuális pont szomszédait nem szeretnénk bejárni, ennek a metódusnak a segítségével jelezhetjük ezt. A turn_back() metódus megvalósításához az objektum külön tárolja az utolsó next() híváskor meghatározott szomszédokat, így ha ebben az irányban nem akarunk továbbmenni, a turn_back() metódus egyszerűen eldobja ezeket az elemeket, egyébként viszont a következő next() hívás első lépésként ezen elemeket a FIFOba teszi. A következő Python kódrészletből látszik, mennyivel átláthatóbbá teszi ez az osztály a vertex-ek bejárását. A kódrészlet a model_mesh 0 indexű vertex-étől legfeljebb öt egység távolságra lévő szomszédos vertex-eket gyűjti össze a nearby_vertex_neighbours listában: vertex_iter = breadth_first_vertex_iterator(model_mesh, 0) base_vertex = model_mesh.get_vertex(0) vertex_neighbours = [] for v in vertex_iter: if base_vertex.distance(v) > 5: vertex_iter.turn_back() else: vertex_neighbours.append(v) Ezzel a módszerrel egyszerűen létrehozható egy depth_first_vertex_iterator osztály is, mely mélységi kereséssel járja be a felület pontjait. Az implementációban az egyetlen különbség a bejárandó vertex-ek tárolásának módja: FIFO helyett LIFO-t kell használnunk A marker és markable osztályok A két osztály segítségével objektumokat jelölhetünk meg valamilyen céllal ( pont). A megjelölni kívánt osztály egyik ősosztályának a markable osztályt választjuk, amely az utolsó jelzés azonosítóját tárolja. A marker osztály minden objektuma különböző azonosítóval rendelkezik. A mark() metódus segítségével megjelölhetünk egy markable objektumot (beállítjuk a markable objektum által tárolt azonosítót az aktuális marker objektum azonosítójára), valamint az is_marked() metódussal összehasonlíthatjuk az azonosítókat. 33

35 A indexed_v3d osztály Mivel a vertex-ek tömbben történő tárolása sok feladat megoldását egyszerűsíti, a Kd-tree adatstruktúra implementációja pedig saját adatterületen tárolja a pontokat, szükség van egy leképezésre a Kd-tree-ben tárolt pontok és a mesh osztályban tárolt vertex-ek között. Ennek megvalósítása az indexed_v3d osztállyal (ennek bázisosztálya a v3d osztály) történik, mely a pont koordinátáin kívül annak mesh objektumban elfoglalt helyét is tárolja. Ez gyakorlatilag kétszeres memóriaigényt jelent, de lehetővé teszi a modellek gyors kezelését A spin_stack osztály Ez az osztály tárolja egy mesh objektum által leírt felület pontjaihoz tartozó spin-image-eket. Mivel a mesh osztály egy tömbben tárolja a felület pontjait, kézenfekvő a választás, hogy a spin_stack osztály is egy tömböt használjon az egyes pontokhoz tartozó spin-image-ek tárolására, így ugyanazzal az indexszel hivatkozhatunk egy pontra, és az ahhoz tartozó spin-image-re. Az implementáció többi részlete gyakorlatilag teljesen megegyezik a Johnson[10] által leírttal Teszteredmények Mivel valódi adatok még nem állnak rendelkezésre, így a tesztelést a Johnson által használt kacsa modellen (2.1. ábra), egy kézzel összeállított szoba modelljén (4.5. ábra), és ezek zajos változatán végeztem (mindegyik modell kb. ezer pontból áll). A hozzáadott zaj mértékét a modell tömegközéppontjától számított átlagos ponttávolsághoz viszonyítva határoztam meg. Az algoritmus paramétereinek finomhangolásához több valódi adatra lenne szükség, így az algoritmus hatékonyságát ezen eredmények alapján nem lehet megítélni ábra. A szoba modellje 34

36 A tesztelés során a modelleken nem szükséges transzformációt végezni, mivel a spin-image algoritmus a ponthalmaz bármilyen helyzete esetén ugyanazt az eredményt adja, így az algoritmus hatékonyságának teszteléséhez elég a pontokhoz zajt adni (zaj nélküli, kevés ismétlődést tartalmazó modellek esetén mindig helyes eredményt ad). A tesztek egyszerűen kiértékelhetők, mivel a zajt tartalmazó modellt az eredeti modell duplikálásával majd egyenletes eloszlású zaj hozzáadásával hoztam létre, így a két modellben a pontok azonos sorrendben szerepelnek, tehát annak ellenőrzéséhez, hogy helyesek-e a meghatározott pontpárok, elég páronként összehasonlítani a pontok indexét. Egyezés esetén helyes az eredmény. A feladat száz véletlenszerűen kiválasztott pont párjának meghatározása volt különböző mértékű zajok esetén. Röviden összegezve az eredményeket: a kacsa modellen végzett tesztek eredménye biztató, a szoba modell eredménye elmarad ugyan a kacsa modellétől, de szintén biztató. A 4.6. ábrán az egy és két százaléknyi maximális zajt tartalmazó kacsa modelleken végzett ötven teszt eredménye látható. A grafikonokról leolvasható, hogy egy százalék zaj mellett tökéletes az eredmény. Két százalék zaj mellett, megfigyelhető, hogy kellően sok kiindulási pontot felhasználva, a csoportosított pontpárok alapján számított legjobb transzformáció valószínűleg helyes eredményt ad. p o 50 n t 40 p á 30 r o 20 k 10 s z 0 á m a -10 Kacsa modell, 1% hiba teszt sorszáma p o 50 n t 40 p á 30 r o 20 k 10 s z 0 á m a -10 Kacsa modell, 2% hiba teszt sorszáma 4.6. ábra. Kacsa modell, egy és két százalék zaj 1 A szoba modell eredményein (4.7. ábra) látható, hogy a hibázási arány hasonló, mint a kacsa modell esetében, viszont láthatóan kisebb a meghatározott pontpárok száma. Ennek oka az, hogy a szoba modell túlzottan mesterséges, így kevés jellegzetes felületrészlet található a modellben: az algoritmusban lévő egyik szűrés azokat a pontpárokat dobja el, amelyek spin-image-einek hasonlósági mértéke nem kellően eltérő a többitől. Ha ezt a küszöbértéket csökkentjük, akkor kevesebb pontot dob el a rendszer, aminek két fontos következménye van: 1 Kék háromszög: helyes, piros négyzet: hibás párosítás 35

37 p o n t p á r o k Szoba modell, 1% hiba s z á 0 m a teszt sorszáma p o 50 n t 40 p á 30 r o 20 k 10 s z 0 á m a -10 Szoba modell, 2% hiba teszt sorszáma 4.7. ábra. Szoba modell, egy és két százalék zaj 1 Megnő a számításigény, mivel több pontpárból több csoportot hozhatunk létre, és ezek mindegyikére meg kell határoznunk a transzformációt. Több hibás pontpár jut át a szűrésen, ezáltal rontva az utolsó lépésként meghatározott transzformáció helyességét. Tehát a küszöbértéket érdemes magasan tartani, viszont azokban az esetekben, amikor többszöri próbálkozásra sem sikerül kellő számú pontpárt meghatározni, ideiglenesen lecsökkenthetjük, hogy kellő számú pontpárt kapjunk (ami egyáltalán nem biztosítja, hogy megközelítőleg helyes transzformációt sikerül meghatározni, de legalább lehetőséget ad rá) A transzformációk ábrázolása Mivel eltoláson és forgatáson kívül semmilyen transzformációra nincs szükség, felesleges az elterjedt 4 4-es mátrix használata. Hatékonyabban megoldható a pontok transzformációja, és a transzformációk konkatenációja, ha a forgatás leírására egy kvaterniót, eltoláshoz pedig egy vektort használunk. Ezzel az ábrázolásmóddal egy pont transzformációját a következő kifejezéssel végezhetjük el: p = t + qp q 1 Forgatás leírásához egységkvaternióra van szükség, ennek inverzét pedig egyszerűen számíthatjuk: negáljuk a kvaternió vektor komponensét. A kifejezés további egyszerűsítésével (mely egyszerűen elvégezhető egy szimbolikus matematikai programcsomaggal, ilyen pl. az általam használt Maxima[14]) azt kapjuk, hogy n pont transzformációjához n szorzásra valamint n összeadásra van szükség. 36

38 Két transzformáció konkatenációjának kiszámítása: q = q 2 q 1 t = t 2 + q 2 t 1 q 1 2 Egyszerűsítések után szorzás és összeadás marad, melyekből 18 szorzást és 12 összeadást csak egyszer kell elvégeznünk egy transzformáció vagy egy transzformáció-konkatenáció során. 4 4-es mátrix használata esetén pont transzformációjakor 16n szorzás és 12n összeadás, transzformáció konkatenáció esetén pedig 64 szorzás és 48 összeadás szükséges. Egy másik lehetőség, hogy a kvaternió helyett egy 3 3-as mátrixot használunk, de mivel az abszolút orientáció meghatározása során kvaternió formájában kapjuk a forgatást, a számításigény nem változik, az átalakítás viszont felesleges pontatlanságokat okoz Irányítás modul (Kovács Róbert) Az irodalomkutatás részben említett parti-game algoritmus került kisebb módosításokkal megvalósításra, de folyik egy quadtree alapú D* search implementálása, amely nem kizárt, hogy jobb eredményeket fog produkálni, mint a jelenleg használt. Az út generálása jelenleg a következő lépésekből áll: a modellkezelő modultól kapott háromdimenziós modell kétdimenziós térképpé alakítása a parti-game-hez hasonló algoritmus alkalmazásával generálunk egy utat, mely azonban csak egy szabadon szálló, kör alakú, minden irányban végtelen gyorsulásra képes objektum számára járható a generált utat ezek után megpróbáljuk úgy átalakítani, hogy a valóságos jármű is képes legyen azon végighaladni A modell térképpé konvertálása A modellkezelő modultól kapott térbeli felületeket lényegében háromszögeket első lépésben két részre osztjuk meredekség szerint: a túl nagy dőlést mutatóak egyértelműen tereptárgy címkét kapnak, a vízszinteshez közeliek közül válogatjuk ki azokat, amelyek esetlegesen a robot számára megközelíthetőek. 37

39 Következő lépésként meghatározásra kerülnek az(ok) a felület(ek), amelyeken a robot jelenleg tartózkodik ezt a kapott pozícióinformáció alapján tesszük meg úgy, hogy a robot méreteit ismervén vesszük azokat a felületeket, amelyek a kerekek alatt helyezkednek el némi tűréssel. Amennyiben a kiindulási hely több felületből áll, úgy meghatározzuk azokat az éleket és élrészeket, amelyek a "széleket" alkotják, vagyis a háromszögek bizonyos tűréssel való egyesítése során az egyesített felület széleit képeznék. Ezek után tovább válogatjuk a vízszintes címkét kapott felületeket olyanokat keresve, amelyek csatolhatóak az eddigiekhez; ha találunk ilyet, akkor azt hozzávesszük az eddig találtakhoz és újra meghatározzuk a virtuális idom széleit, majd addig folytatjuk a keresést, amíg már egyetlen háromszög sem csatolható. A háromszögek szomszédsági viszonyait egy gráfban tároljuk. Az így kiválasztott felületeket följegyezzük a térképre, amennyiben léteznek egymás fölött elhelyezkedő felületek, úgy több térképre. A térkép az egyes pontjaiban három értéket vehet fel: ISMERETLEN, ÜRES, OBJEKTUM. Inicializálásként létrehozunk egy olyan térképet, amely minden pontjában ISMERETLEN értéket tartalmaz, majd erre rávetítjük a kiindulási helytől indulva a felületeket egyszerűen úgy, hogy a magasság koordinátáikat elhagyjuk. Amennyiben egy felület a térkép olyan részére vetülne, amely már nem ISMERETLEN címkéjű, úgy létrejön egy újabb térkép újabb szint vagy ha már létezik, akkor ez lesz az aktuális, és erre vetítődik rá a felület. Minden felület kap egy tulajdonságot, amely azt mutatja meg, hogy melyik térképre lett ráhelyezve. Ezen tulajdonság és a szomszédsági gráf alapján meghatározzuk azokat az éleket, amelyeket átlépve képzeletbeli barangolónknak egy másik térképen kell folytatnia útját, majd ezeket is bejelöljük a térképen. A feldolgozatlan felületeket megvizsgáljuk, hogy bármely részük olyan tartományban van-e, amely zavarná a robot mozgását. Ha ilyet találunk, akkor ezeket bejelöljük a térképeken, tekintet nélkül arra, hogy a kérdéses térképterületen milyen értékű pontok találhatóak. Az összes háromszög feldolgozása után a térképeken lévő ISMERETLEN címkéjű területek közül kiválasztjuk a felderítésre váró részt, majd ezeket a részeket is objektumként jelöljük Az út generálása A parti-game algoritmus Az implementált algoritmus a parti-game elveire épül, de számos helyen különbözik attól. Fő különbség, hogy az eljárás az első bejárás során végleges, a környezet aktuális felbontásához viszonyítva optimális útvonalat generál, szemben az eredetivel, ahol az ügynök első próbálkozása általában jóval hosszabb útvonalat produkál mint a második vagy a harmadik esetben. További lényeges eltérés még, 38

40 hogy az ügynök két típusú tapasztalatot szerezhet szemben az eredeti eggyel: az egyik akkor jön létre, mikor egy cellát megpróbál elhagyni, a másik pedig akkor, amikor egy cella közepét szeretné elérni ezeket az állapottér további felosztásakor vesszük figyelembe. Az eredeti, ügynökös szemléltetési módszert is csak az egyszerűség kedvéért használjuk, mivel jelen esetben közbenső célként sem jelenik meg, hogy az ügynököt eljuttassuk a megadott koordinátákra számunkra a közbenső cél az induló cellától a célcelláig vezető út létrehozása Inicializáció Bemenő adatként az eljárás kap egy vagy több bináris térképet a környezetéről, továbbá egy start és egy cél koordinátát. Az algoritmusnak lényegében mindegy, hogy egy vagy több térképpel kell dolgoznia, mivel a környezet belső reprezentációja egy szomszédsági gráf, melyben több térkép akár bonyolult összeköttetései is egyszerűen ábrázolhatóak. Nem tartozik szorosan az algoritmus keretei közé, de nulladik lépésként egyfajta dilatációt végzünk el, vagyis megnöveljük a tereptárgyak kiterjedését. Ezt a következők miatt tesszük: a procedúrának nem feladata vizsgálni, hogy az általa megtalált úton egy fizikai kiterjedésekkel rendelkező jármű elfér-e, így akkor is talál megoldást, ha az tartalmaz egy vagy több nagyon szűk járatot is. A tereptárgyak "felhízlalásával" viszont elérjük, hogy ezek a sikánok eltűnjenek, továbbá, hogy az eredeti térképen a generált út és a tereptárgyak között minimum akkora távolság legyen, mint amekkora a szélesítés mértéke volt. Említést érdemel még, hogy definiálunk egy maximális és egy minimális cellaméretet is, ahol a maximális cellaméret csak a környezet első felbontásakor lényeges, a minimális pedig azért szükséges, mert szemben az eredeti elmélettel itt nem feltételezhetjük, hogy biztosan létezik út a start és a cél koordináták között. Ezek után a meghatározott maximális méretű cellákra bontjuk a környezetet és triviális módszerrel meghatározzuk a szomszédsági viszonyokat is. Az eredeti eljárásban induláskor csak két cella volt: egy, amely tartalmazta az egész állapotteret, és egy másik, amely csak a cél koordinátát. Két ok is volt, amiért ezt a módszert elvetettem: egyrészt kis tereptárgy-sűrűség esetén bár gyorsan de pazarló útvonal generálódik, több akadály esetén pedig időbe telik, amíg az állapottér felbontása eléri a megfelelő mértéket. Legnagyobb cellaméretnek egyenlőre előre definiált értéket használunk, későbbiek során valószínűleg a kapott térképen lévő objektumok számától függően fogjuk ezt meghatározni egyébként az objektumok számának meghatározásával együtt az algoritmus lassabb, mint az eredeti, de mivel ezt mindenképpen meg kell tennünk egy ettől független procedúra használatához, így nem vesszük figyelembe az időigényesség szempontjából. Az egy lépéses útgenerálás és pontosabb érkezési pozíció elérése miatt a start és célkoordinátákat tartalmazó cellákat minimális méretűre csökkentjük, a követ- 39

41 kező egyszerű módszerrel: LOOP: 1. Megkeressük azt a cellát, amelyik a koordinátát tartalmazza 2. Ha ez minimális méretű, kilépünk 3. Ha nagyobb, mint a minimális cellaméret, felezzük a cellát A célcella minimális méretűvé alakítására azért van szükség, hogy az érkezési pozíciót pontosabban tudjuk elérni, a kiindulási cella átalakítása pedig az ügynök szerepének csökkenése miatt lényeges az eredeti eljárásban az ügynök pontosan a start pozícióból indult, így az utat is pontosan innen generáltuk. A célcella céltól való távolságát (Goal Distance, továbbiakban GD) 0-nak vesszük, majd az általános esetben is használt algoritmussal meghatározunk egy GD értéket minden cellához úgy, hogy egy cella céltól való távolsága egyenlő az egyes szomszédai plusz a szomszédaitól való távolságának minimumával. Két cella távolságát sakktábla távolságot használva határozzuk meg, vagyis egy (x 1,y 1 ) középpontú cella egy másik, (x 2,y 2 ) középpontú cellától t távolságra van, ha max( x 1 x 2, y 1 y 2 ) = t; két cella szomszédos, ha van közös határolószakaszuk. Készülhetett volna speciális algoritmus is a kezdeti céltól való távolságok meghatározására, de teljesen elhanyagolható az az idő a teljes futásidőhöz képest, amíg az általános algoritmust végrehajtjuk A fő eljárás Az algoritmus egy iterációs lépésben megpróbál az aktuális cellából abba a szomszédos cellába jutni, amelyiket a céltól való távolság meghatározásakor választottunk ki. Ezt egészen addig folytatja, amíg vagy el nem éri a cél cellát, vagy az aktuális cella GD értéke végtelen nem lesz mely esetben a kiválasztott cellákat kisebb méretűre vágja és újból megpróbálkozik a célba jutással. Indulásként vesszük azt a cellát, amelyik a start koordinátát tartalmazza, majd megpróbálunk áthaladni a következőbe. Ezt oly módon tesszük, hogy az ügynököt az aktuális cella közepébe helyezzük, és utasítjuk, hogy haladjon a két cella közös határolószakaszának közepe felé. Amennyiben akadályba ütközik, úgy az aktuális cellát fölvesszük a szétvágandók közé, a két cella távolságát végtelenre állítjuk és frissítjük a cellák céltól való távolságát is. Ha viszont sikeresen elért a kijelölt pontig, úgy utasítjuk, hogy haladjon a második cella középpontja felé. Ha ez nem sikerül, akkor végrehajtjuk a fent leírtakat azzal a különbséggel, hogy most a második cella kerül be a szétvágandók listájába. Ha az ügynök bármely lépésben akadályba ütközött, úgy az aktuális cella nem változik, ellenkező esetben 40

42 viszont azt a cellát vesszük aktuálisnak, amelyikben az ügynök éppen tartózkodik, és ezt a cellát föl is jegyezzük egy útvonallistába melynek a generálása a célunk. Ha a céltól való távolságok frissítése során az útvonallistában szereplő bármely egymás utáni két cella GD értéke növekszik, úgy addig töröljük a lista elemeit, amíg egy GD érték szerint szigorúan csökkenő sorozatot nem kapunk, majd a legutolsó cellát aktuálissá téve folytatjuk az algoritmust ez biztosítja, hogy már az első esetben megkapjuk az optimális utat a cél felé. Az eredeti algoritmusban az ügynök egyenesen a következő cella közepébe halad, és ha közben akadályba ütközik, akkor jó eséllyel mindkét cella fölbontását elvégzi. Ez azért rossz, mert hozzávetőlegesen megduplázza a karbantartandó cellák listáját, melyek értékeinek frissítése veszi igénybe a legtöbb időt. Ansari és Williams javasolták[1], hogy csak a vesztes oldali (vesztes: az, amelyikből nem vezet út a célba) cellákat bontsuk föl, és változtassuk meg az ügynök célját a két cella határolószakaszának közepére kiküszöbölendő a szomszédos cellák méretbeli különbségéből adódó problémákat. Az ötlet nem volt alkalmazható az alapelvek közti különbségek miatt, de kiindulási alapot jelentett a jelenlegi verzióhoz. Szintén ők javasolták, hogy a fölbontandó cellák listájából csak a legnagyobb cellákat vágjuk ketté, így biztosítva, hogy elakadás esetén az akadály mentén lévő parányi cellák később legyenek felbontva mint azok az óriási cellák, amelyeken keresztül létezik út, csak a jelenlegi diszkretizáció mellet azt még nem találtuk meg ezt az ötletet teljes egészében tudtuk alkalmazni. Ha az iteráció végén a célcellát még nem értük el és van még szétvágható cella, úgy először kiszűrjük azokat a cellákat a szétvágandók listájából, amelyek nem a győztes-vesztes határvonalon fekszenek így nem vezethetnek be új utat a célig és a maradék közül a legnagyobbakat felezzük. Minden újonnan létrejött cellával kapcsolatban újra meghatározzuk a szomszédságokat, töröljük az ezekre a területekre vonatkozó tapasztalatokat és újraszámoljuk a céltól való távolságokat, majd újrakezdjük az egész útkeresést a start koordinátától indulva. Annak ellenére, hogy a teljes újrakezdés miatt sok esetben jó útszakaszok is elvesznek, az eljárás olyan gyorsan visszaépíti a már egyszer bejárt utat, hogy nem láttam értelmét erre külön figyelmet fordítani. A teljes algoritmus kétféleképpen érhet véget: vagy az aktuális cella céltávolsága végtelen és nincs több szétvágható cella és így nem létezik ezzel a minimális cella mérettel út vagy elértük a célcellát, és így triviálisan előállítható az útvonallistából egy, az aktuális diszkretizáció mellett optimális út A cellák céltól való távolságának meghatározása Mivel az egész algoritmus futási idejének java részét a cellák GD értékének számolásával tölti, fontos, hogy ezt lehetőleg minél optimálisabban tegye, így 41

43 szóba sem jöhetett olyan megoldás, amely minden, a cellák szomszédsági viszonyaiban beállt változáskor az összeset törli és újraszámolja a célcellától kiindulva. Ennél gyorsabb módszerre volt szükségünk, így a következő procedúrát alkalmaztuk: Minden cellának van egy UNDEFINED nevű flag-je, melynek igaz értéke esetén a cella GD értéke bizonytalan. Létezik továbbá egy FIFO lista (sor), amely azokat a cellákat tartalmazza, amelyek céltól való távolságát felül kell vizsgálni. A sor ügyel arra, hogy egy elem ne szerepeljen többször benne. LOOP: 1. Levesszük a legfelső elemet a sorból; ha nincs több elem, akkor kilépünk 2. Megvizsgáljuk a cella összes szomszédját, olyat keresve, amelyből biztos út vezet a célba annak eldöntését, hogy létezik-e biztos út, egy egyszerű eljárással végezzük 3. Ha találtunk ilyen cellákat, akkor ezek közül a legkisebb GD+távolság értékűt megtesszük az aktuális cella célhoz vezető következő cellájának és e szerint módosítjuk GD értékét. Újfent megvizsgáljuk a cella összes szomszédját, és ha bármelyikre igaz a következők valamelyike, akkor fölvesszük a felülvizsgálandók sorába, és UNDEFINED flag-jüket beállítjuk: GD értéke nagyobb, mint a jelenlegi cella GD értéke+ a távolságuk következő cellának a jelenlegi cella van megjelölve, és a cella GD értéke nem egyezik a jelenlegi cella GD érték+ a távolságukkal 4. Minden olyan szomszédos cellát fölveszünk a felülvizsgálandók sorába, amelyek UNDEFINED flag-je igaz értékű és a távolságuk kisebb végtelennél Tesztek Mivel a képfeldolgozó rendszer még nem volt olyan állapotban, hogy valós képeket is tudjon produkálni, a tesztek során kénytelen voltam mesterségesen elkészített tesztképekkel/térképekkel dolgozni. A tesztek alatt úgy találtam, hogy az útvonal elkészítése olyan térképeken, amelyek véleményem szerint hasonló tulajdonságokkal rendelkeznek az objektumok sűrűsége és elhelyezkedése szempontjából mint amilyeneket a képfeldolgozó rendszer fog generálni, szinte elhanyagolható idő alatt megtörténik. Lényegesen több időbe telik viszont olyan térképeken navigálni, amelyeken a legkisebb cellamérethez közelítő szűk sikánokon keresztül vezet az út; ezeken az állapottereken akár több másodpercig is eltarthat, míg az eljárás eltalál a célig ezt a kis méretű cellák nagyon nagy száma okozza. 42

44 A 4.8, 4.9 és 4.10 ábrákon látható teszteredmények egy AMD Athlon 1400XP processzoros, 256 Mbyte DDR memóriával ellátott gépen készültek 4 pixeles minimum és 64 pixeles maximum cellamérettel, a bal fölső sarokban lévő start és a jobb alsó sarokban lévő cél koordinátákkal ábra. Várhatóan a valódihoz hasonló térkép: 3,58 ms 4.9. ábra. Nagy bonyolultságú térkép: 49,67 ms ábra. Szűk folyosókat tartalmazó térkép: 3.520,50 ms A generált út átalakítása Az út átalakítása során két fő szempontot kell figyelembe vennünk: azt, hogy a jármű nem képes bármely pozícióból bármely irányba továbbhaladni, továbbá, hogy nem kör alakú, és így nem mindegy, hogy egy pozícióban milyen irányultsággal helyezkedik el. Egy esetleges további szempont lehet, hogy a jármű első kereke nem képes nulla idő alatt az általunk kívánt irányba állni, és így az alatt az idő alatt, amíg a kerék fordul, a kocsi a tervezettől eltérő irányban halad. Ez utolsó szempontot jelenleg nem vesszük figyelembe az útvonal tervezése során, mivel 43