Samostatná práce z předmětu Inteligentní robotika Předběžné řešení (7.12.2000) Karel Čepelák, Tomáš Ditrich, Radek Pavlíček Katedra kybernetiky České vysoké učení technické Technická 2, Praha 6, 16627
Abstrakt Cílem této práce je vyřešit problém manipulátoru, přenášejícího hmotný bod z počátečního do koncového bodu na podložce. Na podložce kromě hmotného bodu jsou umístěny blíže neupřesněné překážky, kterým se musí hmotný bod vyhnout. Vstupem úlohy je obraz scény z perspektivní kamery, ze kterého získáme souřadnice počátečního a koncového bodu. Pro pohyb robota je vyřešena přímá a inverzní kinematická úloha. Nakonec je popsán přípustný plánovací algoritmus, řešící vyhýbání se hmotného bodu překážkám.
1. Úvod V praxi se stále častěji setkáváme s manipulátory používanými v průmyslových aplikacích. V rámci předmětu Inteligentní robotika řešíme cvičný příklad problému manipulátoru spolupracujícího s kamerou. Zadání této úlohy je podrobně popsáno v [2]. V práci budou používány tři druhy souřadnic. Kartézské souřadnice robota v trojdimenzionálním prostoru, dále jen kartézské souřadnice. Tyto souřadnice jsou použity v obrázcích 1. a 2. Dalšími souřadnicemi budou pixelové souřadnice obrazových bodů v obrázku z kamery, dále jen pixelové souřadnice. Posledními souřadnicemi jsou kloubové souřadnice robota, udávající nastavení jednotlivých krokových motorů robota, dále jen kloubové souřadnice. Rozsah kloubových souřadnic je uveden v tabulce 1, ve sloupci počet kroků. Kloub E F G
Úhel Rozsah v úhlech Počet kroků -100 až 100 0 až 100 γ -100 až 100 0 až 100 β -170 až 170 0 až 100 α Tabulka 1. Rozsahy kloubů manipulátoru
3. Řešení úlohy Strukturu řešení problému jsme rozdělili do třech částí. V první části bude řešen problém polohy počátečního a koncového bodu přenášeného bodu, v souřadnicích robota. Ve druhé části budeme řešit kinematickou úlohu robota. Ve třetí části pak budeme navrhovat plánovací algoritmus, který bude řešit problém vyhýbání se hmotného bodu překážkám a plánování nejkratší cesty.
3.1. Problém polohy počátečního a koncového bodu V této části bude řešena poloha počátečního bodu B a koncového bodu A přenášeného předmětu v kartézských souřadnicích robota. K dispozici máme obrázek z perspektivní kamery, umístěné na neznámém místě nad pracovní plochou. Na tomto obrázku je umístěno pět lícovacích značek, jejihž souřadnice v kartézských souřadnicích robota známe. Dále jsou na obrázku vyznačeny počáteční a koncový bod, jejichž souřadnice v kartézských souřadnicích robota neznáme. Z obrázku ručně odečteme pixelové souřadnice čtyř krajních lícovacích značek. S využitím pixelových a kartézských souřadnic těchto značek vytvoříme transformační matici pro přepočet pixelových souřadnic na kartézské souřadnice. Prostřední značku použijeme na ověření správnosti transformační matice. Úloha je řešena pomocí homogenních souřadnic které získáme z kartézských přidáním další souřadnice rovno jedné a vynásobení celého vektoru libovolnou konstantou. Mezi pixelovými souřadnicemi kamery a kartézskými sořadnicemi na pracovní ploše platí transformační vztah éu ù é xα ù ê yα ú = T ⋅ ê v ú ê ú ê ú êë1 úû êë α úû (1) éa b c ù T = êê d e f úú êë g h i úû kde [xα,yα,α] jsou homogenní souřadnice bodu na pracovní ploše, [u,v,1] jsou souřadnice bodu v obraze v pixelových homogenních souřadnicích, T je transformační matice. Z toho vyplývají rovnice pro výpočet transformační matice u ⋅ a + v ⋅ b + c − xu ⋅ g − xv ⋅ h − x ⋅ i = 0 . (2) u ⋅ d + v ⋅ e + f − yu ⋅ g − yv ⋅ h − y ⋅ i = 0 Pro určení matice T stačí čtyři body, u kterých známe kartézské souřadnice na pracovní ploše a pixelové souřadnice v obrazu. Z obrázku odečteme pixelové souřadnice bodů A a B. Tyto souřadnice převedeme pomocí transformační matice T na souřadnice kartézské, které potřebujeme pro řešení dalších bodů úlohy. V této části byla řešena korespondence mezi body A a B z obrázku kamery a počáteční a cílovou polohou přenášeného předmětu na pracovní ploše.
3.2. Kinematika robota V této části, řešením přímé a inverzní kinematické úlohy dostaneme vztahy pro vzájemný přepočet kartézských a kloubových souřadnic. Použitý manipulátor má tři rotační klouby, tedy tři stupně volnosti. Uspořádání manipulátoru je na obrázku 1. Manipulátor tvoří tři ramena a, b, c. Manipulátor je umístěn v kartézských souřadnicích [x0, y0, z0]. Koncový bod manipulátoru má kartézské souřadnice vzhledem k pracovní ploše [x, y, z] a vzhledem k bodu P kartézské souřadnice [x’, y‘, z‘]. Kloubové souřadnice jsou [u, v, w], kterým odpovídají úhly α,≡β,≡γ, viz tabulka 1.
z’ [x0,y0,z0] a P
y’ α β
z
x’ b
-γ c [x,y,z]
y x Obrázek 1. Prostorové uspořádání manipulátoru Řešení přímé kinematické úlohy provedeme pomocí homogenních souřadnic, které dostaneme přidáním čtvrté souřadnice o velikosti jedna ke kartézským souřadnicím. Transformace kartézských souřadnic vztažených ke koncovému bodu do kartézských souřadnic pracovní plochy s využitím geometrických vazeb je popsána maticovou rovnicí é1 0 ê0 1 Α=ê ê0 0 ê ë0 0 H H x = Α.x1
0 x0ù é sinα cosα 0 0ù é sinβ 0 y0úú êê− cosα sinα 0 0úú êê 0 ⋅ ⋅ 1 z0 ú ê 0 0 1 0ú ê− cosβ ú ê ú ê 0 1û ë 0 0 0 1û ë 0
0 cosβ 0 ù écosγ 1 0 0 úú êê 0 ⋅ 0 sinβ − a ú ê sinγ ú ê 0 0 1û ë 0
0 − sinγ 1 0 0 cosγ 0 0
bù é1 0úú êê0 ⋅ 0ú ê0 ú ê 1û ë0
0 1 0 0
0 0 1 0
cù 0úú , 0ú ú 1û
L L kde x je vektor souřadnic bodu vztažených k pracovní ploše a x1 vektor souřadnic vztažených ke koncovému bodu manipulátoru. Poloha koncového bodu je pak dána vztahem é xù é0ù ê yú ê ê ú = Α ⋅ ê0 , êzú ê0 ê ú ê ë1 ë1
Inverzní kinematickou úlohu vyřešíme pomocí geometrických vztahů mezi rameny manipulátoru, viz obrázek 2.
[1]
[2]
z’ y’ P α’
x’
b
β’’ γ’
β’
l
c
r
[x’,y’,z’]
Obrázek 2. Geometrického znázornění řešení inverzní kinematické úlohy Výsledné vztahy jsou x ' = ( x − x 0) y ' = ( y − y 0) z ' = ( z − z 0 + a)
,
[3]
r = x' 2 + y ' 2 , y' α ' = ar cos(− ) pro x’≥0 , r>0 r y' α ' = − ar cos(− ) pro x’<0 , r>0 r + α '∈ R pro r=0 ,
[4]
l = r 2 + z'2 b2 + l 2 − c2 β’’ = arccos( ) 2bl z' β ' = arccos(− ) , l 2 b + c2 − l 2 γ ' = arccos 2bc
,
[5]
,
[6] [7]
,
[8]
,
[9]
[10] ,
y' ≤ sin(80°) a r>0 nebo r=0, pak α=α’, β=β’+β’’ a γ=γ’-180° nebo r β=β’-β’’ a γ=180°-γ .
Pokud
[11]
y' ≤ sin(80°) a r>0 , pak α=α‘-180° pro α‘>0°, α=α‘+180° pro α‘<0° , r β=-β’+β’’ a γ=γ’-180° nebo β=-β’-β’’ a γ=180°-γ .
Pokud −
Kde r, l,α‘ ,β’ ,=β’’ ,γ’ jsou pomocné proměnné pro výpočet, jejichž geometrická interpretace je na obrázku 2. Jednoznačnost inverzní kinematické úlohy je zaručena následujícími omezeními. Úhel α volíme vždy v intervalu (-90°, 90° > a úhel γ je vždy menší nebo roven nule. Přímá kinematická úloha je jednoznačná. V úloze inverzní je řešení více. Pro pracovní plochu platí, že vně kruhových výsečíkde α∈<-10,10>∪<170,190>, dostaneme čtyři různá řešení a uvnitř výsečí dvě.
3.3. Plánovací algoritmus V tomto bodě bude popsán plánovací algoritmus pro plánování cesty přenášeného předmětu z počátečního do cílového bodu. Předmět se musí vyhýbat překážkám, které jsou umístěny na pracovní ploše. Uvažujeme zjednodušení úlohy, kdy překážkám se musí vyhybat pouze přenášený předmět ve tvaru hmotného bodu. Ramena robota překážkami mohou volně procházet. Procházený stavový prostor je v kloubových souřadnicích. Jednotlivé body tohoto prostoru nazýváme uzly. Následovník uzlu je takový uzel, na který je možné přejít změnou jedné souřadnice o hodnotu jedna. Neprůchozí uzel je takový uzel, který nemá žádného následovníka kromě již prošlých uzlů. Průchozí uzel má alespoň jednoho následovníka, který není prošlý. Algoritmus má k dispozici dva seznamy prošlých uzlů. První seznam P obsahuje úspěšně prošlé uzly a reprezentuje tak dosud uraženou cestu ve stavovém prostoru. Druhý seznam N obsahuje neprůchodné uzly. Popis algoritmu: 1) Jsme v uzlu Xi, kde i je pořadové číslo uzlu seznamu P 2) Pro uzel Xi vygenerujeme seznam všech šesti následníků V (generace bodů i+1) 3) Z vygenerovaného seznamu V vyřadíme uzly obsažené v některém ze seznamů P a N a také vyřadíme uzly ležící mimo rozsah manipulátoru 4) Pro zbylé spočteme heuristickou funkci a seřadíme seznam V podle ní Je li pro některý z nich fce h’(n i+1) = 0, našli jsme cíl a končíme 5) Postupně testujeme průchodnost do jednotlivých bodů seznamu V. Neprůchodné uzly zařadíme do seznamu N. V případě, že uzel ze seznamu V je průchodný, uzel Xi zapíšeme na konec seznamu prošlých uzlů P , i ⇐ i+1 a přesouváme se do bodu 1). 6) Žádný uzel ze seznamu V není průchodný. Je třeba se vrátit zpátky na uzel Xi-1, který nalezneme na konci seznamu P, odkud ho musíme vymazat. Uzel Xi zapíšeme do seznamu N, i ⇐ i-1 a vracíme se k bodu 1). Je li seznam P prázdný, konec s chybou. V algoritmu použitá heuristická funkce je f‘(n i+1) = g‘(n i+1) + h‘(n i+1) , kde g‘(n i+1) je odhad ceny nejlevnější cesty ze startu do uzlu s pořadím i+1 a h‘(n i+1) je odhad ceny nejlevnější cesty z uzlu s pořadím i+1 do cíle. Takový algoritmus prohledávání stavového prostoru je nazýván jako A*, viz. [2]. Funkce g‘(n i+1) je součet ceny prošlé cesty ze startu do pořadového uzlu i a ceny hrany z i do i+1. Protože všechny hrany mají stejnou hodnotu, je pro všechny uzly v i+1 pořadí funkce g‘(n i+1) shodná a nemá proto vliv na rozhodování. Funkce h‘(n i+1) se počítá jako cena nejkratší cesty ve stavovém prostoru od uzlu pořadí i+1 do cíle. V případě, že možných cílů ve stavovém prostoru je více, počítá se cesta do nejbližšího cíle. Cena cesty se
počítá jako součet absolutních hodnot z rozdílů všech tří souřadnic stavového prostoru. Rozdíly souřadnic se počítají mezi startovním a cílovým uzlem cesty. Podle [2] platí, že algoritmus A* je přípustný, jestliže jeho funkce h’(n i+1)<=h(n i+1), kde h(n i+1) je cena skutečné cesty. Naše funkce h‘(n i+1) má hodnotu rovnou délce nejkratší možné cesty bez uvažování překážek. Proto bude vždy kratší nebo stejně dlouhá než nejkratší skutečná cesta, jejíž délce se rovná h(n i+1). Výše popsaný algoritmus, řešící uspořádané prohledávání stavového prostoru, je algoritem přípustným, protože najde vždy cestu k cíli, pokud tato cesta existuje.
4. Postup a experimenty V tomto bodě bude popsán postup při řešení celého zadaného problému. Dále budou popsány průběhy jednotlivých experimentů a měření. Postup: Kroky z bodu 3.2.: - spočteme přímou a inverzní kinematickou úlohu robota. Inverzní úlohu počítáme pomocí kosinové věty. Dostaneme tak vztahy pro vzájemný přepočet mezi kartézskými a kloubovými souřadnicemi Kroky z bodu 3.1.: - načteme pixelové a kartézské souřadnice čtyř krajních lícovacích značek a spočteme transformační matici - načteme pixelové souřadnice bodů A a B a pomocí transformační matice vypočteme jejich kartézské souřadnice Kroky z bodu 3.3.: - konec manipulátoru přemístíme z bodu A do bodu B, pomocí algoritmu prohledávání stavového prostoru. Na této cestě si zapamatujeme body stavového prostoru, přes které se dostáváme do bodu B. Tyto body jsou uloženy v seznamu P - v bodě B uchopíme přenášený bod a za po zapamatované cestě ho přeneseme do bodu A. V případě, že tato cesta už neplatí, protože se změnilo postavení překážek, najdeme cestu do cíle pomocí prohledávacího algoritmu V práci jsme provedli tyto měření a experimenty. Maximální chyba polohování manipulátoru na podložce. Velikost kroku na konci podložky o kartézských souřadnicích [403, 297, 0] pro kloub G je 16, pro kloub F je 29 a pro kloub E je 19. Vzdálenost dvou poloh manipulátoru o souřadnicích [403, 297, 0] a [428, 339, 14], kde se změní všechny kloubové souřadnice jednotku je 50. Maximální chyba polohování manipulátoru je 25. To je také největší možná chyba vzniklá zaokrouhlením vypočtených kloubových souřadnic. Absolutní chyby transformace mezi pixelovými a kartézskými souřadnicemi. Pátá lícovací značka leží uprostřed pracovního stolu. To znamená, že v kartézských souřadnicích leží nejdále od lícovacích značek použitých pro výpočet transformace. Protože perspektivní zobrazení tuto vlastnost zachovává, leží páta značka nejdále od ostatních i v pixelových souřadnicích. Pro body, které leží blíže k lícovacím značkám jedna až čtyři platí, že absolutní chyba při jejich transformaci z pixelových do kartézských souřadnic bude menší, až konečně pro body odpovídající lícovacím značkám jedna až čtyři bude nulová. Pro pátou lícovací značku jsme spočítali chybu transformace. Absolutní chyba transformace z pixelových do kartézských souřadnic pro tento bod a tedy i pro ostatní body vychází ±2.
Experiment s prohledáváním stavového prostoru. Při tomto experimentu hledáme cestu mezi překážkami z bodu A do bodu B. Použitá heuristika, kdy sčítáme rozdíly jednotlivých souřadnic, preferuje pohyb robota k takovému cíli, kdy je počet kroků ramene minimální. Při pohybu, který vyžaduje změnu více kloubových souřadnic je pravděpodobné, že počet kroků ramene bude vyšší, než při pohybu se změnou pouze jedné souřadnice. Proto se dá očekávat, že manipulátor bude volit takové pohyby, kdy se bude dlouhodobě měnit pouze jediná souřadnice. Provedli jsme dva pokusy. Při prvním se dostával robot z bodu A do bodu B se souřadnicemi podle zadání. U druhého pokusu je bod A umístěn těsně u překážek. Popis cesty konce ramene manipulátoru při těchto experimentech v kartézských souřadnicích, je popsán na obrázcích 3 a 4. Na obrázcích 5 a 6 je popsán popis cesty ve stavovém prostoru. Na obrázku 5 je pro lepší orientaci přerušovanou čarou spojen start s cílem. Na obrázku 6 je bodem A označen start a bodem B cíl, do kterého se manipulátor dostal.
200
200
150
150
100
100
50
50
0
0
-50
-50
B
400
B
400
300
300
A
200
200 100
100
0
0 -100
0
-100
300
200
100
400
500 -100
Obr. 3. Popis prvního pokusu v kartézských souřadnicích
-100
300
200
100
0
400
500
Obr. 4. Popis druhého pokusu v kartézských souřadnicích (Bod A je těsně u překážek)
B 30
30
29
29
28
28
27
27
26
26
25 72
25 72 70
100 80
68
60 40
66 64
20 0
Obr. 5. Popis druhého pokusu v kloubových souřadnicích
100
70
A
68 66
80 60 40
20 0
Obr. 6. Zapamatovaná cesta stavovým prostorem při druhém pokusu
Z průběhu obou pokusů je vidět, že algoritmus volí jako úspornější pohyb, kdy se mu mění pouze jediná souřadnice.
5. Zhodnocení V práci byla řešena úloha manipulátoru, přenášejícího hmotný bod mezi dvěma body na podložce. Manipulátor je na začátku v bodu A a má do tohoto bodu přemístit hmotný bod z bodu B. Mezi těmito body mohou ležet překážky, o kterých nevíme. Absolutní chyba při výpočtu kartézských souřadnic bodů A a B vyšla ±2. Tato chyba se ale neprojevuje, protože absolutní chyba polohování konce manipulátoru v blízkosti podložky vychází ± 25. Při výpočtu kloubových souřadnic cíle pomocí inverzní kinematické úlohy dostáváme dvě nebo čtyři různá řešení. Pro body A a B na zadaných souřadnicích vycházejí čtyři řešení. Pro start vybíráme libovolné řešení. Řešení cíle jsou při prohledávání stavového prostoru brána v úvahu všechna. Algoritmus, který prohledává stavový prostor tvořený kloubovými souřadnicemi, se pokouší dostat vždy do nejbližšího cíle. Cestu stavovým prostorem z bodu A do bodu B hledáme při cestě manipulátoru k bodu B. Zpátky, už s hmotným bodem, se manipulátor vrací po nalezené cestě. Kvůli výše zmíněné chybě polohování není manipulátor schopen dostat se vždy přesně na body A a B. Manipulátor se proto polohuje na nejbližší možnou pozici.
6. Literatura [1] Šára, R. : Obecná teorie systémů – přednášky, PDF dokumenty na adrese : http://cmp.felk.cvut.cz/cmp/courses/OTS, 29. března 2000 [2] Pajdla, T. : Inteligentní robotika – přednášky, obrázky ve formátu gif na adrese: http://cmp.felk.cvut.cz/cmp/courses/iro2000/iro2000/index.html, 20.listopadu 2000
