Za´padoˇceska´ univerzita v Plzni Fakulta aplikovany´ch vˇed Katedra informatiky a vy´poˇcetn´ı techniky
Bakal´ aˇ rsk´ a pr´ ace Robot Kinbot
Plzeˇ n 2014
Michal Bratner
Prohl´ aˇ sen´ı Prohlaˇsuji, ˇze jsem bakal´aˇrskou pr´aci vypracoval samostatnˇe a v´ yhradnˇe s pouˇzit´ım citovan´ ych pramen˚ u. V Plzni dne 5. kvˇetna 2014 Michal Bratner
Podˇ ekov´ an´ı Chtˇel bych podˇekovat Ing. Petru Vanˇeˇcekovi, Ph.D. za ochotu, uˇziteˇcn´e rady, trpˇelivost a ˇcas vˇenovan´ y pˇri konzultac´ıch, kter´e mi pomohly k lepˇs´ımu vypracov´an´ı t´eto pr´ace.
Anotace Tato bakal´aˇrsk´a pr´ace se zab´ yv´a vyuˇzit´ım senzoru Microsoft Kinect pro ovl´ad´an´ı robota Kinbo. C´ılem pr´ace je vytvoˇrit s pomoc´ı platformy Kinbo2 moduly pro ovl´ad´an´ı robota a interakci s ˇclovˇekem. V pr´aci je uveden popis urˇcen´ı pozice robota v prostoru za pomoci QR k´od˚ u. D´ale je pops´ana interakce s ˇclovˇekem a reakce na r˚ uzn´a gesta. Souˇca´st´ı pr´ace je tak´e navigaˇcn´ı ˇca´st pro pohyb robota v prostoru. Na z´avˇer je uveden popis struktury v´ ysledn´e pr´ace.
Abstract This bachelor thesis deals with using Microsoft Kinect sensor to control Kinbo robot. Objective of this thesis is to create a patform Kinbo2 using modules for control robot and human interaction. The work describes the determination of the robot position in space with help of QR codes. Further describes human interaction and reaction to different gestures. The work also includes the navigation part for the robot movement in space and concludes with a description of the structure of the final work.
Obsah ´ 1 Uvod 2 Pouˇ zit´ e prostˇ redky 2.1 Platforma Kinbo2 . . 2.2 Navigaˇcn´ı k´ody . . . 2.3 Knihovna ZXing.net 2.4 Microsoft Kinect . .
1
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
. . . .
3 Urˇ cen´ı pozice robota 3.1 V´ ypoˇcet souˇradnic pomocn´eho bodu 3.2 V´ ypoˇcet vzd´alenosti bodu od senzoru 3.3 Nalezen´ı pr˚ useˇc´ık˚ u kruˇznic . . . . . . 3.4 Urˇcen´ı spr´avn´eho pr˚ useˇc´ıku . . . . . 3.5 V´ ypoˇcet u ´hlu natoˇcen´ı robota . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
6 . 6 . 7 . 9 . 14 . 15
. . . . . . . . . .
17 17 17 17 18 18 18 18 19 19 19
. . . . . . . . . .
. . . .
2 2 3 4 4
. . . .
4 Navigace robota 4.1 Hled´an´ı k´odu . . . . . . . . . . 4.1.1 Rozhl´ednut´ı . . . . . . . 4.1.2 Pˇresun . . . . . . . . . . 4.1.3 Objeven´ı pˇrek´aˇzky . . . 4.1.4 Nalezen´ı k´odu . . . . . . 4.2 Posun k c´ıli . . . . . . . . . . . 4.2.1 Natoˇcen´ı . . . . . . . . . 4.2.2 Pohyb vpˇred . . . . . . . 4.2.3 Objeven´ı pˇrek´aˇzky . . . 4.2.4 Dokonˇcen´ı pohybu vpˇred
. . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
. . . . . . . . . .
5 Interakce s ˇ clovˇ ekem 20 5.1 Rozpozn´an´ı gesta . . . . . . . . . . . . . . . . . . . . . . . . . 21 5.2 Implementovan´a gesta . . . . . . . . . . . . . . . . . . . . . . 22 5.2.1 Vstupn´ı gesto . . . . . . . . . . . . . . . . . . . . . . . 22
OBSAH
5.3
OBSAH 5.2.2 Gesta pro otoˇcen´ı vpravo a vlevo . . . . . . . . . . . . 23 5.2.3 V´ ystupn´ı gesto . . . . . . . . . . . . . . . . . . . . . . 24 Sledov´an´ı ˇclovˇeka . . . . . . . . . . . . . . . . . . . . . . . . . 24
6 Struktura syst´ emu 6.1 Pouˇzit´e zpr´avy . . . . . . . . . . . . . 6.1.1 Zpr´ava se souˇradnicemi . . . . . 6.1.2 Zpr´ava o chybˇe . . . . . . . . . 6.1.3 Zpr´ava pro motory . . . . . . . 6.1.4 Zpr´ava o dosaˇzen´ı c´ıle . . . . . 6.1.5 Zpr´ava o chybn´ ych souˇradnic´ıch 6.1.6 Ukonˇcovac´ı zpr´ava . . . . . . . 6.2 Syst´em serveru . . . . . . . . . . . . . 6.3 Syst´em klienta . . . . . . . . . . . . . . 6.4 Syst´em robota . . . . . . . . . . . . . . 6.5 Propojen´ı modul˚ u. . . . . . . . . . . .
. . . . . . . . . . .
. . . . . . . . . . .
. . . . . . . . . . .
. . . . . . . . . . .
. . . . . . . . . . .
. . . . . . . . . . .
. . . . . . . . . . .
. . . . . . . . . . .
. . . . . . . . . . .
. . . . . . . . . . .
. . . . . . . . . . .
. . . . . . . . . . .
. . . . . . . . . . .
25 25 25 26 26 27 27 27 27 28 29 30
7 Popis bˇ ehu programu
32
8 Dosaˇ zen´ e v´ ysledky
34
9 Z´ avˇ er
35
A Pˇ r´ıloha 39 A.1 Uˇzivatelsk´a pˇr´ıruˇcka . . . . . . . . . . . . . . . . . . . . . . . 39
5
´ 1 Uvod Modul´arn´ı platforma Kinbo2 poskytuje jednoduch´e a snadno rozˇsiˇriteln´e rozhran´ı pro programov´an´ı stejnojmenn´eho robota vybaven´eho senzorem Microsoft Kinect. Modul´arnost platformy zajiˇst’uje jej´ı snadnou rozˇsiˇritelnost a pouˇzitelnost s r˚ uzn´ ymi druhy robot˚ u. Architektura platformy je rozdˇelena na tˇri samostatn´e syst´emy pro server, klienta a robota. Toto rozdˇelen´ı je provedeno z d˚ uvodu bˇehu jednotliv´ ych syst´em˚ u na r˚ uzn´ ych poˇc´ıtaˇc´ıch. Syst´emy pak mezi sebou komunikuj´ı pomoc´ı poˇc´ıtaˇcov´e s´ıtˇe. Kaˇzd´ y syst´em je kv˚ uli zabezpeˇcen´ı pˇred chybami zp˚ usoben´ ymi uˇzivatelsk´ ym projektem rozdˇelen do samostatn´ ych proces˚ u platformy a uˇzivatelsk´eho projektu. C´ılem t´eto pr´ace je navrhnout a implementovat moduly pro jednotliv´e syst´emy, kter´e umoˇzn´ı robotovi Kinbo zjistit aktu´aln´ı pozici v prostoru a umoˇzn´ı mu tak´e interakci s ˇclovˇekem. Aby bylo moˇzn´e moduly pouˇz´ıvat pomoc´ı platformy Kinbo2 , mus´ı b´ yt vytvoˇreny jako dll knihovny v programovac´ım jazyce C#. Po pˇreˇcten´ı pr´ace z´ısk´ate pˇrehled o postupech, kter´e jsou vyuˇziteln´e pro dosaˇzen´ı c´ıle t´eto pr´ace. V kapitole 3 je pops´an zp˚ usob nalezen´ı pozice robota, kapitola 4 pak popisuje rozhodov´an´ı robota o n´asleduj´ıc´ım kroku. Kapitola 5 pojedn´av´a o zp˚ usobu interakce robota s uˇzivatelem pomoc´ı senzoru Microsoft Kinect a kapitoly a popisuj´ı v´ yslednou strukturu a ˇcinnost projektu vytvoˇren´eho v t´eto pr´aci.
1
2 Pouˇzit´e prostˇredky Pˇri vytv´aˇren´ı t´eto pr´ace jsem vyuˇzil nˇekolik jiˇz existuj´ıc´ıch produkt˚ u, jejichˇz funkcionalita je vhodn´a pro c´ıl t´eto pr´ace, a sice ovl´ad´an´ı robota Kinbot.
2.1
Platforma Kinbo2
Hlavn´ım u ´ˇcelem t´eto platformy je poskytnut´ı jednoduch´eho a rozˇsiˇriteln´eho rozhran´ı, jehoˇz pomoc´ı lze ovl´adat mobiln´ıho robota s tankov´ ym podvozkem vybaven´eho zaˇr´ızen´ım Microsoft Kinect. Jak je uvedeno v [1], architektura platformy je rozdˇelena na tˇri syst´emy: server, klient a robot (viz obr. 2.1). Rozdˇelen´ı je provedeno z d˚ uvodu, ˇze kaˇzd´ y syst´em je urˇcen pro bˇeh na jin´em poˇc´ıtaˇci. Syst´emy spolu tedy musej´ı komunikovat prostˇrednictv´ım poˇc´ıtaˇcov´e s´ıtˇe. Kaˇzd´ y syst´em je kv˚ uli zabezpeˇcen´ı pˇred rizikem chyb zp˚ usoben´ ych uˇzivatelsk´ ym projektem jeˇstˇe rozdˇelen do samostatn´ ych proces˚ u platformy a uˇzivatelsk´eho projektu. Tyto procesy vz´ajemnˇe komunikuj´ı prostˇrednictv´ım s´ıt’ov´eho loopback rozhran´ı. Procesy uˇzivatelsk´eho projektu mezi sebou komunikuj´ı prostˇrednictv´ım poˇc´ıtaˇcov´e s´ıtˇe nez´avisle na procesech platformy a vytv´aˇrej´ı tak komunikaˇcn´ı kan´al pro zpr´avy zaslan´e uˇzivatelsk´ ym syst´emem.
Obr´azek 2.1: Blokov´ y diagram v´ yvojov´e platformy Kinbo2 . Pˇrevzato z [1].
2
Pouˇzit´e prostˇredky
2.2
Navigaˇcn´ı k´ody
Navigaˇ cn´ı k´ ody
Pro funkˇcnost t´eto pr´ace jsou vyuˇz´ıv´any k´ody rozm´ıstˇen´e v okol´ı. Slouˇz´ı jako orientaˇcn´ı body pro robota, kter´ y po jejich nalezen´ı dok´aˇze urˇcit vlastn´ı polohu v prostoru. Pro tento u ´ˇcel bylo vyzkouˇseno v´ıce druh˚ u k´od˚ u (napˇr. DataMatrix a QR k´od). V´ ysledkem bylo vyuˇzit´ı QR k´od˚ u, kter´e se projevily jako nejv´ yhodnˇejˇs´ı z hlediska uloˇziteln´e informace a rozpoznatelnosti vyuˇzitou knihovnou. Pouˇzity jsou QR k´ody s ˇclenitost´ı 21x21 bod˚ u. Aby bylo dosaˇzeno co nejvˇetˇs´ıho dosahu ˇcitelnosti, ale z´aroveˇ n zachov´ana rozumn´a velikost, jsou pouˇz´ıv´any k´ody o velikosti 20x20 centimetr˚ u. Uloˇzeny jsou v nich souˇradnice, na kter´ ych se dan´ y QR k´od nach´az´ı a u ´hel natoˇcen´ı QR k´odu v˚ uˇci ose x pouˇzit´eho souˇradn´eho syst´emu prostoru, ve kter´em se robot nach´az´ı. Hodnota u ´hlu se zvyˇsuje proti smˇeru hodinov´ ych ruˇciˇcek. Data jsou uloˇzena ve form´atu souradniceX,souradniceY,uhel. Pˇr´ıklad m˚ uˇze b´ yt QR k´od s uloˇzenou informac´ı 100,100,180, kter´ y je zobrazen na obr´azku 2.2. Hodnota kaˇzd´e souˇradnice ud´av´a poˇcet centimetr˚ u od zaˇca´tku souˇradn´eho syst´emu, u ´hel je uveden ve stupn´ıch. Maxim´aln´ı moˇzn´e hodnoty souˇradnic pˇri udrˇzen´ı uveden´e minim´aln´ı ˇclenitosti k´odu jsou dev´ıtim´ıstn´e, coˇz vzhledem k vyj´adˇren´ı v centimetrech odpov´ıd´a maxim´aln´ı velikosti mapovateln´eho prostoru v ˇr´adech tis´ıc˚ u kilometr˚ u. Hodnoty jsou zak´odov´any jako text pomoc´ı online geener´atoru QR k´od˚ u dostupn´eho na [8].
Obr´azek 2.2: QR k´od s informac´ı 100,100,180
3
Pouˇzit´e prostˇredky
2.3
Knihovna ZXing.net
Knihovna ZXing.net
Vzhledem k pouˇz´ıvan´emu programov´emu vybaven´ı platformy Kinbo2 bylo krit´eriem pro hled´an´ı pouˇziteln´e knihovny tak´e to, aby ji bylo moˇzno pouˇz´ıt v prostˇred´ı C# .NET. Tomuto krit´eriu vyhovuje nalezen´a knihovna ZXing.Net, kter´a je dostupn´a na [2]. Jedn´a se o knihovnu pro dek´odov´an´ı r˚ uzn´ ych druh˚ u k´od˚ u, vˇcetnˇe vyuˇzit´ ych QR k´od˚ u, pro kter´e poskytuje kvalitn´ı podporu. Pr´ace s knihovnou ZXing.Net je pomˇernˇe jednoduch´a a nen´ı potˇreba sloˇzit´ ych konstrukc´ı pro vyuˇzit´ı jej´ı funkˇcnosti. Pouˇzit´a verze 0.12.0.0 nav´ıc obsahuje tak´e pˇr´ımou podporu pro datov´ y typ ColorImageFrame, kter´ y je v zaˇr´ızen´ı MS Kinect pouˇz´ıv´an pro reprezentaci sn´ıman´eho obrazu. Toho m˚ uˇzeme s v´ yhodou vyuˇz´ıt a nen´ı tˇreba prov´adˇet komplikovan´e pˇrevody do jin´eho datov´eho typu. Lze tedy obraz reprezentovan´ y t´ımto datov´ ym typem pˇr´ımo poslat do dekod´eru implementovan´eho v t´eto knihovnˇe. Dekod´er n´aslednˇe v obrazu vyhled´av´a QR k´ody. Pokud hled´an´ı skonˇc´ı u ´spˇeˇsnˇe a dok´aˇze detekovat k´od, vr´at´ı pole v´ ysledk˚ u reprezentuj´ıc´ı vˇsechny nalezen´e k´ody, kter´e se podaˇrilo bez chyby dek´odovat. Pokud v obraze ˇz´adn´ y QR k´od nenajde, vr´at´ı hodnotu null. Z v´ ysledku, kter´ y je vr´acen pˇri u ´spˇeˇsn´em hled´an´ı, m˚ uˇzeme snadno z´ıskat deˇsifrovan´ y text. M˚ uˇzeme tak´e z´ıskat souˇradnice, na kter´ ych se v obraze dan´ y k´od nach´az´ı. Tyto souˇradnice n´aslednˇe vyuˇzijeme pro nalezen´ı vzd´alenosti QR k´odu od zaˇr´ızen´ı.
2.4
Microsoft Kinect
Zaˇr´ızen´ı Microsoft Kinect (viz obr. 2.3) bylo p˚ uvodnˇe vytvoˇreno jako periferie k hern´ı konzoli Xbox 360. S jeho pomoc´ı m˚ uˇze hr´aˇc k ovl´ad´an´ı pouˇz´ıvat pohyby sv´eho tˇela, coˇz umoˇznilo nov´ y rozmˇer vn´ım´an´ı a proˇz´ıv´an´ı hern´ıch z´aˇzitk˚ u. N´as ovˇsem v´ıce zaj´ım´a technick´a specifikace. Kinect sn´ım´a sv´e okol´ı pomoc´ı RGB kamery s rozliˇsen´ım aˇz 1280x960 pixel˚ u. Takov´e rozliˇsen´ı ovˇsem generuje pouze 12 sn´ımk˚ u za sekundu. Vyuˇzitelnˇejˇs´ı variantou je tedy rozliˇsen´ı 640x480 pixel˚ u s frekvenc´ı 30 sn´ımk˚ u za sekundu. Je tak´e v´ yhodnˇejˇs´ı pro synchronizaci dat z hloubkov´e kamery, kter´a tak´e pracuje s rozliˇsen´ım 640x480 pixel˚ u a frekvenc´ı 30 sn´ımk˚ u za sekundu. Pro z´ısk´an´ı hloubkov´ ych
4
Pouˇzit´e prostˇredky
Microsoft Kinect
Obr´azek 2.3: Senzor Microsoft Kinect. Pˇrevzato z [4].
dat m´a Kinect emitor infraˇcerven´ ych paprsk˚ u, kter´ y do prostoru vys´ıl´a body v pevnˇe stanoven´e mˇr´ıˇzce. Podle [9] senzor tuto mˇr´ıˇzku n´aslednˇe sn´ım´a infraˇcervenou kamerou. V´ ysledn´a hloubka je vypoˇctena na z´akladˇe triangulace mezi emitovanou a pˇreˇctenou konfigurac´ı mˇr´ıˇzky bod˚ u. Tato technologie se jmenuje Light Coding a byla vyvinuta spoleˇcnost´ı PrimeSense. Zorn´e pole kamer je 57 ◦ v horizont´aln´ım smˇeru a 43 ◦ ve vertik´aln´ım smˇeru. Kinect lze tak´e naklonit ve vertik´aln´ı ose na obˇe strany aˇz o 27 ◦ Na obr´azku 2.4 m˚ uˇzeme vidˇet rozloˇzen´ı komponent v senzoru MS Kinect.
Obr´azek 2.4: Rozloˇzen´ı komponent senzoru Microsoft Kinect. Pˇrevzato z [4].
5
3 Urˇcen´ı pozice robota Aby mohl robot urˇcit svou polohu v prostoru s dan´ ym souˇradn´ ym syst´emem, potˇrebuje nejprve nal´ezt urˇcit´ y z´achytn´ y bod. K tomu slouˇz´ı jiˇz zm´ınˇen´e QR k´ody se souˇradnicemi. Pokud robot pomoc´ı zaˇr´ızen´ı MS Kinect nalezne ve sv´em okol´ı QR k´od, m´a referenˇcn´ı bod, s jehoˇz pomoc´ı dok´aˇze urˇcit vlastn´ı pozici. Bohuˇzel kv˚ uli technick´ ym limit˚ um senzoru je moˇzn´e urˇcit pozici robota pouze po nalezen´ı k´odu ve vzd´alenosti od 0,8 do 2,4 metru. Na kratˇs´ı vzd´alenost neˇz 0,8 metru nen´ı senzor schopen rozeznat vzd´alenost k´odu a na vˇetˇs´ı vzd´alenost neˇz 2,4 metru jiˇz knihovna ZXing.net nedok´aˇze QR k´od o rozmˇerech 20x20 cm rozeznat kv˚ uli n´ızk´emu rozliˇsen´ı sn´ıman´eho obrazu.
3.1
V´ ypoˇ cet souˇ radnic pomocn´ eho bodu
Kdyˇz se pod´ıv´ame na strukturu QR k´odu (napˇr. na obr. 2.2), vid´ıme tˇri referenˇcn´ı ˇctverce v roz´ıch. Knihovna ZXing pˇri nalezen´ı QR k´odu vrac´ı pole souˇradnic v obraze, kter´e odpov´ıdaj´ı pr´avˇe tˇemto bod˚ um. Pokud tedy lev´emu horn´ımu bodu (oznaˇcme jej A) pˇriˇrad´ıme hodnoty souˇradnic, kter´e jsou uloˇzeny v QR k´odu, pak dopoˇcten´ım souˇradnic prav´eho horn´ıho bodu (oznaˇcme jej C) z´ısk´ame pomocn´ y bod, s jehoˇz pomoc´ı m˚ uˇzeme vypoˇc´ıtat souˇradnice robota. Jelikoˇz velikost vˇsech QR k´od˚ u, kter´e jsou k t´eto pr´aci vyuˇzity, je shodn´a, z˚ ust´av´a shodn´a tak´e vzd´alenost tˇechto dvou bod˚ u, kterou tedy m˚ uˇzeme prohl´asit za konstantu. Pro moˇznost vypoˇcten´ı souˇradnic pomocn´eho bodu potˇrebujeme jeˇstˇe tˇret´ı bod, kter´ y si oznaˇc´ıme B, a kter´ y by s body A a C vytvoˇril troj´ uheln´ık. Bod B m˚ uˇzeme zvolit libovolnˇe, takˇze budeme zn´at jeho souˇradnice a tak´e vzd´alenost od bodu A. Spojen´ım tˇechto bod˚ u n´am tedy vznikne troj´ uheln´ık. Jeho obecnou podobu popisuje obr´azek 3.1.
Obr´azek 3.1: Obecn´ y troj´ uheln´ık
6
Urˇcen´ı pozice robota
V´ypoˇcet vzd´alenosti bodu od senzoru
Zn´ame body A a B a zn´ame tak´e strany b a c. Stranu b jsme totiˇz oznaˇcili za konstantu d´ıky nemˇen´ıc´ı se velikosti QR k´od˚ u. V QR k´odech je tak´e uloˇzen u ´hel, kter´ y ud´av´a natoˇcen´ı k´odu v˚ uˇci ose x, pˇriˇcemˇz jeho hodnota se zvyˇsuje nat´aˇcen´ım k´odu v protismˇeru hodinov´ ych ruˇciˇcek. Pokud tedy strategicky um´ıst´ıme bod B, budeme moci u ´hel α prohl´asit za u ´hel uloˇzen´ y v k´odu. Um´ıst´ıme tedy bod B tak, aby strana c byla rovnobˇeˇzn´a s osou x. Pro jednoduchost bude bod B vˇzdy um´ıstˇen tak, ˇze jeho souˇradnice x je nulov´a a souˇradnice y je shodn´a jako u bodu A. D´ıky tomu zn´ame d´elku strany c, kter´a je rovna hodnotˇe souˇradnice x bodu A. Nyn´ı tedy m˚ uˇzeme vypoˇc´ıtat d´elku strany a. To provedeme za pomoci Cosinov´e vˇety, kter´a m´a tvar a2 = b2 + c2 − 2bc · cos α.
(3.1)
Nyn´ı zn´ame vzd´alenost bodu C od bodu A i od bodu B a m˚ uˇzeme dopoˇc´ıtat souˇradnice bodu C pomoc´ı pr˚ useˇc´ık˚ u dvou kruˇznic. Tento postup je pops´an v sekci 3.3.
3.2
V´ ypoˇ cet vzd´ alenosti bodu od senzoru
Abychom mohli urˇcit pozici robota v prostoru, nestaˇc´ı n´am pouze zn´at souˇradnice nalezen´ ych bod˚ u QR k´odu, ale potˇrebujeme tak´e zn´at jejich vzd´alenost od senzoru. K tomu m˚ uˇzeme vyuˇz´ıt mˇeˇren´ı hloubky, kter´e prov´ad´ı senzor MS Kinect. Prvn´ım probl´emem, na kter´ y pˇri vyuˇzit´ı hloubkov´eho obrazu, sn´ıman´eho senzorem MS Kinect, naraz´ıme, je sn´ım´an´ı hloubky jinou kamerou, neˇz kter´a sn´ım´a barevn´ y obraz. Vznik´a tedy situace, kdy jsou jednotliv´e sn´ımky vz´ajemnˇe posunuty. Pokud tedy uv´aˇz´ıme v barevn´em obraze napˇr´ıklad bod se souˇradnicemi x = 100 a y = 100, pak v hloubkov´em obraze bod se stejn´ ymi souˇradnicemi nereprezentuje tento bod v barevn´em obraze. K omezen´ı vlivu tohoto probl´emu je v knihovnˇe Kinect SDK dostupn´e na [3] implementov´ana mapovac´ı funkce MapColorFrameToDepthFrame, jej´ımˇz u ´ˇcelem je co nejpˇresnˇeji pˇriˇradit kaˇzd´ y bod hloubkov´eho obrazu na pozici se souˇradnicemi odpov´ıdaj´ıc´ıho bodu v barevn´em obraze tak, aby bylo moˇzn´e nal´ezt obˇe odpov´ıdaj´ıc´ı hodnoty na stejn´ ych souˇradnic´ıch. Na obr´azku 3.2 je zn´azornˇen sn´ımek z barevn´e kamery, v jehoˇz ˇcerven´e sloˇzce je vloˇzena namˇeˇren´a hloubka. V prav´em obr´azku je hloubka bez pouˇzit´ı mapovac´ı funkce a v lev´em
7
Urˇcen´ı pozice robota
V´ypoˇcet vzd´alenosti bodu od senzoru
obr´azku je pouˇzita mapovac´ı funkce. Modr´a m´ısta oznaˇcuj´ı neplatn´e hodnoty. Jde o m´ısta, pro kter´a senzor nedok´azal zmˇeˇrit hloubku.
Obr´azek 3.2: Sn´ımky ze senzoru MS Kinect zn´azorˇ nuj´ıc´ı vztah mezi barevn´ ym obrazem a hloubkou. Vlevo po pouˇzit´ı mapovac´ı funkce, vpravo bez mapov´an´ı.
Pokud se na obr´azc´ıch zamˇeˇr´ıme na kˇreslo, lze celkem snadno zpozorovat, ˇze po pouˇzit´ı mapovac´ı funkce (lev´ y obr´azek) hodnoty namˇeˇren´e hloubky mnohem l´epe kop´ıruj´ı tvar skuteˇcn´eho pˇredmˇetu, zat´ımco bez vyuˇzit´ı mapovac´ı funkce (prav´ y obr´azek) jsou rozd´ıly pomˇernˇe znaˇcn´e a jasnˇe viditeln´e. Na lev´em obr´azku si m˚ uˇzeme tak´e vˇsimnout, ˇze po aplikov´an´ı mapovac´ı funkce je obraz ze tˇr´ı stran ohraniˇcen p´asem neplatn´ ych hodnot. Tento p´as je zp˚ usoben jiˇz zm´ınˇenou problematikou odliˇsn´eho zorn´eho pole kamer pro barevn´ y a hloubkov´ y obr´azek. V tˇechto m´ıstech tedy nen´ı moˇzn´e urˇcit hloubku, tud´ıˇz ani dopoˇc´ıtat vzd´alenost pˇredmˇet˚ u v tomto p´asmu. Dalˇs´ım probl´emem je to, ˇze nelze pˇr´ımo vyuˇz´ı hloubku namˇeˇrenou senzorem, protoˇze Kinect nemˇeˇr´ı vzd´alenost objektu od senzoru, ale jeho hloubku v obraze v˚ uˇci rovinˇe senzoru (viz obr. 3.3).
Obr´azek 3.3: Porovn´an´ı mˇeˇren´e hloubky senzorem Microsoft Kinect a skuteˇcn´e vzd´alenosti objektu od senzoru. Pˇrevzato z [6].
8
Urˇcen´ı pozice robota
Nalezen´ı pr˚ useˇc´ık˚ u kruˇznic
Z obr´azku je ale tak´e patrn´e, ˇze skuteˇcnou vzd´alenost nen´ı probl´em dopoˇc´ıtat. Knihovna ZXing totiˇz pro bod, jehoˇz vzd´alenost chceme urˇcit, vrac´ı souˇradnice v obraze, takˇze na tˇechto souˇradnic´ıch m˚ uˇzeme pˇreˇc´ıst hloubku, kter´a n´am urˇc´ı d´elku jedn´e strany pravo´ uhl´eho troj´ uheln´ıku (svisl´a strana na obr. 3.3). Jelikoˇz zn´ame velikost obrazu a tak´e u ´hel zorn´eho pole senzoru, lze stanovit konstantu ud´avaj´ıc´ı hodnotu u ´hlu na jeden pixel obrazu. Pokud nyn´ı v horizont´aln´ı ose urˇc´ıme vzd´alenost bodu od stˇredu obrazu a vyn´asob´ıme tuto vzd´alenost konstantou pro hodnotu u ´hlu na jeden pixel, z´ısk´ame u ´hel, kter´ y sv´ıraj´ı namˇeˇren´a hloubka a skuteˇcn´a vzd´alenost. Nyn´ı tedy v pravou ´hl´em troj´ uheln´ıku zn´ame jeden z u ´hl˚ u (oznaˇcme jej α), pˇrilehlou odvˇesnu (namˇeˇren´a hloubka, oznaˇc´ıme ji depth) a hled´ame velikost pˇrepony (poˇc´ıtan´a vzd´alenost, oznaˇc´ıme ji distance). Vyuˇzijeme tedy goniometrickou funkci cosinus:
cos α =
depth . distance
(3.2)
Jednoduchou z´amˇenou cosinu a nezn´am´e distance z´ısk´ame rovnici
distance =
depth . cos α
(3.3)
V´ ysledek t´eto rovnice je n´ami hledan´a vzd´alenost bodu od senzoru.
3.3
Nalezen´ı pr˚ useˇ c´ık˚ u kruˇ znic
Kdyˇz jiˇz m´ame dopoˇcteny tak´e souˇradnice pomocn´eho bodu, m˚ uˇzeme uvaˇzovat, ˇze m´ame troj´ uheln´ık, jehoˇz dva vrcholy jsou reprezentov´any dan´ ymi body v QR k´odu, pro kter´e m´ame urˇcen´e souˇradnice a ve tˇret´ım vrcholu se nach´az´ı robot, jehoˇz pozici hled´ame. Zn´ame tedy dva vrcholy troj´ uheln´ıku a d´ıky dˇr´ıve popsan´emu postupu tak´e jejich vzd´alenosti od robota, tedy dvˇe strany troj´ uheln´ıku. Mohli bychom tak´e dopoˇc´ıtat tˇret´ı stranu, tedy tu, kter´a spojuje dva zn´am´e vrcholy, ale tuto stranu pˇri v´ ypoˇctech nebudeme potˇrebovat. Pro nalezen´ı nezn´am´eho vrcholu totiˇz vyuˇzijeme postupu, kter´ y je pouˇz´ıv´an v geometrii pˇri tvorbˇe troj´ uheln´ıku podle vˇety sss. Tento postup je n´asleduj´ıc´ı: 1. Sestroj´ıme stanu AB, kde A a B jsou zn´am´e vrcholy 9
Urˇcen´ı pozice robota
Nalezen´ı pr˚ useˇc´ık˚ u kruˇznic
2. Z vrcholu A sestroj´ıme kruˇznici o polomˇeru rovn´em d´elce strany AC, kde C je nezn´am´ y vrchol 3. Z vrcholu B sestroj´ıme kruˇznici o polomˇeru rovn´em d´elce strany BC, kde C je nezn´am´ y vrchol 4. Pr˚ useˇc´ık tˇechto kruˇznic n´am d´av´a nezn´am´ y vrchol C 5. Spojen´ım vrchol˚ u A a B s vrcholem C z´ısk´ame troj´ uheln´ık ABC Pro n´aˇs v´ ypoˇcet hledan´eho vrcholu C n´am staˇc´ı vyuˇz´ıt znalosti bod˚ u 2, 3 a 4 ve zm´ınˇen´em postupu. Bod 1 je pro n´as zbyteˇcn´ y, protoˇze n´as zaj´ımaj´ı pouze souˇradnice bodu C, v´ ysledn´ y troj´ uheln´ık tak´e r´ ysovat nebudeme, tud´ıˇz nemus´ıme vyuˇz´ıt ani bod 5. Budeme tedy poˇc´ıtat pr˚ useˇc´ıky dvou kruˇznic, jejichˇz stˇredy budou body A a B. Pro popis kruˇznic pouˇzijeme obecnou rovnici kruˇznice, kter´a je ve tvaru (x − sx )2 + (y − sy )2 = r2 .
(3.4)
Tuto obecnou rovnici uprav´ıme do tvaru x2 + y 2 − 2 · sx · x − 2 · sy · y + p = 0,
(3.5)
kde x a y jsou souˇradnice hledan´eho vrcholu, sx a sy jsou souˇradnice stˇredu kruˇznice a p je odvozeno ze vztahu p = s2x + s2y − r2 ,
(3.6)
kde r je polomˇer kruˇznice. Polomˇer kruˇznice je roven vzd´alenosti bodu od senzoru MS Kinect. Tuto vzd´alenost z´ısk´ame pˇri nalezen´ı QR k´odu v obraze pˇred samotn´ ym v´ ypoˇctem. V naˇsem pˇr´ıpadˇe m´ame dvˇe kruˇznice, vznikne n´am tedy soustava dvou rovnic o dvou nezn´am´ ych. Naˇs´ım u ´kolem je nyn´ı tuto soustavu vyˇreˇsit, protoˇze jej´ım ˇreˇsen´ım jsou souˇradnice pr˚ useˇc´ık˚ u naˇsich dvou kruˇznic, pˇriˇcemˇz jeden z nich odpov´ıd´a n´ami hledan´e pozici. Prvn´ım krokem je odstranˇen´ı druh´ ych mocnin u nezn´am´ ych. Toho doc´ıl´ıme jednoduch´ ym odeˇcten´ım druh´e rovnice od prvn´ı. 10
Urˇcen´ı pozice robota
Nalezen´ı pr˚ useˇc´ık˚ u kruˇznic
Obecnˇe tedy dostaneme jednu rovnici ve tvaru
(−2 · sx1 + 2 · sx2 ) · x + (−2 · sy1 + 2 · sy2 ) · y + p1 − p2 = 0.
(3.7)
V pˇr´ıpadˇe, kdy sx1 = sx2 , nebo sy1 = sy2 , n´am po t´eto u ´pravˇe zbyde jedna rovnice o jedn´e nezn´am´e, kterou jiˇz nen´ı ˇz´adn´ y probl´em vyˇreˇsit. T´ım z´ısk´ame hodnotu jedn´e souˇradnice, kterou n´aslednˇe dosad´ıme zpˇet do obecn´e rovnice kruˇznice (3.5). Na tom, kterou z naˇsich dvou rovnic si vybereme, nez´aleˇz´ı. V´ ypoˇcet bude fungovat stejn´ ym zp˚ usobem, at’ dosad´ıme do kter´ekoliv rovnice. Zvol´ıme tedy napˇr´ıklad rovnici prvn´ı kruˇznice, do kter´e budeme dosazovat. T´ım n´am vznikne kvadratick´a rovnice, jej´ımˇz vyˇreˇsen´ım z´ısk´ame tak´e druhou hledanou souˇradnici. Mˇejme tedy kvadratickou rovnici ve zn´am´em tvaru k1 · x2 + k2 · x + k3 = 0.
(3.8)
Uvaˇzujme, ˇze z pˇredchoz´ıch v´ ypoˇct˚ u jiˇz zn´ame souˇradnici y. Pak tedy koeficienty kvadratick´e rovnice odpov´ıdaj´ı n´asleduj´ıc´ım vtah˚ um:
k1 = 1, k2 = −2 · sx1 , k3 = y 2 − 2 · sy1 · y + p1 , kde y je jiˇz vypoˇcten´a souˇradnice pr˚ useˇc´ıku a sx1 , sy1 a p1 jsou koeficienty z obecn´e rovnice (3.5) pro prvn´ı kruˇznici. Pokud bychom neznali souˇradnici y, ale souˇradnici x, byl by v´ ypoˇcet koeficient˚ u obdobn´ y, pouze s t´ım rozd´ılem, ˇze bychom z obecn´e rovnice (3.5) nepouˇz´ıvali koeficienty pro nezn´amou y, ale pouˇzili bychom odpov´ıdaj´ıc´ı koeficienty pro nezn´amou x. Pokud ovˇsem zm´ınˇen´a rovnost keficient˚ u v rovnici (3.7) neplat´ı, z´ısk´ame jednu rovnici o dvou nezn´am´ ych a postup je m´ırnˇe sloˇzitˇejˇs´ı. Mus´ıme totiˇz nejprve vyj´adˇrit jednu nezn´amou. Uved’me si pˇr´ıklad, kdy vyj´adˇr´ıme nezn´amou y:
y=
(−2 · sx1 + 2 · sx2 ) · x − p1 + p2 . (−2 · sy1 + 2 · sy2 ) 11
(3.9)
Urˇcen´ı pozice robota
Nalezen´ı pr˚ useˇc´ık˚ u kruˇznic
Takto vyj´adˇrenou nezn´amou opˇet dosad´ıme zpˇet do obecn´e rovnice kruˇznice (3.5). Ani v tomto pˇr´ıpadˇe nez´aleˇz´ı, do kter´e z naˇsich dvou rovnic budeme dosazovat, budeme tedy tak´e dosazovat do rovnice prvn´ı kruˇznice. Po dosazen´ı z´ısk´ame opˇet kvadratickou rovnici. N´asleduj´ıc´ı postup je tedy shodn´ y pro oba pˇr´ıpady, pouze koeficienty kvadratick´e rovnice (3.8) budou vypoˇcteny odliˇsn´ ym zp˚ usobem. Pokud v rovnici (3.9) provedeme dˇelen´ı, m˚ uˇzeme ji pˇrepsat do jednoduˇsˇs´ıho tvaru y = t1 · x + t2 ,
(3.10)
kde plat´ı n´asleduj´ıc´ı vztahy: t1 =
(−2·sx1 +2·sx2 ) (−2·sy1 +2·sy2 )
t2 =
(−p1 +p2 ) , (−2·sy1 +2·sy2 )
Zjednoduˇsen´ y tvar 3.10 m˚ uˇzeme vyuˇz´ıt pro pˇrehlednˇejˇs´ı vyj´adˇren´ı v´ ypoˇctu koeficient˚ u kvadratick´e rovnice, kter´e se v tomto pˇr´ıpadˇe poˇc´ıtaj´ı n´asleduj´ıc´ım zp˚ usobem: k1 = 1 + t21 , k2 = −2 · sx1 − 2 · t1 · sy1 , k3 = t22 − 2 · t2 · sy1 + p1 . Nyn´ı jiˇz tedy m´ame vytvoˇrenu kvadratickou rovnici a m˚ uˇzeme s jej´ı pomoc´ı vypoˇc´ıtat determinant. Nejen, ˇze jej potˇrebujeme pro urˇcen´ı pr˚ useˇc´ık˚ u, ale tak´e n´am urˇc´ı, kolik pr˚ useˇc´ık˚ u n´ami poˇc´ıtan´e kruˇznice maj´ı. Determinant kvadratick´e rovnice vypoˇcteme ze vztahu D = k22 − 4 · k1 · k3 ,
(3.11)
kde k1 , k2 , k3 jsou koeficienty kvadratick´e rovnice (3.8). Podle v´ ysledku determinantu m˚ uˇzeme rozliˇsovat 3 r˚ uzn´e situace, kter´e jsou zn´azornˇeny na obr´azku 3.4. Prvn´ı moˇznost´ı je v´ ysledek D < 0, kter´ y znamen´a, ˇze kruˇznice 12
Urˇcen´ı pozice robota
Nalezen´ı pr˚ useˇc´ık˚ u kruˇznic
nemaj´ı ˇza´dn´ y spoleˇcn´ y bod. Za pˇredpokladu, ˇze v okol´ı budou spr´avnˇe rozm´ıstˇeny QR k´ody se spr´avn´ ymi hodnotami souˇradnic, nemˇela by tato situace nikdy nastat. Dalˇs´ı moˇznost´ı je D = 0, kdy kruˇznice maj´ı jedin´ y spoleˇcn´ y bod, kter´ y by mˇel odpov´ıdat n´ami hledan´e pozici. Posledn´ı a pravdˇepodobnˇe nejˇcastˇejˇs´ı variantou je v´ ysledek D > 0, kdy kruˇznice maj´ı dva spoleˇcn´e body. V tomto pˇr´ıpadˇe tedy jeˇstˇe mus´ıme urˇcit, kter´ y z tˇechto pr˚ useˇc´ık˚ u odpov´ıd´a bodu, kter´ y hled´ame.
Obr´azek 3.4: Vz´ajemn´e polohy dvou kruˇznic
Kdyˇz m´ame vypoˇcten´ y determinant, zb´ yv´a n´am jiˇz pouze posledn´ı krok k urˇcen´ı souˇradnic pr˚ useˇc´ık˚ u kruˇznic. Pokud jsme jiˇz po rozd´ılu obecn´ ych rovnic jednotliv´ ych kruˇznic z´ıskali jednu souˇradnici pr˚ useˇc´ıku, je tato souˇradnice shodn´a pro vˇsechny existuj´ıc´ı pr˚ useˇc´ıky. Uvaˇzujme, ˇze jiˇz zn´ame souˇradnici y a podle v´ ysledku determinantu jsme zjistili, ˇze existuj´ı dva pr˚ useˇc´ıky. Jejich souˇradnice x vypoˇcteme t´ımto vzorcem:
x1,2
√ −k2 ± D , = 2 · k1
(3.12)
pˇriˇcemˇz souˇradnice jednotliv´ ych pr˚ useˇc´ık˚ u jsou P1 [x1 , y] a P2 [x2 , y]. Pokud jsme ovˇsem dosud v pr˚ ubˇehu v´ ypoˇctu nezjistili ˇz´adnou ze souˇradnic pr˚ useˇc´ık˚ u, vypoˇcteme pomoc´ı vzorce (3.12) jednu ze souˇradnic, ˇreknˇeme souˇradnici x. V´ ypoˇcet souˇradnice y n´aslednˇe provedeme tak, ˇze vyuˇzijeme rovnici (3.9), do kter´e postupnˇe dosad´ıme souˇradnici x vˇsech existuj´ıc´ıch pr˚ useˇc´ık˚ u. T´ım dostaneme pr˚ useˇc´ıky se souˇradnicemi P1 [x1 , y1 ] a P2 [x2 , y2 ].
13
Urˇcen´ı pozice robota
3.4
Urˇcen´ı spr´avn´eho pr˚ useˇc´ıku
Urˇ cen´ı spr´ avn´ eho pr˚ useˇ c´ıku
Jak jsme jiˇz zm´ınili v pˇredchoz´ım textu, m˚ uˇze nastat situace, kdy n´ami poˇc´ıtan´e kruˇznice maj´ı dva pr˚ useˇc´ıky. Tyto dva pr˚ useˇc´ıky jsou naˇsimi potencion´aln´ımi body, kter´e hled´ame. Pouze jeden z nich ovˇsem m˚ uˇze b´ yt ten spr´avn´ y. V tomto pˇr´ıpadˇe m˚ uˇzeme vyuˇz´ıt toho, ˇze vid´ıme v obraze oba pomocn´e body v QR k´odu najednou, tud´ıˇz v´ıme, kter´ y je vlevo a kter´ y vpravo. Pro urˇcen´ı spr´avn´eho pr˚ useˇc´ıku pouˇzijeme v´ ypoˇcet vz´ajemn´e polohy bodu a pˇr´ımky. Necht’ body na pˇr´ımce oznaˇc´ıme A a B. Kontrolovan´ y pr˚ useˇc´ık oznaˇc´ıme jako X. Nyn´ı urˇc´ıme smˇerov´ y vektor pˇr´ımky ~v : ~v = B − A
(3.13)
a vektor mezi pr˚ useˇc´ıkem a bodem pˇr´ımky ~u: ~u = X − A.
(3.14)
Podle [5] pokud tyto dva vektory spoj´ıme do matice
M=
ux vx uy vy
!
,
(3.15)
tak determinant t´eto matice urˇc´ı polohu bodu v˚ uˇci pˇr´ımce. Vypoˇcteme tedy determinant vzorcem det = ux · vy − uy · vx .
(3.16)
Pokud det < 0, pak je bod nalevo od pˇr´ımky, pokud vyjde det > 0, bod je napravo od pˇr´ımky a pokud det = 0, pak bod leˇz´ı na pˇr´ımce. V naˇsem pˇr´ıpadˇe vˇzdy spr´avn´ y pr˚ useˇc´ık leˇz´ı napravo od pˇr´ımky, protoˇze pokud by leˇzel nalevo, d´ıvali bychom se na QR k´od ze zadn´ı strany a jelikoˇz QR k´ody budou um´ıstˇeny na pevn´e pˇrek´aˇzce, byl by to nesmysl.
14
Urˇcen´ı pozice robota
3.5
V´ypoˇcet u ´hlu natoˇcen´ı robota
V´ ypoˇ cet u ´ hlu natoˇ cen´ı robota
Nyn´ı jiˇz zn´ame souˇradnice robota v prostoru, ale abychom mˇeli kompletn´ı informaci o poloze a mohli s jej´ı pomoc´ı n´aslednˇe zaˇc´ıt navigovat robota a rozhodnout o jeho dalˇs´ım pohybu, potˇrebujeme zn´at tak´e jeho natoˇcen´ı v prostoru. Tuto informaci bohuˇzel nem˚ uˇzeme z´ıskat pˇr´ımo, protoˇze robot tento u ´daj s´am nemˇeˇr´ı. Mus´ıme u ´hel natoˇcen´ı tedy dopoˇc´ıtat ze vstupn´ıch u ´daj˚ u, kter´e m´ame k dispozici. V naˇsem pˇr´ıpadˇe je vstupn´ım zaˇr´ızen´ım senzor MS Kinect. Z obrazu ale obecnˇe nem˚ uˇzeme natoˇcen´ı poznat, potˇrebujeme tedy opˇet nˇejak´ y referenˇcn´ı bod. K tomuto u ´ˇcelu n´am opˇet poslouˇz´ı vyuˇz´ıvan´e QR k´ody. Jak jiˇz bylo zm´ınˇeno dˇr´ıve, u ´hel, kter´ y je v nich uloˇzen, urˇcuje jejich natoˇcen´ı v˚ uˇci ose x. Jelikoˇz zn´ame natoˇcen´ı k´odu v˚ uˇci souˇradn´emu syst´emu, m˚ uˇzeme s pomoc´ı u ´hlu natoˇcen´ı robota v˚ uˇci QR k´odu z´ıskat natoˇcen´ı robota v˚ uˇci souˇradn´emu syst´emu. Mus´ıme ale u ´hel natoˇcen´ı robota v˚ uˇci k´odu vypoˇc´ıtat. K tomu m˚ uˇzeme vyuˇz´ıt hloubkovou mapu senzoru MS Kinect. Jak je zm´ınˇeno v sekci 3.2, senzor MS Kinect mˇeˇr´ı hloubku k pˇredmˇet˚ um v˚ uˇci rovinˇe senzoru. N´ami hledan´ yu ´hel tedy m˚ uˇzeme z´ıskat pomoc´ı rozd´ılu hloubky dvou bod˚ u. K tomu opˇet vyuˇzijeme referenˇcn´ı body QR k´odu um´ıstˇen´e v jeho lev´em a prav´em horn´ım rohu. Zn´ame totiˇz jejich rozteˇc. Kdyˇz postup zn´azorn´ıme geometricky, jde o pravo´ uhl´ y troj´ uheln´ık (viz obr. 3.5). Pouˇzit´e referenˇcn´ı body reprezentuj´ı vrcholy A a B. Jejich rozteˇc je tedy d´elka strany c. Kdyˇz uv´aˇz´ıme, ˇze strana b je rovnobˇeˇzn´a s rovinou senzoru, v˚ uˇci kter´e je mˇeˇrena hloubka, pak rozd´ıl mezi hloubkami bod˚ u A a B je roven stranˇe a. Jelikoˇz d´ıky hloubkov´e mapˇe tyto body zn´ame, zn´ame tak´e ´ d´elku strany a. Uhel α je tedy roven u ´hlu natoˇcen´ı robota v˚ uˇci k´odu.
Obr´azek 3.5: Pravo´ uhl´ y troj´ uheln´ık
15
Urˇcen´ı pozice robota
V´ypoˇcet u ´hlu natoˇcen´ı robota
Pro v´ ypoˇcet u ´hlu vyuˇzijeme geometrickou funkci sinus: a sin α = . c
(3.17)
Toto ovˇsem jeˇstˇe nen´ı v´ ysledek, protoˇze naˇs´ım c´ılem je u ´hel natoˇcen´ı robota v˚ uˇci souˇradn´emu syst´emu. Jelikoˇz robot mus´ı na dan´ y k´od vidˇet, tak pokud by st´al rovnobˇeˇznˇe s k´odem, byl by v˚ uˇci nˇemu o 180 ◦ otoˇcen. Oznaˇc´ıme-li tedy u ´hel natoˇcen´ı k´odu jako β, u ´hel natoˇcen´ı robota v˚ uˇci k´odu je α a v´ ysledn´ y n´ami hledan´ yu ´hel γ, pak bychom v´ ysledn´ yu ´hel z´ıskali ze vztahu γ = (β + 180◦ + α)mod360◦ .
(3.18)
Modulo 360 ◦ je zde pouˇzito proto, aby nevznikl u ´hel vˇetˇs´ı, neˇz je u ´hel cel´eho kruhu.
16
4 Navigace robota Aby se mohl robot z v´ ychoz´ı pozice pˇresunout do poˇzadovan´e c´ılov´e pozice, mus´ı b´ yt schopen pohybovat se prostorem a urˇcovat svoj´ı pozici. Je tud´ıˇz implementov´ano nˇekolik pravidel, podle kter´ ych se rozhoduje o dalˇs´ım pohybu robota. Celou navigaci robota m˚ uˇzeme shrnout do dvou velk´ ych skupin.
4.1
Hled´ an´ı k´ odu
Prvn´ı skupina popisuje rozhodov´an´ı, pokud robot hled´a ve sv´em okol´ı QR k´ody a snaˇz´ı se urˇcit svoji polohu.
4.1.1
Rozhl´ ednut´ı
Rozhl´ednut´ı je krok, kter´ y je proveden na zaˇca´tku kaˇzd´e vyhled´avac´ı sekvence a tak´e po kaˇzd´em posunut´ı bˇehem vyhled´av´an´ı. Jeho c´ılem je nal´ezt v okol´ı robota QR k´od, podle kter´eho by se dala urˇcit poloha robota. Je mu zad´ana rotace smˇerem vpravo, tedy ve smˇeru hodinov´ ych ruˇciˇcek. Pokud nen´ı nalezen k´od, konˇc´ı ot´aˇcen´ı po dovrˇsen´ı otoˇcen´ı o 360 ◦ .
4.1.2
Pˇ resun
Pokud nebyl bˇehem rozhl´ıˇzen´ı nalezen ˇza´dn´ y k´od, zjevnˇe v okol´ı robota ˇz´adn´ y nen´ı a je potˇreba hledat jinde. Proto po dokonˇcen´ı ot´aˇcen´ı pokraˇcuje robot j´ızdou vpˇred ve smˇeru, ve kter´em bylo ukonˇceno ot´aˇcen´ı. T´ımto pohybem vpˇred uraz´ı robot vzd´alenost, kter´a by podle pˇredpokladu odpov´ıdala jednomu metru a nenaraz´ı-li pˇri pˇresunu na k´od, pokraˇcuje dalˇs´ım rozhl´ednut´ım.
17
Navigace robota
4.1.3
Posun k c´ıli
Objeven´ı pˇ rek´ aˇ zky
Pˇri pohybu vpˇred je potˇreba kontrolovat, aby robot nenarazil do pˇrek´aˇzky. K tomu je vyuˇzita hloubkov´a mapa senzoru MS Kinect, ve kter´e se vyhled´avaj´ı pˇr´ıliˇs bl´ızk´e pˇredmˇety. Pokud je v obraze takov´ y pˇredmˇet nalezen, je pohyb robota zastaven a pokraˇcuje ot´aˇcen´ım a hled´an´ım voln´e trasy. Jakmile pˇrek´aˇzka z obrazu zmiz´ı a podle hloubkov´e mapy je cesta voln´a, zaˇcne se robot pohybovat t´ımto smˇerem do pˇredpokl´adan´e vzd´alenosti p˚ ul metru a n´asleduje opˇet rozhl´ıˇzen´ı.
4.1.4
Nalezen´ı k´ odu
Pokud je v obraze nalezen k´od, s jehoˇz pomoc´ı se podaˇrilo naj´ıt polohu robota v prostoru, ukonˇcuje se f´aze vyhled´av´an´ı a pˇrech´az´ı se do f´aze posunu k c´ıli. Prvn´ım krokem je natoˇcen´ı.
4.2
Posun k c´ıli
Druh´a skupina popisuje rozhodov´an´ı o pohybu v pˇr´ıpadˇe, ˇze robot zjistil svoji pozici a nyn´ı se snaˇz´ı pˇresunout do zadan´e c´ılov´e pozice.
4.2.1
Natoˇ cen´ı
Natoˇcen´ı je prvn´ı krok f´aze posunu k c´ıli. Kdyˇz zn´ame polohu robota a v´ıme, na kter´e m´ısto se chceme posunout, zn´ame smˇer pˇr´ıˇst´ıho pohybu robota. Souˇcasnou hodnotu natoˇcen´ı robota tak´e zn´ame, takˇze v´ıme, o jak´ y u ´hel bychom mˇeli robota otoˇcit. Nastav´ı se tedy ˇcas odpov´ıdaj´ıc´ı otoˇcen´ı robota o poˇzadovan´ yu ´hel a zah´aj´ı se nat´aˇcen´ı. Pˇred zah´ajen´ım nat´aˇcen´ı je tak´e vyhodnoceno, kter´ ym smˇerem je v´ yhodnˇejˇs´ı se natoˇcit. Rozhodovac´ı podm´ınkou je porovn´an´ı rozd´ıl˚ u: dif f1 = targetAngle − robotAngle dif f2 = targetAngle − (robotAngle + 360◦ ),
18
Navigace robota
Posun k c´ıli
kde targetAngle je u ´hel, do kter´eho se chceme natoˇcit a robotAngle je aktu´aln´ı u ´hel natoˇcen´ı robota. Pokud dif f1 <= dif f2 , rozhoduje se podle velikosti dif f1 . Je-li jeho hodnota z´aporn´a, je v´ yhodnˇejˇs´ı ot´aˇcet se doprava, jinak se ot´aˇc´ı vlevo. Pokud ale dif f2 < dif f1 , je v´ yhodnˇejˇs´ı toˇcit doprava. Jako pˇr´ıklad si uved’me situaci, kdy targetAngle = 270◦ a robotAngle = 0◦ . Pokud by se robot napˇr´ıklad vˇzdy toˇcil pouze doleva (uvaˇzoval by se pouze rozd´ıl dif f1 ), musel by se v tomto pˇr´ıpadˇe otoˇcit o 270 ◦ , zat´ımco toˇcen´ım doprava by mu staˇcilo pouze 90 ◦ (z´ısk´ano z rozd´ılu dif f2 ).
4.2.2
Pohyb vpˇ red
Pokud vyprˇsel ˇcas, po kter´ y se mˇel robot ot´aˇcet, aby se nasmˇeroval do ˇza´dan´eho smˇeru, rozjede se robot rovnˇe v dan´em smˇeru. Spust´ı se odpoˇcet ˇcasu rovn´eho pohybu, kter´ y by mˇel odpov´ıdat ujet´ı poˇzadovan´e vzd´alenosti v tomto smˇeru.
4.2.3
Objeven´ı pˇ rek´ aˇ zky
Postup pˇri objeven´ı pˇrek´aˇzky je totoˇzn´ y, jako ve f´azi vyhled´av´an´ı pozice. Pokud naraz´ıme na pˇrek´aˇzku ve f´azi posunu k c´ıli, tak po vyhnut´ı se pˇrek´aˇzce pˇrech´az´ı robot opˇet do f´aze vyhled´av´an´ı.
4.2.4
Dokonˇ cen´ı pohybu vpˇ red
Pokud uplyne pˇrednastaven´a doba, po kterou mˇel robot jet rovnˇe v zadan´em smˇeru, zastav´ı se a pˇrejde do f´aze vyhled´av´an´ı, kdyˇz se zaˇcne rozhl´ıˇzet a hledat k´od, kter´ ym by ovˇeˇril svou novou pozici.
19
5 Interakce s ˇclovˇekem Souˇca´st´ı pr´ace je tak´e schopnost robota rozeznat ˇclovˇeka a pˇr´ıpadnˇe reagovat na jeho gesta. Rozpozn´av´an´ı ˇclovˇeka je implementov´ano jiˇz pˇr´ımo v SDK pro senzor MS Kinect. Staˇc´ı pouze pˇri spouˇstˇen´ı povolit SkeletonStream. Senzor MS Kinect umoˇzn ˇuje sledovat v´ıce lid´ı najednou, pro u ´ˇcely t´eto pr´ace vˇsak staˇc´ı sledov´an´ı jedin´e postavy. Senzor MS Kinect reprezentuje ˇclovˇeka jako kostru sloˇzenou z 20 bod˚ u (viz obr. 5.1). S pomoc´ı tˇechto bod˚ u a jejich vz´ajemn´e polohy lze rozezn´avat jednotliv´a gesta.
Obr´azek 5.1: Reprezentace lidsk´eho tˇela senzorem MS Kinect. Pˇrevzato z [7]
Kaˇzd´ y bod kostry je reprezentov´an souˇradnicemi x, y a z. Obr´azek 5.2 popisuje souˇradn´ y syst´em pro sledov´an´ı kostry. Ten totiˇz nen´ı shodn´ y se syst´emem pro hloubkov´ y a barevn´ y obraz. Souˇradnice v syst´emu pro sledov´an´ı kostry jsou reprezentov´any v metrech. Senzor je um´ıstˇen v poˇc´atku tohoto souˇradn´eho syst´emu a ve smˇeru, kter´ ym je nam´ıˇrena kamera, roste kladn´a hodnota osy z. Budeme-li tedy pozice v tomto syst´emu reprezentovat trojic´ı (x,y,z), pak bod (0,0,z) je um´ıstˇen ve stˇredu obrazu. Pˇri pohledu ve smˇeru senzoru pak od stˇredu obrazu smˇerem vlevo roste kladn´a hodnota osy x a na20
Interakce s ˇclovˇekem
Rozpozn´an´ı gesta
opak vpravo kles´a do z´aporn´ ych hodnot. U osy y pak jsou rostouc´ı kladn´e hodnoty v horn´ı polovinˇe obrazu a z´aporn´e v doln´ı polovinˇe.
Obr´azek 5.2: Souˇradn´ y syst´em pro sledov´an´ı lidsk´e kostry. Pˇrevzato z [7]
5.1
Rozpozn´ an´ı gesta
Abychom mohli rozpozn´avat gesta, potˇrebujeme zn´at vz´ajemnou polohu urˇcit´ ych bod˚ u kostry, kter´e se na dan´em gestu pod´ılej´ı. Vz´ajemnou pozici m˚ uˇzeme zkoumat napˇr´ıklad porovn´av´an´ım souˇradnic jednotliv´ ych bod˚ u, ale vzhledem k nutnosti br´at v potaz jistou odchylku, nen´ı to pˇr´ıliˇs pohodln´e ˇreˇsen´ı. Mnohem lepˇs´ım, aˇckoliv matematicky o nˇeco n´aroˇcnˇejˇs´ım, ˇreˇsen´ım je vyhodnocovat u ´hel, kter´ y sv´ıraj´ı tˇri body kostry. Je vhodn´e vyuˇz´ıt gesta, kter´a nejsou pro lidsk´ y pohyb zcela bˇeˇzn´a, aby nedoch´azelo ke zbyteˇcn´ ym ne´ umysln´ ym rozpozn´an´ım. Uved’me si tedy pˇr´ıklad, kdy budeme sledovat vz´ajemnou polohu bod˚ u HandRight (oznaˇcme jej jako bod A), ShoulderRight (oznaˇcme jej jako bod B) a Spine (oznaˇcme jej jako bod C). Tyto body m˚ uˇzeme spojit do vektoru ~u, kter´ y bude reprezentovat u ´hel mezi ramenem a rukou: ~u = (Ax − Bx , Ay − By )
(5.1)
a vektoru ~v , kter´ y bude reprezentovat u ´hel mezi ramenem a p´ateˇr´ı: ~v = (Cx − Bx , Cy − By ). 21
(5.2)
Interakce s ˇclovˇekem
Implementovan´a gesta
Body kostry jsou sice v trojrozmˇern´em prostoru, ale osu z pro tyto u ´ˇcely zanedb´ame, protoˇze n´am pro rozpozn´an´ı gesta staˇc´ı vz´ajemn´a poloha v ro´ vinˇe. Uhel mezi tˇemito vektory z´ısk´ame ze vzorce odvozen´eho ze skal´arn´ıho souˇcinu dvou vektor˚ u:
cos α =
~u ∗ ~v . |~u| ∗ |~v |
(5.3)
Kdyˇz z tohoto vzorce vyj´adˇr´ıme u ´hel α, z´ısk´ame hledanou hodnotu a m˚ uˇzeme ji porovnat, zda odpov´ıd´a urˇcit´emu gestu.
5.2
Implementovan´ a gesta
V projektu je implementov´ano nˇekolik z´akladn´ıch gest, jejich poˇcet lze snadno nav´ yˇsit o nov´a gesta. Pˇriloˇzen´e obr´azky jsou sn´ımky poˇr´ızen´e senzorem MS Kinect. Senzor zachyt´av´a obraz zrcadlovˇe obr´acen´ y, takˇze zde pˇriloˇzen´e obr´azky byly pˇrevr´aceny v˚ uˇci svisl´e ose, aby bylo pro ˇclovˇeka sn´aze rozpoznateln´e, kter´a ˇc´ast tˇela je lev´a a kter´a prav´a. V obr´azku je tak´e zelenˇe vyznaˇcena kostra ˇclovˇeka, jak j´ı sn´ım´a senzor MS Kinect.
5.2.1
Vstupn´ı gesto
Vstupn´ı gesto slouˇz´ı k zah´ajen´ı sledov´an´ı ˇclovˇeka. Dokud robot toto gesto nezaregistruje, tak na ˇza´dn´a jin´a gesta nereaguje a jakmile toto gesto rozpozn´a, zaˇcne sledovat ˇclovˇeka a ostatn´ı ˇcinnosti pˇreruˇs´ı. Vstupn´ı gesto je nejsloˇzitˇejˇs´ı ze vˇsech implementovan´ ych gest, aby se co nejv´ıce omezilo riziko nechtˇen´eho spuˇstˇen´ı sledov´an´ı. Pro rozpozn´an´ı tohoto gesta jsou kontrolov´any dva u ´hly. Prvn´ı sv´ıraj´ı body Spine, ShoulderCenter a ElbowRight. Hodnota u ´hlu se mus´ı pohybovat v rozmez´ı 50 ◦ aˇz 90 ◦ . Druh´ yu ´hel sv´ıraj´ı body ShoulderCenter, ElbowRight a HandRight. Hodnota tohoto u ´hlu se tak´e mus´ı pohybovat v rozmez´ı 50 ◦ aˇz 90 ◦ . Na obr´azku 5.3 lze vidˇet uk´azku tohoto gesta.
22
Interakce s ˇclovˇekem
Implementovan´a gesta
Obr´azek 5.3: Vstupn´ı gesto
5.2.2
Gesta pro otoˇ cen´ı vpravo a vlevo
Tato gesta d´avaj´ı robotovi pokyn, aby se otoˇcil o 360 ◦ toˇcen´ım ve smˇeru urˇcen´em zvednutou rukou. Gesta se liˇs´ı pouze vyuˇzit´ım prav´e, pˇr´ıpadnˇe lev´e ruky. Pop´ıˇseme si pˇr´ıklad s vyuˇzit´ım prav´e ruky. Toto gesto je opˇet urˇceno pozorov´an´ım dvou u ´hl˚ u. Prvn´ı sv´ıraj´ı body Spine, ShoulderCenter a ElbowRight. Hodnota u ´hlu se mus´ı pohybovat v rozmez´ı 60 ◦ aˇz 90 ◦ . Druh´ yu ´hel sv´ıraj´ı body Spine, ShoulderCenter a ElbowLeft. Hodnota tohoto u ´hlu mus´ı b´ yt ◦ menˇs´ı neˇz 60 . Jde tedy o gesto, kdy prav´a ruka je zvednuta pˇribliˇznˇe do v´ yˇse ramen a lev´a ruka je pˇripaˇzen´a. Na obr´azku 5.4 lze vidˇet uk´azku tohoto gesta.
Obr´azek 5.4: Gesto pro otoˇcen´ı robota smˇerem vpravo
23
Interakce s ˇclovˇekem
5.2.3
Sledov´an´ı ˇclovˇeka
V´ ystupn´ı gesto
Posledn´ım implementovan´ ym gestem je v´ ystupn´ı gesto, po jehoˇz rozezn´an´ı robot ukonˇcuje sledov´an´ı ˇclovˇeka a vrac´ı se zpˇet k pˇresunut´ı do c´ılov´eho bodu zadan´eho uˇzivatelem. Tak´e toto gesto je tedy urˇceno pozorov´an´ım dvou u ´hl˚ u. Prvn´ı sv´ıraj´ı body Spine, ShoulderCenter a ElbowLeft. Hodnota u ´hlu se mus´ı pohybovat v rozmez´ı 60 ◦ aˇz 90 ◦ . Druh´ yu ´hel sv´ıraj´ı body Spine, ShoulderCenter a ElbowRight. Hodnota tohoto u ´hlu mus´ı b´ yt tak´e v rozmez´ı 60 ◦ ◦ aˇz 90 . Jde tedy o gesto, kdy jsou obˇe ruce zvednuty pˇribliˇznˇe do v´ yˇse ramen. Na obr´azku 5.5 lze vidˇet uk´azku tohoto gesta.
Obr´azek 5.5: V´ ystupn´ı gesto
5.3
Sledov´ an´ı ˇ clovˇ eka
Pokud je robotem sledov´an ˇclovˇek a nen´ı pr´avˇe prov´adˇena ˇcinnost urˇcen´a rozpoznan´ ym gestem, snaˇz´ı se robot udrˇzet ˇclovˇeka uprostˇred obrazu a tak´e udrˇzovat vzd´alenost od ˇclovˇeka. Pokud se tedy bod kostry Spine vych´ yl´ı od stˇredu obrazu o hodnotu vˇetˇs´ı neˇz 0.1 v ose x, zaˇcne se robot t´ımto smˇerem nat´aˇcet, aby kompenzoval posun ˇclovˇeka a vr´atil jej zpˇet do stˇredu obrazu. Pˇri zah´ajen´ı sledov´an´ı ˇclovˇeka je tak´e poznamen´ana hodnota osy z pro bod kostry Spine. Pokud dojde ke zvˇetˇsn´ı aktu´aln´ı hodnoty nad poznamenanou hodnotu, zaˇcne se robot posouvat vpˇred, dokud aktu´aln´ı hodnota opˇet nebude niˇzˇs´ı. Hodnoty pouˇz´ıvan´e pro tuto korekci jsou souˇradnice bodu ze souˇradn´eho syst´emu kostry popsan´eho v sekci 5.
24
6 Struktura syst´emu Aby byla dodrˇzena struktura platformy, kter´a je pops´ana v [1], je i tato pr´ace vytvoˇrena a rozdˇelena do v´ıce syst´em˚ u. Platforma je rozdˇelena do syst´em˚ u serveru, klienta a robota, takˇze stejn´e rozdˇelen´ı mus´ı m´ıt tak´e uˇzivatelsk´ y program. Kaˇzd´ y syst´em vyuˇz´ıv´a moduly, kter´e jsou vytvoˇreny jako samostatn´e knihovny, jejichˇz propojen´ı je definov´ano konfiguraˇcn´ım XML souborem. Syst´emy mezi sebou navz´ajem komunikuj´ı pomoc´ı poˇc´ıtaˇcov´e s´ıtˇe TCP. Z toho plyne nutnost nadefinov´an´ı zpr´av, kter´e budou v komunikaci vyuˇz´ıv´any. Samotnou komunikaci mezi syst´emy zajiˇst’uje platforma Kinbo2 , uˇzivatel pouze mus´ı nadefinovat vstupy a v´ ystupy syst´em˚ u a jejich propojen´ı s moduly dan´ ych syst´em˚ u.
6.1
Pouˇ zit´ e zpr´ avy
Jelikoˇz mezi sebou syst´emy komunikuj´ı pomoc´ı poˇc´ıtaˇcov´e s´ıtˇe, je nutn´e nadefinovat zpr´avy pro vz´ajemnou komunikaci. Aby ovˇsem reˇzie pos´ıl´an´ı zpr´av nezab´ırala pˇr´ıliˇs mnoho ˇcasu, je definov´ano pouze p´ar z´akladn´ıch zpr´av. Dalˇs´ı zpr´avy je moˇzn´e snadno pˇri dalˇs´ım v´ yvoji doplnit. Dalˇs´ım d˚ uvodem, proˇc nen´ı definov´ano vˇetˇs´ı mnoˇzstv´ı zpr´av, je ten, ˇze veˇsker´a v´ ypoˇcetn´ı logika je prov´adˇena pouze na stranˇe robota bez spolupr´ace zbyl´ ych syst´em˚ u.
6.1.1
Zpr´ ava se souˇ radnicemi
Jedn´a se o z´akladn´ı zpr´avu projektu, ve kter´e jsou odesl´any zadan´e c´ılov´e souˇradnice pro syst´em robota. Podporov´any jsou pouze souˇradnice zadan´e cel´ ym ˇc´ıslem. Typ zpr´avy je MyKinboMessage, kter´ y byl pˇrevzat z programov´e pˇr´ılohy pr´ace [1] a form´at zpr´avy je n´asleduj´ıc´ı: <souradniceX>,<souradniceY>;
<souradniceX> oznaˇcuje zadanou souˇradnici x, <souradniceY> oznaˇcuje zadanou souˇradnici y a oznaˇcuje c´ılov´ y syst´em, kter´emu je zpr´ava urˇcena. Zpr´ava je pos´ıl´ana ze syst´emu klienta do syst´emu robota. Pokud 25
Struktura syst´emu
Pouˇzit´e zpr´avy
je tato zpr´ava pˇrijata v syst´emu robota pˇred spuˇstˇen´ım samotn´e ˇcinnosti syst´emu, signalizuje poˇzadavek na spuˇstˇen´ı. Pokud jiˇz ˇcinnost robota byla spuˇstˇena, jsou pouze pˇrenastaveny c´ılov´e souˇradnice.
6.1.2
Zpr´ ava o chybˇ e
Jde o zpr´avu, kdy robot klientovi oznamuje ne´ uspˇeˇsn´ y pokus o spuˇstˇen´ı senzoru MS Kinect. Tato chyba m˚ uˇze nastat, pokud nen´ı ˇza´dn´ y senzor pˇripojen, nebo pˇripojen´ y senzor nen´ı pˇripraven k pouˇzit´ı. Typ zpr´avy je MyKinboMessage a jej´ı form´at je: KinectMissing; oznaˇcuje c´ılov´ y syst´em, v tomto pˇr´ıpadˇe klienta.
6.1.3
Zpr´ ava pro motory
Dalˇs´ı d˚ uleˇzitou zpr´avou je zpr´ava typu RobotMotorsMessage. Tento typ je definov´an v platformˇe Kinbo2 a slouˇz´ı k nastaven´ı v´ ykon˚ u jednotliv´ ych motor˚ u robota. Form´at zpr´avy je: L: ; R: <pravyMotor> oznaˇcuje nastavovanou hodnotu v´ ykonu lev´eho motoru a <pravyMotor> oznaˇcuje nastavovanou hodnotu v´ ykonu prav´eho motoru. Hodnoty jsou uvedeny v desetinn´ ych ˇc´ıslech a mohou nab´ yvat hodnot z intervalu <-1;1>. Tato zpr´ava je odes´ıl´ana syst´emem robota platformˇe Kinbo2 , kter´a jiˇz zajiˇst’uje samotn´e nastaven´ı motor˚ u.
26
Struktura syst´emu
6.1.4
Syst´em serveru
Zpr´ ava o dosaˇ zen´ı c´ıle
Jde o zpr´avu typu MyKinboMessage, kterou robot odes´ıl´a klientovi pot´e, co dos´ahl poˇzadovan´ ych souˇradnic. Jej´ı form´at je: Done; oznaˇcuje c´ılov´ y syst´em, v tomto pˇr´ıpadˇe klienta.
6.1.5
Zpr´ ava o chybn´ ych souˇ radnic´ıch
Tato zpr´ava typu MyKinboMessage je zas´ıl´ana robotem klientovi, pokud pˇrijat´e souˇradnice nejsou ˇc´ısla. Form´at zpr´avy je: badCoords; oznaˇcuje c´ılov´ y syst´em.
6.1.6
Ukonˇ covac´ı zpr´ ava
Tato zpr´ava typu MyKinboMessage je zas´ıl´ana klientem robotovi pˇri ˇz´adosti o ukonˇcen´ı bˇehu. Stejnou zpr´avu n´aslednˇe pos´ıl´a robot klientovi zpˇet jako potvrzen´ı. Po pˇrijet´ı t´eto zpr´avy jsou oba syst´emy ukonˇceny. Form´at zpr´avy je: exit; oznaˇcuje c´ılov´ y syst´em.
6.2
Syst´ em serveru
Obecnˇe syst´em serveru v projektech bˇeˇz´ıc´ıch s platformou Kinbo2 zajiˇst’uje spojen´ı mezi klientem a robotem. Jeho u ´kolem je tak´e vykon´avat v´ ypoˇcetn´ı 27
Struktura syst´emu
Syst´em klienta
logiku n´aroˇcnˇejˇs´ıch operac´ı, protoˇze v´ ykon poˇc´ıtaˇce na stranˇe robota je limitov´an. V projektu t´eto bakal´aˇrsk´e pr´ace se vˇsak pˇredpokl´ad´a, ˇze poˇc´ıtaˇc robota dok´aˇze v´ ypoˇcetn´ı logiku t´eto pr´ace zvl´adnout, tud´ıˇz server zde pln´ı pouze prvn´ı zm´ınˇen´ yu ´ˇcel, a sice pˇrepos´ıl´a zpr´avy mezi klientem a robotem. D´ıky tomu nebylo nutn´e implementovat vlastn´ı server, protoˇze tuto funkci dok´aˇze poskytnout jiˇz samotn´a platforma Kinbo2 . Pokud nav´ıc uv´aˇz´ıme, ˇze senzor MS Kinect sn´ım´a 30 obr´azk˚ u za vteˇrinu, tak pouze komunikace pˇrepos´ılaj´ıc´ı veˇsker´a tato data mezi robotem a serverem by zabrala velk´e mnoˇzstv´ı ˇcasu.
6.3
Syst´ em klienta
Syst´em klienta nem´a v tomto projektu pˇr´ıliˇs moˇznost´ı. Jeho u ´kolem je zadat robotovi c´ılov´e souˇradnice, na kter´e se m´a pˇresunout. Tyto souˇradnice m˚ uˇze kdykoli za bˇehu programu zmˇenit. Uˇzivatel pomoc´ı syst´emu robota m˚ uˇze tak´e kdykoliv vyvolat ukonˇcen´ı programu. Pro tyto ˇcinnosti m´a uˇzivatel k dispozici jednoduch´e uˇzivatelsk´e rozhran´ı. Po spuˇstˇen´ı projektu se objev´ı u ´vodn´ı okno uˇzivatelsk´eho rozhran´ı klienta (viz obr. 6.1). Jsou zde dvˇe vstupn´ı pole pro zad´an´ı c´ılov´ ych souˇradnic, na kter´e m´a robot dojet. Zad´avat lze pouze cel´a ˇc´ısla, jinak modul klienta ozn´am´ı, ˇze vstupn´ı hodnoty nejsou v poˇra´dku. Pod vstupn´ımi poli jsou dvˇe tlaˇc´ıtka. Tlaˇc´ıtkem Send potvrd´ıme zadan´e souˇradnice a pokud projdou kontrolou, jsou odesl´any do syst´emu robota. Tlaˇc´ıtkem Exit m˚ uˇzeme kdykoliv ukonˇcit ˇcinnost robota. Po stisknut´ı tohoto tlaˇc´ıtka je robotovi odesl´ana ukonˇcovac´ı zpr´ava a je ukonˇcen i bˇeh klienta.
Obr´azek 6.1: Uˇzivatelsk´e rozhran´ı klienta, zad´avac´ı okno
28
Struktura syst´emu
Syst´em robota
Pokud kontrola zadan´ ych souˇradnic probˇehne v poˇr´adku, zmˇen´ı se vzhled okna (viz obr. 6.2). Zmiz´ı zad´avac´ı pole pro souˇradnice a tlaˇc´ıtko Send se zmˇen´ı na tlaˇc´ıtko New coordinates, kter´e kdyˇz stiskneme, objev´ı se opˇet pˇredchoz´ı okno a m˚ uˇzeme zadat nov´e souˇradnice pro robota. Tlaˇc´ıtko Exit m´a i v tomto oknˇe stejnou funkci.
Obr´azek 6.2: Uˇzivatelsk´e rozhran´ı klienta, informaˇcn´ı okno
6.4
Syst´ em robota
Jak je jiˇz zm´ınˇeno u syst´emu serveru, na v´ ypoˇcetn´ı n´aroˇcnosti t´eto pr´ace by mˇel v´ ykon poˇc´ıtaˇce robota postaˇcovat, takˇze veˇsker´e v´ ypoˇcty jsou prov´adˇeny v tomto syst´emu. Po spuˇstˇen´ı syst´em ˇcek´a na povinn´ y vstup, kter´ ym jsou c´ılov´e souˇradnice, na kter´e m´a robot dojet. Tyto souˇradnice jsou od klienta pˇreposl´any pomoc´ı serveru. Po jejich dek´odov´an´ı teprve dojde ke spuˇstˇen´ı samotn´e ˇcinnosti robota a senzoru MS Kinect. S pomoc´ı senzoru je sn´ım´ano okol´ı a jsou v nˇem vyhled´av´any QR k´ody potˇrebn´e pro navigaci robota v prostoru. Sledov´an je tak´e v´ yskyt lid´ı, kteˇr´ı chtˇej´ı interagovat s robotem a pˇri pˇresunech robota tak´e v´ yskyt pˇrek´aˇzek, do kter´ ych by mohl robot narazit. Ned´ılnou souˇca´st´ı syst´emu je tak´e navigaˇcn´ı ˇca´st popsan´a v kapitole 4. Na rozd´ıl od syst´emu klienta je syst´em robota pouze s konzolov´ ym v´ ystupem bez grafick´eho rozhran´ı.
29
Struktura syst´emu
6.5
Propojen´ı modul˚ u
Propojen´ı modul˚ u
Propojen´ı syst´em˚ u je vytvoˇreno v platformˇe Kinbo2 , ale propojen´ı uvnitˇr jednotliv´ ych syst´em˚ u mus´ı vytvoˇrit uˇzivatel s´am. Slouˇz´ı k tomu konfiguraˇcn´ı XML soubor, jehoˇz struktura je pops´ana v [1]. Pokud se pod´ıv´ame na strukturu konfiguraˇcn´ıho XML pro projekt z t´eto pr´ace, najdeme v n´ı definov´any pouze syst´emy klienta a robota. D˚ uvodem je jiˇz zm´ınˇen´ y fakt, ˇze syst´em serveru nebylo tˇreba implementovat. Kdyˇz si pˇribl´ıˇz´ıme syst´em klienta (obr. 6.3), vid´ıme nejprve pouˇzit´e knihovny. Knihovna ClientModule.dll obsahuje uˇzivatelsk´ y modul pro syst´em 2 klienta, zbyl´e knihovny n´aleˇz´ı platformˇe Kinbo . D´ale vid´ıme definovan´ y v´ ystup syst´emu outRobot, kter´ y slouˇz´ı k odesl´an´ı zpr´avy ze syst´emu klienta do syst´emu robota a vstup syst´emu input, na kter´ y jsou pˇrivedeny pˇr´ıchoz´ı zpr´avy z jin´ ych syst´em˚ u. N´asleduje definov´an´ı modul˚ u, v tomto pˇr´ıpadˇe pouze jednoho, kter´ y ˇr´ıd´ı ˇcinnost klienta. Na z´avˇer jsou zde definice propojen´ı. Vstup syst´emu je napojen na vstup modulu a naopak v´ ystup modulu je napojen na v´ ystup syst´emu.
Obr´azek 6.3: Konfigurace syst´emu klienta
30
Struktura syst´emu
Propojen´ı modul˚ u
Kdyˇz si pˇribl´ıˇz´ıme tak´e syst´em robota (obr. 6.4), vid´ıme, ˇze struktura je velmi podobn´a. Nejprve opˇet definice knihoven, RobotModule.dll obsahuje uˇzivatelsk´ y modul pro syst´em robota a zbyl´e knihovny jsou totoˇzn´e jako u klienta. N´asleduj´ı opˇet v´ ystupy syst´emu, outClient slouˇz´ı pro odes´ıl´an´ı zpr´av pro syst´em klienta a motors je v´ ystup pro nastavov´an´ı hodnot motor˚ um. Tento v´ ystup je napojen na platformu Kinbo2 . Na vstup syst´emu input jsou pot´e opˇet pˇrivedeny pˇr´ıchoz´ı zpr´avy z jin´ ych syst´em˚ u. I v tomto syst´emu je definov´an pouze jeden modul, kter´ y po pˇrijet´ı vstupn´ıch dat spouˇst´ı proces robota. Na z´avˇer jsou zde opˇet definice propojen´ı. Vstup syst´emu je napojen na vstup modulu a naopak v´ ystup modulu je napojen na v´ ystup syst´emu. V´ ystup modulu odes´ılaj´ıc´ı zpr´avy motor˚ um je napojen na v´ ystup syst´emu motors.
Obr´azek 6.4: Konfigurace syst´emu robota
31
7 Popis bˇehu programu Nyn´ı si uvedeme pˇr´ıklad, jak m˚ uˇze vypadat pr˚ ubˇeh programu od spuˇstˇen´ı aˇz po dojet´ı robota na poˇzadovanou pozici. Po spuˇstˇen´ı pomoc´ı Runtime aplikace patˇr´ıc´ı k platformˇe Kinbo2 a zad´an´ı konfiguraˇcn´ıho XML souboru se spust´ı uˇzivatelsk´ y projekt. Kromˇe konzolov´ ych ok´enek jednotliv´ ych syst´em˚ u se objev´ı tak´e grafick´e ok´enko klienta, do kter´eho se zad´avaj´ı souˇradnice, na kter´e m´a robot dojet. Po zad´an´ı a potvrzen´ı souˇradnic dojde k odesl´an´ı souˇradnic do syst´emu robota. V nˇem jsou dek´odov´any a uloˇzeny. Po pˇrijet´ı souˇradnic se teprve spust´ı samotn´a ˇcinnost robota. Dojde tedy k inicializaci a spuˇstˇen´ı senzoru MS Kinect a je zah´ajeno zpracov´av´an´ı sn´ımk˚ u poˇr´ızen´ ych t´ımto senzorem. Prvn´ım zkouman´ ym faktorem pˇri kaˇzd´em nov´em sn´ımku je pˇr´ıtomnost ˇclovˇeka v obraze. Pokud je ˇclovˇek nalezen, zkoum´a se, zda robotovi signalizuje implementovan´e gesto. Pokud je gesto rozpozn´ano, spust´ı se urˇcit´a ˇcinnost, kter´a je s dan´ ym gestem spojena. Aby se sn´ıˇzilo riziko faleˇsn´eho rozpozn´an´ı ˇclovˇeka, nebo reagov´an´ı na ˇclovˇeka v obraze, kter´ y nechce interagovat s robotem, je nutn´e nejprve robotovi signalizovat takzvan´e vstupn´ı gesto, kter´e je bl´ıˇze pops´ano v kapitole 5. Po rozpozn´an´ı vstupn´ıho gesta se robot soustˇred´ı pouze na dan´eho ˇclovˇeka a reaguje na jeho povely aˇz do doby, kdy je rozpozn´ano v´ ystupn´ı gesto, nebo ˇclovˇek nezmiz´ı z obrazu. V tento moment se robot vrac´ı k p˚ uvodn´ı ˇcinnosti, tedy ke snaze dojet na zadan´e souˇradnice. Pokud neprob´ıh´a interakce s ˇclovˇekem, je zkoum´an naˇcten´ y barevn´ y sn´ımek, ve kter´em je vyhled´av´an QR k´od, s jehoˇz pomoc´ı by robot dok´azal urˇcit svoji polohu. Pokud nen´ı v obraze nalezen ˇz´adn´ y k´od, z nˇehoˇz by byl robot schopen rozpoznat svoji pozici, je zah´ajeno prohled´av´an´ı okol´ı. Prvn´ım krokem je spuˇstˇen´ı ot´aˇcen´ı robota ve smˇeru hodinov´ ych ruˇciˇcek, aby prohl´edl sv´e okol´ı a pokusil se v nˇem naj´ıt potˇrebn´ y QR k´od. Pokud se mu ˇz´adn´ y k´od ◦ nepodaˇr´ı nal´ezt ani po otoˇcen´ı o 360 , je ot´aˇcen´ı zastaveno a robot se posune o metr dopˇredu, kde se opˇet bude rozhl´ıˇzet. T´ımto zp˚ usobem pokraˇcuje do doby, neˇz se mu podaˇr´ı nal´ezt k´od, nebo dokud nen´ı pˇreruˇsen interaguj´ıc´ım ˇclovˇekem. Je-li v obraze nalezen k´od, je s jeho pomoc´ı urˇcena robotova poloha. Ta je porovn´ana s poˇzadovanou c´ılovou polohou a podle v´ ysledku je zvolen smˇer, kter´ ym robot bude pokraˇcovat d´ale. Robot se nat´aˇc´ı do pˇr´ım´eho smˇeru k c´ıli. Po natoˇcen´ı do spr´avn´eho smˇeru je tedy robotovi nastaveno, ˇze m´a jet 32
Popis bˇehu programu
rovnˇe po dobu, kter´a by mu dle odhadu mˇela staˇcit na dojet´ı k c´ıli. Bˇehem takov´ehoto pˇresunu robot nov´e k´ody nevyhled´av´a. Po dojet´ı na konec takto zadan´e trasy se robot opˇet rozhl´ıˇz´ı a hled´a k´od, s jehoˇz pomoc´ı by dok´azal urˇcit svoji aktu´aln´ı polohu. V pˇr´ıpadˇe, ˇze robot dojel do poˇzadovan´e c´ılov´e pozice, pˇreruˇs´ı svoji ˇcinnost a ohl´as´ı klientovi dokonˇcen´ı ˇcinnosti. Pˇri kaˇzd´em pohybu vpˇred, tedy pˇri vyhled´av´an´ı k´odu i pˇri c´ılen´em pˇresunu na odhadovanou spr´avnou pozici, robot tak´e v kaˇzd´em sn´ımku kontroluje okol´ı, zda se mu v obraze neobjevila pˇrek´aˇzka, do kter´e by mohl narazit. Pokud takovou pˇrek´aˇzku najde, zastav´ı se a zaˇcne se ot´aˇcet do doby, dokud pˇrek´aˇzka z obrazu nezmiz´ı. N´aslednˇe se posune o p˚ ul metru vpˇred ve smˇeru, ve kter´em jiˇz nevid´ı pˇrek´aˇzku, a opˇet spust´ı prohl´ıˇzen´ı okol´ı a vyhled´av´an´ı k´odu pro urˇcen´ı sv´e nov´e pozice. Pˇri ot´aˇcen´ı se na m´ıstˇe nen´ı tato kontrola pˇrek´aˇzek aktivn´ı, protoˇze d´ıky tankov´emu podvozku by robot mˇel b´ yt schopen otoˇcit se prakticky na m´ıstˇe.
33
8 Dosaˇzen´e v´ysledky Bohuˇzel kv˚ uli technick´ ym pot´ıˇz´ım nebyl k dispozici funkˇcn´ı robot Kinbo, na kter´em bych mohl vyzkouˇset bˇeh cel´e pr´ace. Uvedu zde tedy alespoˇ n test pˇresnosti v´ ypoˇctu pozice robota v prostoru, pro jehoˇz proveden´ı postaˇcil senzor MS Kinect. Test je proveden na dvou r˚ uzn´ ych QR k´odech. V tabulce 8.1 jsou uvedeny namˇeˇren´e hodnoty s QR k´odem 300,300,0. Prvn´ı sloupec oznaˇcuje skuteˇcn´e suˇradnice, na kter´ ych byl um´ıstˇen senzor, ve druh´em sloupci jsou uvedeny vypoˇcten´e souˇradnice a posledn´ı sloupec obsahuje odchylku tˇechto dvou pozic. Souˇradnice i odchylka jsou uvedeny v centimetrech. Tabulka 8.1: Namˇeˇren´e hodnoty s QR k´odem 300,300,0 Skuteˇcn´a pozice Vypoˇcten´a pozice Odchylka [cm] 300; 200 298,2; 198,4 2,41 290; 200 291,86; 198,33 2,5 310; 200 308,47; 199,13 1,76 310; 180 307,56; 179,02 2,63 300; 180 294,83; 178,9 5,29 290; 180 288,14; 179,35 1,97 V tabulce 8.2 jsou uvedeny namˇeˇren´e hodnoty s QR k´odem 100,100,180. Struktura tabulky je totoˇzn´a jako v pˇredchoz´ım pˇr´ıpadˇe. Tabulka 8.2: Namˇeˇren´e hodnoty s QR k´odem 100,100,180 Skuteˇcn´a pozice Vypoˇcten´a pozice Odchylka [cm] 100; 200 102,51; 203,67 4,45 90; 200 90,99; 203,26 3,41 110; 200 108,15; 202,27 2,93 110; 220 113,53; 221,64 3,89 100; 220 98,83; 221,2 1,68 90; 220 91,81; 222,06 2,74 Jak je z tabulek vidˇet, odchylka nepˇresahuje 10 centimetr˚ u, coˇz je pro tuto pr´aci dostaˇcuj´ıc´ı pˇresnost. Vytvoˇren´ y algoritmus pro nalezen´ı pozice robota je tedy podle proveden´ ych test˚ u dostateˇcnˇe kvalitn´ı.
34
9 Z´avˇer V r´amci t´eto pr´ace byly pops´any postupy pro urˇcen´ı pozice robota z nalezen´eho QR k´odu a n´asledn´a pravidla pro pohyb robota prostorem. V pr´aci byl tak´e pops´an zp˚ usob rozpozn´av´an´ı ˇclovˇeka a j´ım signalizovan´ ych gest. Vˇsechny tyto uveden´e postupy byly tak´e implementov´any do v´ ysledn´e pr´ace. Odchylka vypoˇcten´e pozice robota v˚ uˇci re´aln´e pozici se bˇehem test˚ u pohybovala do 10 centimetr˚ u, coˇz je pro tento u ´ˇcel dostateˇcn´a pˇresnost. C´ıl t´eto pr´ace, vytvoˇrit moduly pro urˇcen´ı pozice robota a interakci s ˇclovˇekem, byl dosaˇzen vytvoˇren´ım jednotliv´ ych modul˚ u obsahuj´ıc´ıch funkcionalitu popsan´ ych problematik. Moduly jsou reprezentov´any jako dll knihovny, kter´e lze snadno pomoc´ı konfiguraˇcn´ıho XML souboru propojit s platformou Kinbo2 . Bohuˇzel vzhledem k tomu, ˇze robot nebyl technicky pˇripraven, nebylo moˇzn´e na nˇem pr´aci otestovat, takˇze veˇsker´e testy mohly prob´ıhat pouze na ˇc´asti pr´ace, kter´e pracovaly s obrazem zaznamenan´ ym senzorem MS Kinect. Nebylo tedy moˇzn´e otestovat pˇredevˇs´ım navigaˇcn´ı ˇc´ast pro pohyb robota v prostoru.
35
Pˇ rehled zkratek QR k´ od - Quick Response“ k´od, tedy k´od rychl´e reakce ” MS - Microsoft RGB - Red, Green, Blue. Zkratka berevn´eho modelu, kde se vˇsechny barvy skl´adaj´ı ze tˇr´ı z´akladn´ıch barev: ˇcerven´a-zelen´a-modr´a SDK - software development kit XML - Extensible Markup Language, znaˇckovac´ı jazyk TCP - Transmission Control Protocol
36
Literatura [1]
ALTMAN, P. Modul´arn´ı v´yvojov´a platforma Kinbo2 Plzeˇ n, 2013. Oborov´ y projekt na Fakultˇe aplikovan´ ych vˇed Z´apadoˇcesk´e univerzity na katedˇre informatiky a v´ ypoˇcetn´ı techniky. Vedouc´ı projektu Ing. Petr Vanˇeˇcek, Ph.D.
[2]
ZXing.Net [poˇc´ıtaˇcov´ y soubor]. Ver. 0.12.0.0. 2014 [cit. 4.2014]. http://zxingnet.codeplex.com/releases/view/106588
[3]
Microsoft Kinect SDK [poˇc´ıtaˇcov´ y soubor]. Ver. 1.8 for Windows 2013 [cit. 4.2014]. http://www.microsoft.com/en-us/kinectforwindowsdev/Downloads.aspx
[4]
Kinect for Windows Sensor Microsoft Developer Network [online]. 2014 [cit. 4.2014]. http://msdn.microsoft.com/en-us/library/hh855355.aspx
[5]
Korespondenˇcn´ı semin´aˇr z programov´an´ı [cit. 4.2014]. http://ksp.mff.cuni.cz/tasks/24/cook5.html
[6]
ANDERSEN, M.R. - JENSEN, T. - LISOUSKI, P. - MORTENSEN, A.K. - HANSEN, M.K. - GREGERSEN, T. - AHRENDT, P. Kinect Depth Sensor Evaluation for Computer Vision Applications [online]. 2012 [cit. 4.2014]. http://eng.au.dk/fileadmin/DJF/ENG/PDFfiler/Tekniske rapporter/Technical Report ECE-TR-6samlet.pdf
[7]
Skeletal Tracking Microsoft Developer Network [online]. 2014 [cit. 4.2014]. http://msdn.microsoft.com/en-us/library/hh973074.aspx
37
[online].
2012
LITERATURA
LITERATURA
[8]
QR code generator [online]. 2014 [cit. 5.2014]. http://goqr.me
[9]
ˇ ˇ ˇ ´IHA, K. Detekce a sledov´an´ı POSP´ISIL, A. - PRINOSIL, J. - R polohy hlavy ve video sekvenc´ıch s vyuˇzit´ım zaˇr´ızen´ı Microsoft Kinect [PDF]. Publikov´ano 3.1.2012 [cit. 5.2014]. Dostupn´e z: http://www.elektrorevue.cz/cz/clanky/zpracovanisignalu/5/detekce-a-sledovani-polohy-hlavy-ve-video-sekvencichs-vyuzitim-zarizeni-microsoft-kinect/
38
A Pˇr´ıloha A.1
Uˇ zivatelsk´ a pˇ r´ıruˇ cka
Ke spuˇstˇen´ı uˇzivatelsk´eho projektu vytvoˇren´eho v t´eto pr´aci mus´ı b´ yt nej2 prve spuˇstˇeny syst´emy klienta, serveru i robota platformy Kinbo . K tomu je zapotˇreb´ı pouˇz´ıt runtime aplikaci, kter´a je souˇc´ast´ı modul´arn´ı platformy. Pro snaˇzˇs´ı spuˇstˇen´ı jsou u platformy vytvoˇreny pro jednotliv´e syst´emy tak´e d´avkov´e soubory. Syst´emy lze tedy jednoduˇse spustit soubory server.bat, client.bat a robot.bat. Pokud je platforma spouˇstˇena pouze na lok´aln´ım poˇc´ıtaˇci, lze jednoduˇse spustit vˇsechny tˇri syst´emy d´avkov´ ym souborem __lokalKinbo.bat. Po spuˇstˇen´ı syst´emu klienta se objev´ı spouˇstˇec´ı okno uˇzivatelsk´eho projektu (viz obr. A.1). Tlaˇc´ıtkem Browse vybereme konfiguraˇcn´ı XML uˇzivatelsk´eho projektu, kter´ y chceme spustit a potvrd´ıme volbu tlaˇc´ıtkem Run. Pokud jsou k dispozici vˇsechny potˇrebn´e knihovny, je spuˇstˇen uˇzivatelsk´ y projekt a objev´ı se uˇzivatelsk´e rozhran´ı klienta ze spuˇstˇen´eho projektu, jehoˇz ovl´ad´an´ı je pops´ano v sekci 6.3. Po zad´an´ı c´ılov´ ych souˇradnic je spuˇstˇena ˇcinnost robota, kter´ y jiˇz pracuje bez interakce s klientem.
Obr´azek A.1: Spouˇstˇec´ı okno platformy Kinbo2
39
