Plánování cesty ramene manipulátoru se 3 stupni volnosti

Transkript

1 Intelgentní robotka - samostatná práce: Plánoání cest ramene manplátor se stpn olnost Renáta Smoloňoá, Robert alama, Petr Pošík 9.. Katedra kbernetk Faklta elektrotechncká České soké čená techncké Praze Abstrakt Řešení problém nalezení optmální cest ramene manplátor se stpn olnost se dá separoat na tř nezáslé část. Prní z nch je rčení matematckého pops transformace mez sořadncem prostor obraz snímaného kamero a sořadncem e sktečném sětě. ransformační matc rčjeme ze čtř kalbračních bodů a pátý daný bod žíáme k oěření její spránost. Drho částí je podrobný matematcký pops knematk manplátor (řešení přímé a nerzní knematcké úloh). řetí část pak toří lastní algortms pro nalezení nejkratší cest mez počátečním a cíloým bodem. Algortms zohledňje překážk na cestě, jejchž poloh detekje poze na zdálenost jedna kloboých sořadncích. Úloh tohoto tp toří podstatno sočást složtějších úloh řešených pra, a proto je třeba pochopt a oěřt s základní prncp jejch řešení na jednodšších úlohách. Úod Cílem práce je nalezení optmální řídící sekence pro rameno manplátor, který má přemístt předmět z bod B do bod A. Ve scéně jso místěn překážk a celá scéna je snímána kamero. Překážk rameno detekje pomocí čdla, jež je místěno na jeho konc a které je schopno detekoat překážk zdáleno krok kloboých sořadncích. Řešení úloh jsme rozděll na tř podúloh. Pro každo z nch jsme přjal některé zjednodšjící podmínk a omezení. Př hledání matce přechod od obrazoých sořadnc k sořadncím reálném sětě předpokládáme, že scéna je snímána kamero, ktero lze popsat perspektním modelem. Kamera je po dob měření místěna neznámé, ale e stále stejné pozc (msí být zajštěna platnost nalezené matce přechod). Vzhledem k tom, že máme k dspozc jen postačjící počet kalbračních bodů, počítáme matc přechod přímo řešením sosta lneárních ronc (nepožíáme metod pro řešení přerčené sosta ln. ronc, např. SVD [Krajník998]). Př torbě lastního algortm pro nalezení optmální cest přjímáme nerealstcký předpoklad, že ramena manplátor jso nehmotná. Vhýbat se překážkám msí poze koncoý bod manplátor nesocí předmět. Otázko chopení a olnění předmět se této úloze podrobně nezabýáme a dále předpokládáme, že počáteční a koncoý sta se nacházejí praconím prostor robota. ato zpráa má následjící rozdělení: kaptole je popsán způsob transformace bodů detekoaných obraz snímaném kamero do sořadné sosta spojené s podložko, na které leží předmět překážk, kaptola popsje knematk manplátor a řešení přímé nerzní

2 knematcké úloh, kaptola obsahje odození algortm pro rčení pohb ramene a kaptole 5 najdete zhodnocení ýsledků a záěr. ransformace ze SS obraz kamer do SS spojené s podložko Matce přechod je matematcká transformace, která možňje nazájem přepočítáat sořadnce prostor obraz do sořadnc e sětě a naopak. Předpokládáme, že obraz, který máme k dspozc, bl sejmt perspektní kamero. Je třeba s ědomt, že transformace odozená dalším tet se týká poze bodů ležících roně podložk. Kamera sce snímá celo scén, ale jedně tehd, kdž íme, že bod obraz ležel roně, pro ktero bla transformace tořena, j můžeme požít. Kamera této úloze není požta pro snímání poloh a rozměrů překážek. Zjšťje se pomocí ní poze pozce počátečního a cíloého bod konce ramena manplátor. K dspozc máme 5 bodů, jejchž sořadnce SS podložk známe. Pozce jejch obrazů e snímk kamer jsme schopn rčt. Zjšťjeme je pomocí fnkce MALAB gnpt. Pozce bodů rčjeme manálně pomocí mš, čímž se do měření náší chba způsobená ldským faktorem. Abchom její l omezl, odečítáme sořadnce každého bod 5 a ýsledné sořadnce získááme jednodchým průměroáním. Vzhledem k tom, že toření transformace je proces jednorázoý (transformace zůstáá stejná, pokd se nezmění zájemná orentace a pozce podložk a kamer) a pra b j neblo třeba proádět často, poažjeme toto řešení za postačjící. Naíc je tento způsob rchlý a jednodchý. Pokd b blo třeba proádět zjšťoání transformace atomatck, dalo b se žít např. sronáání se zorem, které s sebo ale nese jná další úskalí. Pro další ýpočt jsme cházel z následjících hodnot: Číslo bod Sořadnce bod reálném sětě Sořadnce bodů e snímk kamer [mm] [pel] (,, ) (66.98,.5) (8,, ) (5.5, 88.5) (8, 77, ) (79.6, 6.5) (, 77, ) (.7, 6.) Podle [Pajdla] lze transformac sořadnc zájemně s odpoídajících bodů do ron perspektní projekc popsat roncí kde!! α =, (.) ato matcoá ronce předstaje sosta lneárních ronc. Je možné ze. ronce jádřt α a dosadt za něj do prních do ronc. Pro každý bod pak máme dojc ronc. kde! =! = α h ( ) ( ) h = h h h je ektor sořadnc obraz bod e snímk kamer [pel], je ektor sořadnc bod SS spojené s podložko [mm], je matce přechod od SS podložk do SS snímk, rozměr matce je je pro každý bod konstanta; zohledňje zdálenost bod od střed projekce =, ektor odpoídající řádkům matce

3 Proedeme-l tento postp pro bod, dostaneme sosta 8 ronc o 9 neznámých. Zapsána matcoě padá takto: = (.) Koefcent matce zapsané do ektor toří praý nloý prostor matce sosta, jejíž koefcent šechn známe. Báz praého nloého prostor matce sosta (ektor koefcentů ) jsme zjstl pomocí příkaz nll sstém MALAB. Pokd z ažoaných bodů jso každé lneárně nezáslé (což je našem případě splněno), má matce sosta hodnost 8 a báz nloého prostor toří jedný ektor. Přespořádáním prků ektor zpět do matce získáme matc přechod od SS podložk do SS snímk kamer. M bdeme ale potřeboat transformac opačno, totž ze SS snímk do sktečného sěta, ted do SS podložk. K tom lze žít lneárnost zobrazení () a pro reglární můžeme psát!! = α (.) Knematka manplátor Určení knematckých lastností manplátor je ntno podmínko ke zdárném řešení celé úloh. Dáá nám odpoěd na otázk, kde se nacházejí klob a ramena manplátor, kdž známe úhl klobů a délk ramen (přímá knematcká úloha, přeod se sořadnc kloboých do sořadnc kartézských), a jak msíme nastat úhl klobů manplátor, ab se koncoý bod ramene nacházel požadoaných kartézských sořadncích (nerzní knematcká úloha). Rameno manplátor, jeho pops a zolené sořadné sosta jso schématck znázorněn příloze na obr... Přímá knematcká úloha Přímo knematcko úloho se rozmí rčení poloh těles a klobů manplátor př známých hodnotách nezáslých parametrů ( našem případě úhlů klobů). Zjednodšeně řečeno, jedná se o přeod z kloboých sořadnc do sořadnc kartézských. Strktr transformačních matc jsme oll podle [Smtný999]. Bod je třeba zadáat homogenních sořadncích. Sestaení transformační matce lze rozložt na dílčí transformace mez 5 SS znázorněným příloze na Obr... ransformace z SS (záěs) do SS (podložka) skládá se z rotace bod o 9º kladném smsl kolem os a posn o ektor O O. ransformace z SS (rameno) do SS (záěs) rotace bod o úhel γ záporném smsl kolem os (bez pos). ransformace z SS (loket) do SS (rameno) rotace bod o úhel ϕ záporném smsl kolem os a posn o ektor O O ose. ransformace z SS (zápěstí) do SS (loket) rotace bod o úhel ε záporném smsl kolem os a posn o ektor O O ose.

4 OO ( OO ) ( OO ) = (.) z cosϕ snϕ OO = (.) snϕ cosϕ cosγ snγ = sn cos (.) γ γ cosε sn ε OO = (.) sn ε cosε Celkoá transformační matce je sočnem matc až. Pozce koncoého bod ramene manplátor se pak spočítá jako OB 5 k = k = (.5) kde je ektor sořadnc koncoého bod manplátor k SS (podložka) ( ) je ektor sořadnc koncoého bod manplátor 5 k = OB SS (zápěstí). Inerzní knematcká úloha Inerzní knematcká úloha (IKÚ) je obecně složtější problém než přímá knematcká úloha (PKÚ). Zatímco PKÚ má žd řešení (manplátor se někde e sém operačním prostor prostě nacházet msí), IKÚ nemsí mít řešení žádné, může mít řešení jedno, konečný počet nebo jch má nekonečně mnoho. V našem případě je řešení IKÚ značně jednodché. Protože se celá podložka nachází praconím prostor manplátor, bde mít IKÚ čtř řešení pro každý bod. Stace je znázorněna příloze na obr... Úhel γ snadno počteme z -oé a -oé složk ektor ( OB) ( O B. γ = arctan (.6) Pro ýpočet úhlů ϕ a ε jsme žl kosínoo ět. V hodnotách těchto úhlů se projeí nejednoznačnost řešení. ( OB) ϕ z = arccos (.7) O B OO + OB OO5 ϕ = arccos (.8) O O O B

5 OO + OO5 OB ε = arccos (.9) O O O O 5 Z těchto dílčích ýsledků lze sestat čtř trojce úhlů γ, ϕ a ε : γ ϕ = ϕ + ϕ ε = π + ε γ = γ ϕ = ϕ ϕ ε = π ε γ = γ + π ϕ = ϕ ε = ε γ = γ + π ϕ = ϕ ε = ε Všechn trojce jso řešením IKÚ.. a. trojce přtom předstají (dík osoé smetr manplátor) shodné pozce ramen prostor, stejně jako. a. trojce.. a. trojce nastaení úhlů je ted ntná poze tehd, pokd úhel γ překročí sůj poolený rozsah.. Oěření PKÚ a IKÚ Spránost přímé a nerzní knematcké úloh jsme oěřoal na třech zolených bodech, které jsme nejpre přeedl na kloboé sořadnce (pomocí IKÚ) a následně zpět do kartézských sořadnc (pomocí PKÚ). Oěření blo proedeno stém MALAB. Z tablk. je zřejmé, že bod prním a třetím slopc jso shodné, čímž jsme potrdl spránost PKÚ a IKÚ. ab.. Oěření spránost PKÚ a IKÚ. zolený bod (,, z) IKÚ (γ, ϕ,ε ) PKÚ (,, z) (9, 8, ) (,.55, -.85) (9, 8, ) (, 77, ) (.55,.667, -.595) (-, 77, ) (8, 77, ) (.666,.6669, -.59) (8, 77, ) Algortms pro pohb ramene manplátor Jedná se o prohledáaní staoého prostor, který je omezen elkost praconí ploch. Naším úkolem je nalezení takoé poslopnost operátorů, jejíž postpno aplkací razí rameno manplátor nejkratší tras z bod B do bod A př respektoání překážk. Přemístění předmět se skládá ze tří kroků: přemístění prázdného ramene manplátor do pozce B, chopení předmět a přenesení předmět do pozce A. Otázko chopení předmět se nezabýáme, předpokládáme, že předmět je hmotný bod a je chopen, kdž do bod místíme konec manplátor. Př přesn manplátor ažjeme, že manplátor je nehmotný, tzn. že prochází překážkam na rozdíl od hmotného bod.. Zjštění sořadnc počátečního a koncoého bod Bod A a B jso na praconí ploše značen značkam, které díme kameře. Sořadnce obo bodů reálném sětě ( SS podložk) zjstíme pomocí matce přechod, ktero jsme počítal kap.. Z reálných sořadnc bod A s pomocí řešení IKÚ (z kap..) počítáme příslšné úhl manplátor. to počtené úhl msíme prat na úhl, které můžeme na manplátor sktečně nastat. oto úprao se bod A posne do jné poloh. Bohžel se této chbě rčení počátečního bod nedá zabránt, neboť máme možnost poze dskrétního nastaoání úhlů manplátor. Abchom alespoň tto chb mnmalzoal, generoal jsme osm nejblžších staů manplátor (odozeno od počt krokoých motorků) okolí bod A a bral jsme ten sta, který měl mnmální ekledosko zdálenost od bod A. Př této úpraě jsme řadl sta nacházející se pod úroní podložk.

6 . Vlastní algortms optmální cest. algortms Prncp:. lož S do N, počt e(n). jestlže N = {} -> řešení nenalezeno, CESA = {}. N lož do CLOSED a do CESA. jestlže N = C -> řešení nalezeno (e sktečnost testjeme, zda se jž nacházíme nám zoleném okolí cíle C) 5. epanze N -> 6 staů D() 6. ločení nepřípstných D() nacházejí se mmo praconí prostor, překážce, nebo se do nch kůl překážce nelze dostat 7. ločení D(), které jso CLOSED 8. není-l žádný D() přípstný, N = {}, jd na krok 9. pro šechn přípstné D() spočt e(d()) a D() s nejmenší e(n) lož do N. jd na krok S N CLOSED e(n) C D CESA počáteční sta aktální sta seznam staů, kterým jsme jž prošl ekledoská zdálenost z N do cíle cíloý sta množna následjících staů poslopnost staů z S do N Z pops algortm je zřejmé, že se jedná o algortms gradentní (z každého sta se slepě přesne do sta, který m dano chíl přnáší nejětší zsk). Výhodo tohoto algortm je, že pro jednodché geometrcké tar překážek najde řešení poměrně rchle. Na drho stran, nejedná se o přípstný algortms, neboť se snadno může stát, že algortms nějakém sta ízne, protože se nemí racet do žádného z staů mnlých. V příloze na Obr.. je kázka cest ramene manplátor dle algortm.. algortms K nalezení optmální cest jsme zoll algortms nformoaného prohledáání staoého prostor s herstcko fnkc A *. Odhad ohodnocení sta n je defnoán jako : f ( n) = g( n) + h( n), kde f (n) odhad ohodnocení sta n g (n) odhad cen cest z počátečního sta do sta n předstaoaný zatím nejkratší cesto h (n) herstcká fnkce - odhad mnmální cen z zl n do koncoého sta Přípstnost algortm Algortms A * je přípstný [Pajdla], případě, že estje jen konečný počet následoníků, cena každého sta je kladná a odhad fnkce h(n) je žd spodním odhadem cen sktečné cest. V našem případě pro každý sta estje šest možných následjících staů, sta jso ohodnocen kladno hodnoto, fnkc h (n) počítáme jako ekledosko zdálenost ze sta n do koncoého sta bez ažoání překážk. Ekledoská zdálenost je nejkratší možná

7 zdálenost mez děm bod ekledoském prostor tzn., že náš odhad nebde nkd horší než sktečná cena a tdíž je tento algortms přípstný. Prncp:. lož S do OPEN, počt f(s). OPEN = {} -> řešení nenalezeno. ber z OPEN sta N s nejmenší f(n) a lož jej do CLOSED.. jestlže N = C -> řešení nalezeno 5. epanze N, jel D = {} jd na 6. ločení nepřípstných staů D 7. pro každý D spočt f(d) a proed krok 8 a 9 8. není-l D OPEN an CLOSED, lož D do OPEN. 9. je-l D OPEN nebo CLOSED, přřadíme D menší z hodnot f(d) a mnlé hodnot. Přesň D CLOSED, kterém se zmenšla hodnota do OPEN. krok S A OPEN CLOSED f(n) D počáteční sta aktální sta seznam staů, kterých očekááme epanz seznam staů, kterých neočekááme epanz odhad ohodnocení sta množna následjících staů V obo algortmech jso problém s testoáním cíloého sta, neboť z důod dskrétního nastaení úhlů není jednoznačně rčeno, zda jž se nacházíme nejblžším sta k cíloém sta nebo nkol. Oba algortm jso končen okamžk, kd se nacházíme rčté blízkost k cíloém sta. Můžeme se tím dopstt jsté chb, ale bohžel jsme nenalezl lepší metod. V algortm A * platíme za přípstnost algortm zětšením počt staů jednodchých překážek, kterým msíme projít, než se dostaneme do cíle. Pro složtější překážk máme šak zarčeno, že pokd řešení estje bde nalezeno a to je hlaním požadakem algortm. 5 Zhodnocení a záěr Reference [Krajník998] E. Krajník : Matcoý počet. ČVU, Praha, 998 [Mařík99] V.Mařík, O.Štěpánkoá, J.Lažanský: Umělá ntelgence I. Academa, Praha, 99. [Pajdla].Pajdla: Intelgentní robotka. Poznámk k přednáškám předmět Intelgentmí robotka. [Smtný999] V.Smtný: Knematka. Poznámk k přednáškám předmět Robotka [Šára999] R.Šára: Obecná teore sstémů. Poznámk k přednáškám předmět OS.

8 Příloha Obr. : Schématcké znázornění robota pro účel PKÚ a IKÚ Cesta ramene manplátor Cesta ramene manplátor Obr.. a Obr.. b Obr.. a Pohled ze shora na cest ramene manplátor podle algortm. Počáteční bod je označen modře, koncoý zeleně. Praconí prostor je omezen osam a. Z tohoto důod došlo k obejtí překážk. Obr.. b Bokors cest manplátor