Budapesti Műszaki és Gazdaságtudományi Egyetem Villamosmérnöki és Informatikai Kar
Önálló laboratórium
Idegenvezető robotplatform fejlesztése
Készítette: Ayhan Dániel Forgács Boglárka Timea Juhász Alexandra
Konzulens: Dr. Kiss Bálint
Budapest, 2016
Tartalomjegyzék 1. Bevezetés................................................................................................................................................3 Az idegenvezető robot...................................................................................................................3 A futó projekt.................................................................................................................................3 Feladatspecifikáció ........................................................................................................................3 2. Platform - DaNI robot .............................................................................................................................4 3. Fejlesztői környezet – LabVIEW..............................................................................................................4 4. Útvonaltervező algoritmusok .................................................................................................................5 Vector Field Histogram - vektormező hisztogram .........................................................................5 Röviden az algoritmusról ...............................................................................................................5 Az algoritmus előnyei ....................................................................................................................5 Az algoritmusról részletesebben ...................................................................................................6 A VFF algoritmus - Virtual Force Fields (virtuális erőterek módszere) ..........................................6 A VFH algoritmus ...........................................................................................................................7 A VFH+ algoritmus .........................................................................................................................8 A VFH* agoritmus ..........................................................................................................................9 A VFH algoritmus a LabVIEW-ban..................................................................................................10 Egyszerű vektormező hisztogram VI..............................................................................................10 Advanced VFH................................................................................................................................11 5. A program ...............................................................................................................................................12 6. Helyzetmeghatározás .............................................................................................................................15 Marker alapú helyzetmeghatározás, a kamera külső paraméterei ...............................................15 A gépi látás alapfogalmai ...............................................................................................................16 PnP probléma ................................................................................................................................17 A marker sarokpontjainak keresése ..............................................................................................17 Implementáció, eredmények ........................................................................................................18 7. Forrásjegyzék ..........................................................................................................................................20
2
1. Bevezetés Az idegenvezető robot Napjainkban számos múzeumban alkalmaznak idegenvezető robotokat. Ezen robotok fő feladata a látogatók körbevezetése a kiállító téren és a kiállított tárgyak bemutatása különböző audiovizuális eszközök segítségével. Fontos, hogy a robot rendelkezzen egy olyan pályatervező algoritmussal, mely megtalálja a legoptimálisabb utat a célpontig, figyelembe véve a robot által előre ismert statikus akadályokat (például a nem a célként definiált kiállítási tárgyakat, asztalokat, székeket). Az algoritmusnak tudnia kell kezelni a dinamikus akadályokat is, vagyis azokat az akadályokat, melyek pozícióját előre nem ismeri a robot. Általánosságban ezek az akadályok a múzeum látogatói. Fontos, hogy egy idegenvezető robot rendelkezzen kommunikációs képességekkel, hiszen feladata a kiállítási tárgyak bemutatása. A kommunikáció történhet egyoldalúan - a robot képernyőn keresztül prezentálja az adott kiállítási darabot -, de létrejöhet kétoldali kommunikáció is, amikor a látogatók által feltett kérdésekre is képes válaszolni a robot.
A futó projekt A Budapesti Műszaki Egyetem Irányítástechnika és Informatika Tanszékén már régebbóta fut egy projekt, melynek keretében egy idegenvezető robotplatformot fejlesztenek a hallgatók. A robotnak képesnek kell lennie a tanszéki laborban való navigációra, mozgásra, az akadályok elkerülésére, valamint a kiállított tárgyak bemutatására valamilyen multimédiás eszközzel (hang/videó lejátszás). A robot fejlesztése a National Instruments által gyártott DaNI platformon kezdődött el. Egy volt hallgató munkájának köszönhetően a robot képes odometria alapján dead reckoning módszerrel tájékozódni. Ez az inkrementális útadók és az iránytű jele alapján történő navigációt jelenti. A két szenzor jelének fúzióját Kálmán-szűrővel végzi a robot. Mivel abszolút pozíció meghatározására nem képes, hanem csak relatív elmozdulást tud érzékelni (bár az iránytűvel képes abszolút orientációt meghatározni), ezért hosszú távon a halmozódó hiba miatt navigációra nem alkalmas ez a módszer. Egy egyszerű algoritmussal akadálykerülésre is képes a robot.
Feladatspecifikáció A fent említett projektbe csatlakoztunk be. Az volt a feladatunk, hogy a volt hallgató diplomamunkájában az akadályok kikerüléséért felelős programrészt cseréljük le egy új algoritmusra. Ayhan Dániel feladatul a robot navigációjának újragondolását kapta. Konzulensünkkel közösen a marker-alapú tájékozódást választottuk, mivel Dánielnek korábban ebben volt tapasztalata.
3
2. Platform - DaNI robot Programunkat a National Instruments által készített robotra, az úgynevezett DaNI robotplatformra fejlesztettük. A DaNI robot differenciális hajtású, két, függetlenül
hajtott
tengellyel
rendelkezik.
Mindkét első kerekén enkóder helyezkedik el, ezzel a kerék elfordulása mérhető. Hátul található egy omnidirekcionális kerék, mely a robot
stabilitását
továbbá
biztosítja.
forgatható
Rendelkezik ultrahangos
távolságérzékelővel és digitális iránytűvel is. Az ultrahangos
távolságérzékelő
segítségével
képes az akadályok detektálására. A robot vezérlését egy NI sbRIO kártya végzi, ez egy egykártyás számítógép, amelyen FPGA is található.
3. Fejlesztői környezet - LabVIEW Algoritmusunkat a National Instruments által 1986-ban kifejlesztett LabVIEW (Laboratory Virtual Instrumentation Engineering Workbench) fejlesztői környezetben készítettük el. A LabVIEW egy olyan grafikus programozási nyelv, melyet eredetileg műszerezési célokra hoztak létre. Két felülettel rendelkezik: egy felhasználói felülettel és egy hozzá tartozó blokk-diagrammal. A felhasználói felületen adhatók meg a felhasználó által látott elemek, és a blokkdiagramon történik a tényleges programozás a különböző elemek összekapcsolásával. A nyelv egyszerűbb programoknál szép kódokat eredményez, viszont grafikus voltából kifolyólag bonyolultabb programok esetén könnyen átláthatatlanná válhat. Így fontos szempont programozáskor a program átláthatóságára való törekvés. A LabVIEW kiterjedt dokumentáció helyett nagyszámú mintaprogramot tartalmaz minden toolboxhoz, így a Robotics Modulhoz is. Ezek alapján lehet megismerkedni a fejlesztői környezet működésével. Feladatunk során főként a Robotics Module Starter Kit 2.0 VI készletét használtuk. Itt megtalálhatóak olyan előre beépített függvények, melyek specifikusan a DaNI robotplatformra íródtak. Ennek megfelelően egyszerűen lehet vezérelni a robot kerekeit, meg lehet adni, hogy az ultrahangos
4
távolságérzékelő milyen szögtartományban pásztázza környezetét és milyen távolságban érzékeljen egy tárgyat akadálynak. A Robotics Environment Simulator-nak köszönhetően létrehozhatunk egy olyan szimulációs környezetet, mely pontosan visszaadja a robot valós környezetben való mozgását annyi különbséggel, hogy a szimulált mozgásban nem jelenik meg az odometriából származó hiba.
4. Útvonaltervező algoritmusok Az útvonaltervező algoritmusok lehetnek globálisak vagy lokálisak. Globális útvonalkereső algoritmusok esetén a célpont megtalálása a legfontosabb szempont és többnyire globális optimalitást biztosítanak. A lokális útvonalkereső algoritmusok ezzel szemben nem biztosítanak globális optimalitást, viszont valós idejűek és az elsődleges céljuk a szenzoradatok alapján az ütközések elkerülése. A Vector Field Histogram is egy ilyen lokális tervező algoritmus.
Vector Field Histogram - vektormező hisztogram A Vector Field Histogram (továbbiakban VFH) egy valósidejű pályatervező algoritmus. Két nagyobb módosításon esett át. Az első megjelenése 1991-re tehető: Johann Borenstein és Yoram Koren alkotta meg.
Röviden az algoritmusról: A VFH algoritmus világmodellje egy kétdimenziós hisztogramrács. Ez a hisztogramrács mintavételi időnként frissítődik a robot szenzoraiból érkező adatokkal. A hisztogramrácson kétlépcsős adatredukció hajtódik végre: először egy ú.n. poláris hisztogramra képezzük le: a robot pillanatnyi helyzete körül felosztjuk a teret szögtartományokra, és az egyes tartományokba kerülő érték reprezentálja, hogy mekkora abban az irányban az akadályok sűrűsége. A szögtartományokba összegzés a távolsággal lineárisan csökken. Ezután kiválasztjuk, hogy mely tartományokban van az akadályok sűrűsége egy adott küszöbszint alatt. Ezek lesznek a völgyek, és belőlük kiválasztjuk azt, amelyik irányban legközelebb esik a célhoz, és a robotot ezután ebbe az irányba kormányozzuk. Mivel a VFH a robot környezetét statisztikusan reprezentálja, ezért kezelni tudja a szenzoradatok bizonytalanságából származó hibákat. Az algoritmus számításba veszi a robot alakját és dinamikáját, és így olyan kormányzási utasításokkal tér vissza, mely az adott robotplatformra specifikus.
Az algoritmus előnyei: Legfőbb előnyei közé tartozik a gyorsasága: a robot a szenzoraival folyamatosan mintavételezi a környezetét és az algoritmus ez alapján nagy sebességgel végzi el a számításokat. A gyorsasága mellett előnyös tulajdonsága továbbá a megbízhatósága, főleg ha az akadályok sűrűn helyezkednek el 5
a pályán. Az algoritmus jól alkalmazkodik a pontatlan szenzoradatokhoz is, illetve össze lehet illeszteni egymással a több szenzorból származó adatokat.
Az algoritmusról részletesebben: A valószínűségi rács A valószínűségi rács egy olyan világmodell, melyet az akadályok valószínűségi reprezentációja alkot. Ennél fogva kiválóan alkalmas a szenzoradatok hibáinak korrigálására. A robot munkaterülete fel van osztva cellákra (a sík fel van osztva négyzetekre). Minden cella tartalmaz egy értéket, mely megadja, hogy mekkora valószínűséggel tartózkodik akadály az adott cellában. A valószínűségi rács mintavételi időnként frissítődik a szenzorokból érkező adatokkal. A frissítéskor az algoritmus figyelembe veszi az adott szenzor karakterisztikáját. Ultrahangos érzékelőknél például melyet mi is használtunk - ha az érzékelő azt jelzi, hogy akadályt lát a hatótávolságán belül, akkor a legvalószínűbb, hogy ez az akadály az érzékelő akusztikus tengelyéhez közel esik, így az algoritmus az akusztikus tengely közelébe eső cellákhoz tartozó értékeket nagyobb mértékben növeli, mint az attól távolabb esőket. Mivel az algoritmus az érzékelők által lefedett teljes területet frissíti mintavételenként, ezért nagy a számítási igénye. A VFH nem ezt a világmodellt használja, viszont az is ezen alapul. A hisztogramrács A VFH algoritmus világmodellje egy hisztogramrács. A hisztogramrács abban különbözik a valószínűségi rácstól, hogy minden mintavételkor csak egyetlen cellát frissít, és így az akadályok valószínűségi eloszlását adja meg. Ultrahangos érzékelőknél a frissített cella az akusztikus tengelyre esik, és a robot szenzorától a szenzor által érzékelt akadálytávolságban helyezkedik el. Ha ugyanabban a cellában mintavételekkor többször akadály tartózkodik, akkor az adott cella értéke folyamatosan növekszik, ezért hívják a módszert hisztogramrácsnak.
A VFF algoritmus - Virtual Force Fields (virtuális erőterek módszere) Ez az algoritmus volt a VFH elődje. Ugyanaz a csapat fejlesztette, mint a VFH-t. Az algoritmus világtérképe a hisztogramrács, ebből állítja elő az algoritmus azt az irányt, amelybe a robotot kormányozni kell az akadályok elkerülése érdekében. Ahogy a robot mozog, egy négyzetes ablak együtt mozog vele. Az ablak által kijelölt terület a hisztogramrácson az aktív terület, és a beleeső cellák az aktív cellák. Úgy teszünk, mintha az aktív cellák taszítőerőt fejtetének ki a robotra (ezek a virtuális erők). Ez az erő arányos az adott cellához tartozó valószínűségi értékkel, és fordítottan arányos a robottól vett távolsággal. Közben a célpont egy konstans nagyságú vonzóerőt fejt ki a robotra.A fent ismertetett erőket összegezzük, és a robotot az eredő erő irányába kormányozzuk. Ez a módszer biztosítja, hogy a robot gyorsan tud reagálni váratlanul megjelenő akadályokra.
6
A VFF algoritmusnak vannak problémái, pl. ajtón vagy szűk folyosón való haladáskor. A problémák arra vezethetők vissza, hogy az akadályokról szerzett összes információnkat leegyszerűsítjük egyetlen vektorrá. Ennek orvoslására használ a VFH kétlépcsős adatredukciót.
A VFH algoritmus A VFH algoritmus először 1991-ben jelent meg, majd két jelentős módosítást élt meg, egyet 1998ban, amikor az algoritmus a VFH+ nevet kapta, majd 2000-ben, amikor VFH*-gá nevezték át. A VFH algoritmus, mint fentebb már említésre került, kétlépcsős adatredukciót hajt végre a hisztogramadatokon. A poláris hisztogram előállítása A robottal továbbra is együtt mozog egy ablak. Az ablakba eső aktív cellákat akadályvektoroknak tekintük. Az akadályvektor az adott cellától a robot felé mutat, nagysága pedig az adott cellához tartozó valószínűségi érték négyzetével arányos, és robottól vett távolsággal csökken. A robot körüli terület n darab szögtartományra van osztva, α felbontással (n=360°/α).
Az egyes szektorokhoz tartozó ún. poláris akadálysűrűség az aktív ablak adott irányba eső akadályvektorainak összegeként áll elő. Az ábrán szépen látszik a koncepció: ahol akadály van (fal, stb.), a poláris akadálysűrűség nagyobb, ahol nincs, ott kisebb. A poláris hisztogramra még alkalmaznak egy simítófüggvényt, hogy ne legyenek benne nagy ugrások. A második adatredukciós lépés Ebben a lépésben előáll az a θ irány, amelybe a robotot kormányozni kell az akadály elkerülése érdekében. A fenti ábrán egy tipikus poláris hisztogram látható: vannak csúcsai (amely irányokban nagy az akadálysűrűség) és völgyei (ahol alacsony az akadálysűrűség).
Azokat a völgyeket, ahol az
akadálysűrűség egy adott küszöb alatt van, jelöltvölgyeknek (candidate valleys) nevezzük. Ilyen jelöltvölgyből általában több is van. Az algoritmust azt választja közülük, amelyik irányban a legközelebb esik a célkoordinátához, és a robotot a kiválasztott völgy közepe felé irányítja.
7
A kiválasztott völgy lehet széles vagy keskeny. Széles völgyek akkor adódnak, ha nagy a távolság az akadályok között, vagy csak egyetlen akadály van a robot környezetében. Keskeny völgyek akkor keletkeznek, ha a robot olyan területen halad keresztül, ahol az akadályok sűrűn helyezkednek el. A program teljesítménye a küszöbszint megválasztására bizonyos határokon belül érzéketlen. Pontosabb meghatározásra csak olyan esetekben lehet szükség, ha gyorsan és sűrűn akadályokkal ellátott területen akar áthaladni a robot, problémák csak túl nagy elállításoknál vannak. Ha a küszöbszint túl nagy, a robot túlságosan megközelítheti az akadályt, ami ütközéshez vezethet. Ha túl alacsony, akkor az algoritmus bizonyos utakra nem fog jelöltvölgyként tekinteni, így szűk átjárókon a robot nem fog áthaladni. Az algoritmusban a robot maximális sebessége futás előtt állítható be. Az algoritmus ezt a sebességet csak akkor csökkenti, ha nagyobb irányváltoztatás szükséges egy akadály miatt.
A VFH+ algoritmus A VFH+ algoritmusban eszközölt módosítások növelték az algoritmus megbízhatóságát, a keletkező útvonal simaságát. Az algoritmus bemenete továbbra is egy hisztogramrács. Ezután az algoritmus négylépcsős adatredukciót hajt végre, hogy végeredményként megkapjuk a robot új irányát. Az első három lépés a poláris hisztogramot állítja elő, a negyedik lépésben pedig az irány kiválasztása történik. Itt már egy ún. maszkolt poláris hisztogramot és egy költségfüggvényt is figyelembe vesz az algoritmus. Első lépcső: A hisztogramrács leképződik az elsődleges poláris hisztogramra. Itt a robottal együttmozgó ablak már kör alakú. Ezen kívül annyi eltérés van a korábban a VFH-nál ismertetett módszertől, hogy itt az algoritmus expliciten figyelembe veszi a robot szélességét. Ehhez az algoritmus megnöveli az akadályok méretét a hisztogramrácsban a robot sugarával (ez alatt azt a távolságot kell érteni, ami a robot középpontjától annak legtávolabbi pontjához mutat). Sőt, a még nagyobb biztonság érdekében nem csak a robot sugarával, hanem a robot sugarának és a robot és az akadály közt szükséges minimális távolságnak az összegével növeli meg az akadályok méretét. Ez a módszer azért jó, mert ezután a robotot pontszerű objektumnak lehet tekinteni. Itt a VFH-ban nem csak egyetlen cella értéke növekszik mintavételenként, hanem az összes, a megnövelt akadályhoz tartozó cella értéke. Így egy olyan poláris hisztogram keletkezik, ami a robot szélességét is figyelembe veszi. Második lépcső: a bináris poláris hisztogram A VFH algoritmusban a fix küszöbérték probémát jelent keskeny völgyek esetén, mivel előfordulhat, hogy a szenzoradatok bizonytalansága miatt az algoritmus egyszer jelöltvölgynek tekinti a fenti völgyet, és a robotot abba az irányba kormányozza, máskor pedig nem tekinti jelöltvölgynek, és ekkor egy másik irányba kormányozza a robotot. Emiatt a robot iránya a két völgy iránya között alternálhat.
8
Ez a probléma a hiszterézises küszöbérték használatával küszöbölhető ki: van két küszöbérték, egy alsó és egy felső. A bináris poláris hisztogram a valószínűségértékek helyett azt tartalmazza, hogy az adott irány szabad (0) vagy blokkolt (1)-e. Az irány szabad, ha a hozzá tartozó valószínűségi érték az alsó küszöb alatt van, és blokkolt, ha ez az érték a felső küszöb felett van. Harmadik lépcső: a maszkolt poláris hisztogram Az eredeti VFH algoritmus nem veszi figyelembe a robot dinamikáját és kinematikáját, azaz helytelenül feltételezi, hogy a robot bármely pillanatban tetszőlegesen irányt változtathat. A VFH algoritmus ezt úgy veszi figyelembe, hogy meghatározhatunk egy maximális irányváltoztatási szöget, melyre a robot képes. Egy adott időpillanatban az ezen a szögtartományon kívül eső területeket blokkoltnak tekintjük. Ezen adatok ismeretében módosítjuk a bináris poláris hisztogramot: már csak azokat az irányokat tekintjük szabadnak, melyekre egyszerre igaz az, hogy a hozzájuk tartozó valószínűségi érték az alsó küszöbszint alatt van, és az, hogy a robot a dinamikájából és kinematikájából adódóan képes abba az irányba fordulni. Negyedik lépcső: az új irány kiválasztása Az eredeti VFH algoritmus a völgy méretétől függetlenül azt választotta, amelyik a legközelebb esik a célkoordinátához. A VFH+ ezzel szemben alkalmaz egy költségfüggvényt is: ez a függvény nem csak azt veszi figyelembe, hogy mekkora a jelöltvölgy távolsága a céltól. Itt is megkülönböztetünk keskeny és széles völgyeket. Keskeny völgyek esetén a robotot a völgy közepe felé kormányozzuk, míg széles völgy esetén a kormányzás iránya lehet a völgy bal oldala, jobb oldala vagy közepe. Így egy adott völgy esetén egy vagy három kormányzási irány jöhet szóba. Egy adott maszkolt poláris hisztogramban megkeressük ezeket az irányokat (ezek az előzetes jelöltirányok), és utána alkalmazzuk rájuk a költségfüggvényt. A költségfüggvény három szempontot vesz figyelembe: mekkora a különbség az adott irány és a célhoz vezető irány között; mekkora a különbség a robot jelenlegi iránya és az adott irány között; illetve mekkora a különbség az adott irány és az algoritmus korábbi meghívásakor kiválasztott irány között. Minél inkább az első tagot vesszük figyelembe, annál célorientáltabb lesz a robot; a második tag növelésével nő a pálya simasága, a harmadik tag növelésével pedig a motoroknak szóló parancsokban csökkennek az ugrások. Az algoritmust azt az irányt választja, amelyiknél a költségfüggvény a legkisebb értéket vette fel.
A VFH* agoritmus A VFH és a VFH+ lokális útvonaltervező algoritmus. A lokáis útvonaltervező algoritmusokkal az az elsődleges probléma, hogy a kiválasztott útvonalnak csak a pillanatnyi hatását veszik figyelembe. A VFH* algoritmus ezt a problémát oldja meg azáltal, hogy kombinálja a VFH+-t az A* algoritmussal. A VFH* algoritmus ott tér el a VFH+-tól, hogy az előzetes jelöltirányok meghatározása után megvizsgálja, hogy mi lesz az egyes irányok választásának a következménye: minden ilyen irányhoz az 9
algoritmus megállapítja a robot új pozícióját és orientációját, majd ismét lefut a VFH+ algoritmus, ahhoz az irányhoz létrehoz egy új poláris hisztogramot, és ennek is minden irányát a fentiekhez hasonlóan megvizsgálja. Így, ha ezt a folyamatot n-szer megismételjük, keletkezik egy n mélységű keresési fa. A keresési fa csomópontjainak értéke az odavezető útvonal költségének az összege, melynek részei a VFH+ algoritmus futtatásakor kerülnek meghatározásra. A VFH* az A* algoritmust használja a legrövidebb útvonal megtalálására a keresési fában.
A VFH algoritmus a LabVIEW-ban A LabVIEW programban két fajta Vector Field Histogram van reprezentálva: a Simple VFH és az Advanced VFH. A lényeges különbség a kettő között az, hogy az Advanced VFH-ban megadhatóak a célkoordináták, mely a programunk szempontjából lényeges. Mindkét beépített VI elérési útja a következő: Functions -> Robotics -> Obstacle Avoidance. Az alábbiakban a két VI tulajdonságait, legfontosabb bemeneteit és kimeneteit részletezzük.
Egyszerű vektormező hisztogram VI
Panic range (Pániktávolság): megadja, hogy milyen távolságra és milyen irányban kell lennie az akadálynak ahhoz, hogy a robot elkezdje a kikerülési manővert. Distances: a robot szenzora és az akadályok közti távolságokat kapja meg egy tömbben. Direction angles (irányszögek): megadja a tárgynak a szenzor középpontjához viszonyított szögét. A pozitív érték azt jelenti, hogy a tárgy a szenzor közepétől jobbra van, a negatív érték pedig azt, hogy balra. Distance threshold (távolságküszöb): megadja azt a távolságot, melynél a VI még nem gondolja a tárgyat akadálynak. Obstacle within panic range (akadály van a pániktávolságon belül): IGAZ értékkel tér vissza, ha egy tárgy egyszerre van a pánik küszöb szögön belül és a pániktávolságon belül. Largest gap (legnagyobb rés): megadja a legnagyobb nyílt területet a robot környezetében. Nearest obstacle (legközelebbi akadály): megadja a szenzorhoz legközelebb lévő tárgy távolságát és irányát. A hisztogram szög és irány szerint rendezve reprezentálja a tárgyak távolságát a szenzor hatótávolságán belül. 10
Advanced VFH
Obstacle clearance distance (akadálymentes távolság): meghatározza azt a minimális távolságot, amellyel a robot közepe megközelíthet egy akadályt. Heading (irány): meghatározza egyrészt azt az irányt, melyre a robot mutat, illetve melynél a célpozíció elhelyezkedik. Distances (távolságok): megkapja egy tömbben a távolságokat a robot szenzora és az érzékelt akadályok között. Direction angles (irányszögek): megkapja az akadályok irányát a robot szenzorának közepéhez viszonyítva. Threshold (küszöb): megadja, hogy a VI milyen távolság esetén ítélje a tárgyat kikerülhetetlennek. Megadható belső és külső küszöbérték. Ha az akadály közelebb van a robothoz, mint a belső küszöbérték, akkor az algoritmus ezt az irányt blokkoltnak tekinti. Ez az irány csak akkor lesz újra szabad, ha az érzékelőből származó adat szerint az akadály a külső küszöbértéken kívül van. Ezzel véd az algoritmus az ellen, hogy a szenzorhibák miatt a robot mozgása oszcilláljon. Maximum heading change (maximális irányváltozás): megadja, hogy mekkora lehet a robot maximális irányváltoztatása, ha azt az irányt, amelyben a robot éppen áll, elzárja egy akadály. Az irányváltozást limitálni kell aszerint, hogy a robot szenzorai mekkora szögtartományban szolgáltatnak adatot. Alapértelmezett értéke 1,5708. Chosen heading (választott irány): a VI válaszként megad egy irányt, melybe a robotot kormányozni kell. Az irányt a célkoordináták és a robot környezetében lévő akadályok alapján számítja ki.
11
5. A program Mivel a LabVIEW-ban való fejlesztés erősen támaszkodik a mintaprogramok használatára, mi is mintaprogramokra alapoztuk a munkánkat. Legnagyobbrészt két mintaprogramot használtunk: -
Simulated Mecanum Robot
-
Starter Kit 2.0 Dead Reckoning
Az első programban az Advanced VFH VI van implementálva, a második programban pedig a DaNI robot szimuláicós környezete található. Azt tűztük ki célul, hogy a Starter Kit robothoz tartozó szimulációs környezethez (mely pl. a Starter Kit 2.0 Dead Reckoning mintaprogramban szerepel) illesztjük a Simulated Mecanum Robot mintaprogramban is implementált VFH algoritmust. A programunk még fejlesztésre szorul, ill. nem sikerült illeszteni a volt hallgató fent említett diplomamunkájába. A program fő problémája, hogy valahol hibáztunk az odometriai adatok meghatározásánál, ezért a program által érzéket θ szög nagymértékben eltér a robot valódi elfordulási szögétől, emiatt nem a kívánt helyre megy a robot. A program felhasználói felületén található egy térkép, melyen a robotot - melynek pozícióját dead reckoninggal becsüljük - piros háromszög jelzi. Ezen a térképen sárga körrel megadható a célkoordináta. A célkoordináta a programban futás közben is változtatható. A felhasználói felület:
A programunk a LabVIEW terminológiát használva Sense, Think and Act (érzékelés, döntés és beavatkozás) részekre különíthető el. A program több hurokból áll, ezek egymással megosztott változókon keresztül kommunikálnak.
12
A program legfőbb problémája a helymeghatározásból származik. Az érzékelt elfordulási szög sokkal nagyobb, mint a robot tényleges elfordulása. Ez nem a dead reckoningból származó hiba, hanem valami más jellegű probléma, melyet nem tudtunk megoldani. Ezen felül a robot mozgása nem túl természetes, viszont az akadályokat kikerüli, és akadály észlelésekor nem áll meg, csak lelassul, illetve tolat. Az alábbi programrészlet meghívja a környezetet. Folyamatosan mozgatja az ultrahangos érzékelőt, és az általa érzékelt adatokból előállítja a distances és az angles nevű tömböket, melyek mutatják, hogy milyen szögben milyen távolságban lát a robot akadályt. Ezeket az adatokat a felhasználói felületen grafikonon meg is jeleníti. Ez a programrész a Starter Kit 2.0 Dead Reckoning mintaprogramból származik. Az Estimate Posture VI-jal meghatározza a robot helyzetét dead reckoninggal. Ezt az estimated_posture megosztott változóban tároltuk el. Itt történik továbbá a Steering Frame-en keresztük a motorok sebességének írása.
A program további részei a Simulated Mecanum Robot mintaprogramból származnak. A következő programrész a felhasználói felületen látható térképet jeleníti meg, melyen piros háromszög ábrázolja a robotot, és sárga körrel jelölhető meg a cél. A célkoordináták beolvasását a User Interface Event Handler event struktúra végzi.
13
A következőkben inicializáljuk az odometriai adatokat, valamint a kiadott sebességet és szöget (ez utóbbi adatokkat vezéreljük a steering frame-en keresztül a robot motorjait). A szimulációs hurok a robot szögén (Heading_Cmd) lévő PID szabályzó alkalmazásához szükséges.
Végül pedig itt látható az Advanced VFH függvény meghívása. A distances és angles megosztott változók az ultrahangos érzékelő által érzékelt adatokat tartalmazzák, a VX_Cmd, VY_Cmd és Heading_Cmd változókba pedig a VFH függvény által előállított kívált sebességértékek.
14
6. Helyzetmeghatározás Marker alapú helyzetmeghatározás, a kamera külső paraméterei A marker olyan, általában 2D-s minta, amely gépi látással jól elkülöníthető, alkalmas információ kódolására és/vagy pozicionálásra. Elhelyezhető például marker magán a roboton, amelyet külső kamerával vagy kamerarendszerrel figyelve háromszögeléssel meghatározható a robot pozíciója. A környezeten is elhelyezhető marker, így a megfigyelő rendszer a roboton magán helyezkedik el.
2. ábra (forrás: youtube.com)
1. ábra (forrás: youtube.com)
A fenti ábrákon példákat látunk gépi látással történő helyzetmeghatározásra. A bal oldali képen lévő quadrokopteren három fehér gömb található, ezt érzékeli a külső kamerarendszer. A jobb oldali képen a Boston Dynamics Atlas robotja látható, amelyik marker alapú azonosítást és pozícionálást végez, ám a kamera a roboton található. A mi alkalmazásunkban az utóbbi megoldást választottuk. Feltételezzük tehát, hogy a kamera a robothoz képest nem mozdul el, így a robot pozíciója és orientációja a kamera hasonló paramétereinek ismeretéből számítható egy ismert (kalibráció során meghatározott) transzformáció alapján.
15
A gépi látás alapfogalmai A probléma jól ismert a képfeldolgozás területén. A kamera külső paramétereit kell meghatározzuk. Az algoritmusok áttekintése előtt ismerkedjünk meg a kameraparaméterek meghatározásakor használatos fogalmakkal. A kamerát úgynevezett pinhole-kameramodellel írjuk le. Ez a modell azt feltételezi, hogy a kép keletkezése olyan módon történik, hogy a háromdimenziós teret perspektivikusan vetítjük a képsíkra.
3. ábra (forrás: http://openmvg.readthedocs.io/en/latest/_images/pinholeCamera.png)
A kamerához definiálható egy kamera koordináta-rendszer. Ennek a koordináta-rendszernek az origójának a vetítés középpontját tekintjük, a z tengelye merőleges a vetítés síkjára. A kép koordináta-rendszer síkbeli koordináta-rendszer, a síkra vetített pontokat értelmezzük ebben. x és y tengelyei a kamera koordináta-rendszer tengelyeivel egyirányúak. Amennyiben homogén koordinátákkal jellemezzük a tér pontjait, mind a perspektív vetítés, mind a világ koordináta-rendszer és a kamera koordináta-rendszer közötti eltolás, forgatás mátrixszal kifejezhető:
A vetítés mátrixát kameramátrixnak hívjuk, a paramétereit pedig a kamera belső paramétereinek nevezzük. Mivel ezek a kamera rendeltetésszerű használata során nem változnak jelentősen, a kamera szerves részei, egyszeri kalibrálással meghatározhatók. A koordináta-rendszerek közti transzformáció paraméterei a külső paraméterek. Az összes paraméter meghatározható kellően nagy számú pontpár segítségével. A pontpárok alatt a világ koordináta-rendszerben ismert pozíciójú pontokat és azok kép koordináta-rendszerben megadott vetületét értjük. 16
A kameramátrixot a feladat megoldása során ismertnek feltételeztük, hiszen az időben állandónak tekinthető. A lényegi probléma a marker nevezetes pontjainak megkeresése a képen, és azok abszolút (világ koordináta-rendszerben értelmezett) koordinátáinak ismeretében a külső paraméterek meghatározása. A valóságban a pinhole kameramodell sajnos nem állja meg a helyét, a képet többféle torzítás terheli például abból adódóan, hogy nem vetítést alkalmazunk, hanem lencserendszert, valamint, hogy a detektoron elhelyezkedő pixelek gyártástechnológiája nem tökéletes. ezeket a paramétereket most elhanyagoljuk, így a fenti összefüggésben a 𝛾 paraméter 0 lesz.
PnP probléma Amennyiben a külső paramétereket pontpárok segítségével kívánjuk meghatározni, a feladatot a szakirodalomban PnP, azaz Perspective-n-Point problémának nevezik. A legtöbb algoritmus abból indul ki, hogy a kameramátrix már ismert, ezt ezért mi is feltételezhetjük. Egyik klasszikus algoritmus a P3P algoritmus, amely csak 4 pontpárból számítja a paramétereket. Három pont kell ahhoz, hogy véges sok egyértelműen megoldást kapjunk (négy ponttal már túlhatározott lenne a feladat). A negyedik pontot az algoritmus arra használja fel, hogy a véges sok megoldás közül a helyeset kiválassza. Ezt úgy teszi meg, hogy a lehetséges négyféle megoldás közül azt választja, amelyikkel a negyedik pontot a képre vetítve a legkisebb hibát kapjuk a megadott képi ponthoz képest. Ezt aztán tetszőleges optimumkereséssel tovább lehet finomítani, pl. LevenbergMarquardt eljárással. Ezt az eljárást használhatjuk akkor is, ha több, mint négy pont áll rendelkezése. Az algoritmus OpenCV könyvtábeli implementációját használtuk a megvalósítás során. Ez az implementáció mind a P3P algoritmust, mind a Levenberg-Marquardt-féle iteratív megoldást tartalmazza. Megadható neki továbbá a paraméterek becsült értéke, ezzel az iteratív algoritmust gyorsíthatjuk.
A marker sarokpontjainak keresése Az általunk használt marker egy 7x7-es rácsból áll. A rács négyzetei lehetnek feketék vagy fehérek. A rács széle körben végig fekete, a belső 5x5-ös területet információtárolásra használhatjuk. [TODO kép] A feladat tehát a négyzet sarokpontjainak megtalálása. A problémának többféle megközelítése létezik, lehet feature extraction technikával, sarokkereséssel próbálkozni. Mivel a perspektív vetítés egyenességtartó, ezért mi a problémát egyenesek keresésére vezettük vissza. Ebben szerepet játszott, hogy a LabVIEW környezet Vision modulja által nyújtott lehetőségek ilyen téren korlátozottak, a modul nem általános képfeldolgozási feladatokra lett tervezve. A Vision modul egyik példaalkalmazásából indultunk ki, amely egyeneseket keres egy adott tartományon. A tartományt párhuzamos egyenesek mentén vizsgálja, minden egyenes mentén egy egydimenziós élkeresést hajt végre (vagyis megkeresi a gradiens maximális értékét), és az így kapott pontokra próbál egyenest illeszteni különböző módszerekkel.
17
Ha az élek képi helyzetére van egy becslésünk, akkor a keresési tartományt beállíthatjuk úgy, hogy tartalmazza a becslés környezetét. Az algoritmus implementációjakor feltételeztük, hogy ilyen becsléssel rendelkezünk. Egyébként amennyiben kellően gyors algoritmusunk van, a marker előző megtalált pozíciója is egy jó becslés lehet, lévén, hogy a robot annyit nem mozdult el, hogy a keresési tartományból kimozduljon a marker képe. A robot többi szenzora alapján dead reckoning módszerrel becsült pozíció és orientáció segítségével ennél jobb becslést is tudunk adni.
4. ábra
A képen a marker sarka látható, és az élkeresés folyamatát szemlélteti. Narancssárgával a becslés által meghatározott egyenesek láthatók. Ezek láthatóan pontatlanok. Ezek környékén keressük a valódi éleket. A "környékük" alatt az ábrán zölddel jelölt, őket befoglaló téglalapokat értjük. A megtalált egyenesek pirossal vannak jelölve, és látszik, hogy valóban a marker sarokpontjánál metszik egymást.
Implementáció, eredmények A szotfver végső soron a National Instruments myRIO kártyáján fog futni, ugyanis a DaNI robothoz nem csatlakoztatható USB-s kamera. Mivel a myRIO programozása leginkább LabVIEW környezetben támogatott, ezért a szoftver nagy részben LabVIEW-ban lett implementálva. A modul (VI) bemenete [TODO]. A kimenete a marker alapján számított pozíció, illetve, hogy egyáltalán sikerült-e megtalálni a markert. A kamera képét a Vision modullal dolgozzuk fel, az előző fejezetben ismertetett módon. A megtalált sarokpontokat ezek után átadjuk az OpenCV-s implementációnak (SolvePnP() függvény). Az OpenCV-s kódot, mivel C++-ban íródott, DLL-be kellett fordítani, és így tudjuk meghívni LabVIEW-ból. Jelen állapotában a szoftver képes egy adott, kellően jó becslés alapján meghatározni a marker helyzetét a képen és az alapján a kamera külső paramétereit. Az nincs megoldva, hogy kezdeti becslést honnan nyerünk, de erre jó megoldás lehet, hogy a robotot mindig ugyanabból a pozícióból indítjuk. Sajnos a szoftver még nem fut a végleges hardveren (a myRIO kártyán), egyelőre csak PC-s környezetben működik. 18
A kész tesztfelületet az alábbi ábra szemlélteti. A felület ki tudja jelezni, hogy a detektálás sikeres volt-e, illetve mik a kamera külső paraméterei. Az x, y, z paraméterek a transzlációs, az r1, r2, r3 paraméterek pedig a rotációs (jelen esetben RPY) szögeket jelölik.
5. ábra
19
7. Forrásjegyzék Martin Hirzer: Marker Detection for Augmented Reality Applications http://lrs.icg.tugraz.at/pubs/hirzer_tr_2008.pdf Francesc Moreno-Noguer Vincent Lepetit Pascal Fua: Accurate Non-Iterative O(n) Solution to the PnP Problem https://infoscience.epfl.ch/record/179767/files/top.pdf Gao, Xiao-Shan; Hou, Xiao-Rong; Tang, Jianliang; Cheng, Hang-Fei: Complete Solution Classification for the Perspective-Three-Point Problem http://www.mmrc.iss.ac.cn/~xgao/paper/ieee.pdf http://iplimage.com/blog/p3p-perspective-point-overview/ Finding optimal rotation and translation between corresponding 3d points http://nghiaho.com/?page_id=671 https://github.com/Itseez/opencv/tree/master/modules/calib3d/src http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html# solvepnp http://docs.opencv.org/2.4/doc/tutorials/calib3d/camera_calibration/camera_calibration.html
http://zone.ni.com/reference/en-XX/help/372983C-01/lvrobovi/simple_vfh/ http://zone.ni.com/reference/en-XX/help/372983B-01/lvrobovi/advanced_vfh/ https://en.wikipedia.org/wiki/Vector_Field_Histogram Johann Borenstein, Yoram Koren: The Vector Field Histogram – Fast Obstacle Avoidance for Mobile Robots http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=88137 Iwan Ulrich, Johann Borenstein: VFH*: Local Obstacle Avoidance with Look-Ahead Verification Iwan Ulrich, Johann Borenstein: VFH+: Reliable Obstacle Avoidance for Fast Mobile Robots http://www.ni.com/white-paper/11564/en/ https://www.me.utexas.edu/~longoria/CyVS/Lab3/index.html#Ex2 http://download.ni.com/pub/devzone/epd/mobile_robotics_experiments.pdf http://web.eecs.utk.edu/~leparker/Courses/CS594-fall08/Lectures/Oct-23-ObstAvoidII+Architectures.pdf
20
