Řízení pohybu manipulátoru v prostoru s překážkami INTELIGENTNÍ ROBOTIKA Alena Pivoňková, Pavel Dohnal, Jiří Izer Katedra řídicí techniky, ČVUT FEL
Abstrakt Cílem práce je navrhnout algoritmus ovládání ramen manipulátoru pro přesun předmětu z počáteční polohy B do cílové polohy A s požadavkem na minimální délku trajektorie. Scéna je snímána perspektivní kamerou. Její obraz slouží k určení krajních bodů dráhy.V pracovním prostoru jsou umístěny překážky, jejichž přítomnost lze zjistit proximitním čidlem, které se nachází na chapadle manipulátoru. Poloha překážek není předem známa. Trajektorie je plánována algoritmem A*. Byla řešena přímá a inversní kinematická úloha, nalezena transformační matice mezi souřadnicovými systémy kamery a pracovní plochy a dále byl implementován algoritmus pro nalezení optimální cesty. Algoritmus byl ověřen experimentem.
1. Úvod Tato samostatná práce z předmětu Inteligentní robotika na ČVUT FEL (viz[1]) řeší hledání optimální cesty manipulátoru mezi dvěma body. Koncové body dráhy jsou zjišťovány z obrazu perspektivní kamery. Proximitní čidlo detekuje všechny neznámé překážky v bezprostřední blízkosti. Cílem navrženého algoritmu je přesunout předmět z počáteční polohy do cílové s uvažováním co nejkratší délky trajektorie. Návrh vychází z několika omezení: • • •
manipulátor je nehmotný, koncové body jsou v dosahu chapadla, při pohybu manipulátoru se žádná překážka na scéně nepohybuje.
Detailní popis zadání viz [2], skupina 5. Úlohu lze rozdělit na tři nezávislé podúlohy. Nejprve je třeba na základě snímků z perspektivní kamery stanovit polohu počátečního a cílového bodu (kapitola 2). K pohybování ramen manipulátoru slouží řešení přímé a inversní kinematické úlohy (kapitola 3). K plánování trajektorie je využito algoritmu A*, u kterého je jako minimalizační kritérium uvažována délka trajektorie (kapitola 4).
2. Určení polohy obrazových bodů V této části je vyřešeno určení kartézských souřadnic bodů A a B. Scéna je snímána perspektivní kamerou umístěnou na neupřesněném místě nad snímanou oblastí (Obr.1). Na.snímané scéně je pět kalibračních značek se známými kartézskými souřadnicemi, dále je tam počáteční bod B a cílový bod A, jejichž kartézské souřadnice hledáme (Obr.2)
1
Obr.1: obrázek z kamery
Obr.2: pracovní plocha
Jelikož je ze snímané scény podstatná pouze rovina, tak vztah mezi obrazovými souřadnicemi r r kamery u = (u, v) a souřadnicemi pracovní plochy x = ( x, y ) je: r h1T h11 h12 r H = h2T = h21 h22 r h T h 3 31 h32
u i xi α i ⋅ vi = H ⋅ y i 1 1
h13 h23 , h33
(1)
r r kde x = [ xi , y i ,1] jsou souřadnice i-tého bodu pracovní plochy, u = (u i , vi ,1) jsou souřadnice i-tého bodu v obraze kamery, α i je konstanta a H je transformační matice mezi obrazovou rovinou kamery a pracovní plochou manipulátoru. Z (1) vyplývají pro souřadnice každého bodu tři rovnice, pomocí kterých se vypočítají koeficienty transformační matice H : r r r r α i vi = h2T x , r r α i = h3T x .
α i u i = h1T x , (2)
Po následné úpravě rovnice pro každý bod: r r r r h1T x − u i h3T x = 0 , r r r r h2T x − vi h3T x = 0 .
(3)
Pro čtyři kalibrační body vyjde soustava osmi rovnic o devíti neznámých, které představují parametry matice H : xi 0
yi 0
1
0 0 xi
0 yi
0 − u i xi 1 − v i xi
M M
2
− ui yi − vi y i
− u i h11 − vi h12 ⋅ = 0. M h33
(4)
Předchozí rovnici lze zapsat také jako
r A⋅h = 0 ,
(5)
r kde A je matice [8x9] změřených dat a h je vektor [9x1] neznámých v matici homografie H .
Vztah (5) lze rozepsat na 8 lineárně nezávislých rovnic. Podmínkou je, že žádné 3 body nesmějí ležet na přímce. Tuto soustavu je možno řešit metodou SVD (Singular Value Decomposition), která využívá principu nejmenších čtverců (viz [3]). Matice A tedy může být rozložena: (6) A = U ⋅ D ⋅V T a následně upravena: r σ 1 v1r O ⋅ rM = 0 . (7) U⋅ v8r σx r 0 v9r r r r r r Nyní je třeba nalézt vektor h ortogonální k v1 ...v8 , tj. musí platit viT .h = 0 pro i = 1...8 . r r Na.ortogonalitě v9 nezáleží, neboť σ 9 =0. Řešením je tedy vektor v9 . Transformační matice má tvar: 0.4469 223.0847 v91 v92 v93 4.5021 3 H = v94 v95 v96 = 10 ⋅ − 0.0712 − 2.1644 974.7831 (8) v97 v98 v99 0.0005 0.0026 2.4248
Správnost řešení je ověřena na pátém bodě, absolutní chyba měření pro osu x je 0,4880 mm a pro osu y 0,6591 mm. Výsledky jsou v Tab.1. Bod u [pixel] v [pixel] x [mm] y [mm]
1
2
3
4
5 (známý)
5 (vypočtený)
A
B
92
744
619
110
399
-
151
663
402
362
104
119
229
-
158
335
0
383
383
0
192
192.4880
29.7654
340.4033
0
0
277
277
138
138.6591
227.5933
26.5576
Tab.1: ověření správnosti souřadnic A a B
3. Kinematika robota Tato kapitola popisuje řešení kinematiky robota. Nejprve je odvozena přímá kinematická úloha (PKU), dále inversní kinematická úloha IKU a následně je zkontrolována korektnost odvozených vztahů.
3.1. Přímá kinematická úloha PKU řeší na základě znalosti nastavení kloubů manipulátoru polohu koncového bodu chapadla, viz. Obr.3a a Obr.3b.
3
Obr.3a: značení rozměrů ramen
Obr.3b: značení polohy chapadla
Rovnice PKU jsou: h = b. sin ϕ F − a. sin (− ϕ E − ϕ F ) = b. sin ϕ F + a. sin (ϕ E + ϕ F ) , x = 191 + h. sin ϕ G = 191 + [b. sin ϕ F + a. sin(ϕ E + ϕ F )]. cos ϕ G ,
(9)
y = 138 − h. cos ϕ G = 138 − [b. sin ϕ F + a. sin(ϕ E + ϕ F )]. sin ϕ G , z = 780 − [ z 0 + z x1 ] = 780 − [b. cos ϕ F + a. cos(ϕ E + ϕ F )] , kde ϕ E , ϕ F , ϕ G reprezentují úhly natočení kloubů robota; a, b jsou délky jednotlivých ramen manipulátoru a x, y, z jsou výsledné souřadnice chapadla. Předchozí rovnice lze jednoznačně vyjádřit v tzv. Denavinově-Hartenbergově notaci (viz [4]): 1 0 O ΑP = 0 0
0 0 1 0 0 1 0 0
0 1 0 cos ϕ F Α GF = 0 sin ϕ F 0 0
xp y p , zp 1
0 b sin ϕ F , − b cos ϕ F 1
cos(ϕG + π ) − sin(ϕG + π ) sin(ϕ + π ) cos(ϕ + π ) G G ΑGP = 0 0 0 0
0 0 0 , 1 − d 0 1
0 1 0 cosϕ E Α FE = 0 sin ϕ E 0 0
0
0
a sin(ϕ E − ϕ F ) , cos ϕ F cosϕ E − a cos(ϕ E − ϕ F ) 0 0 1 i −1 kde jednotlivé matice Α i značí převod ze souřadné soustavy (i − 1) do soustavy Pro.otevřený kinematický řetězec lze napsat výsledný vztah: 0 − sin ϕ F
0 − sinϕ E
O = Α OP ⋅ Α GP ⋅ Α GF ⋅ Α FE ⋅ X ,
(10)
i.
(11)
kde O je počáteční bod souřadné soustavy světa a X je koncové chapadlo robota v tomto prostoru. 4
Ke zjištění koncové polohy chapadla manipulátoru tedy stačí využít vztahů (9), ve kterých je vyřešeno posunutí souřadné soustavy pracovního prostoru. Za předpokladu znalosti úhlů natočení jednotlivých kloubů ramen lze jednoznačně určit polohu chapadla.
3.2. Inversní kinematická úloha IKU řeší nalezení hodnot nastavení jednotlivých kloubů manipulátoru v případech, že je známa poloha konce chapadla. Výsledná úloha je většinou nejednoznačná a je třeba zvolit vhodná řešení respektující převážně fyzikální omezující podmínky. Na Obr.3a a Obr.3b jsou znázorněny jednotlivé veličiny použité k výpočtům. Nejprve je potřeba každý bod X = [ x, y, z ] posunout vzhledem k počátku souřadné soustavy: (12)
x0 = x − 191 , y 0 = y − 138 , z 0 = 780 − z .
K řešení IKU byly použity vztahy (13).
ϕ G = arctan
y0 x0
z0 c
α = arcsin
z 0 = 780 − z z0 < 0
c = c1 + c2
ϕ F = 90 0 + ϕ F 2 + α
ϕ F = 90 0 + ϕ F 2 − α
ϕ F = 90 − ϕ F 2 + α
ϕ F = 90 0 − ϕ F 2 − α
a1
0
b1
ϕ F = ϕ F1 + ϕ F 2 a
ϕ F = ϕ F1 − ϕ F 2 b
z0 ≥ 0
(13)
a2
b2
Možné varianty řešení: • • • • •
0 řešení – prostor nedosažitelný volným koncem manipulátoru, 1 řešení – prostor dosažitelný jednou konfigurací manipulátoru, 2 řešení – prostor dosažitelný dvěma konfiguracemi manipulátoru, 4 řešení – prostor dosažitelný čtyřmi konfiguracemi manipulátoru, ∞ řešení – prostor dosažitelný teoreticky nekonečně mnoha konfiguracemi.
Obr.4a: rozbor řešení (pohled z boku)
Obr.4b: rozbor řešení (pohled z vrchu)
Počet řešení je znázorněn v Obr.4a. Mimo vyšrafovanou oblast je počet řešení nulový, v ose z nekonečný a v ostatních oblastech záleží na nastavení kloubu G. Situace je vyobrazena na Obr.4b. Pokud bude tento kloub v oblasti „1.řešení“, potom počet řešení 5
udávají červené hodnoty v Obr.4a, pokud bude v oblasti „2.řešení“, pak platí modré hodnoty tamtéž.
3.3. Přesnost a korektnost přímé a inversní kinematické úlohy Ověření správnosti PKU a IKU lze provést vzájemným dosazením výsledků z IKU do PKU. Pro jednoduchost stačí zvolit třeba dvě libovolné polohy chapadla manipulátoru a pomocí IKU stanovit všechny jim odpovídající kloubové souřadnice. Dosazením do PKU se velmi snadno provede kontrola. Výsledky jsou shrnuty v Tab.2. ( x, y , z )
IKU1 (ϕ E , ϕ F , ϕ G ) [°]
IKU2 (ϕ E , ϕ F , ϕ G ) [°]
PKU ( x, y, z )
[25, 25, 25] [100, 100, 100]
[-55.76, 45.16, -47.98] [-67.34, 56.36, -74.82]
[-55.76, -15.37, 47.98] [-67.34, 39.86, 74.82]
[25, 25, 25] [100, 100, 100]
Tab.2: ověření správnosti PKU a IKU
V praxi se mohou objevit dvě hlavní příčiny nepřesnosti řízení manipulátoru. Jednak jsou to velikosti kroků jednotlivých kloubů, které znemožní přesné nastavení koncové polohy chapadla, a dále zaokrouhlování při výpočtech v Matlabu.
4. Plánování trajektorie Tato část se zabývá plánováním optimální trajektorie pohybu robota s využitím algoritmu A* (viz [5]) a jejím přeplánováním v okamžiku nárazu na překážku. Základní myšlenka plánování je založena na principu generování a následného testování. Plánování probíhá iteračně. Nejprve je naplánována cesta z počátečního bodu do koncového a následně je ověřována možnost projití manipulátoru touto cestou. Vzhledem k tomu, že poloha překážek není dopředu známa, je nutné využít informací z proximitního čidla na konci manipulátoru. Manipulátor se pohybuje po naplánované cestě do okamžiku, než je detekována překážka. Při její detekci je doplněna informace o prostředí a je provedeno přeplánování cesty z aktuální pozice do cílové, tentokrát již s větší znalostí prostředí. Tento algoritmus je opakován až do doby, než je chapadlo manipulátoru v cílové poloze (nebo v minimální realizovatelné vzdálenosti od ní).
4.1. Simulátor manipulátoru Prvním krokem pro vizualizaci postupu plánování a výsledné cesty je implementování simulátoru manipulátoru v prostředí Matlab. Vstupními parametry je matice trajektorie a parametry manipulátoru. Příklad trajektorie v prostředí bez překážek je na Obr.5.
Obr.5: plánování trajektorie bez překážek 6
4.2. Reprezentace prostředí Manipulátor si postupným „ohmatáváním“ vytváří vnitřní reprezentaci prostředí, kterou využívá pro plánování cesty. Robot může postupovat pouze v krocích motorů (diskrétní stavový prostor). Pro každou polohu manipulátoru stačí evidovat pouze dva stavy (je překážka, není překážka). Prostředí je tedy reprezentováno maticí (100x100x100), kde jednotlivé indexy matice představují kroky motoru manipulátoru ( G × F × E ). Matice obsahuje „0“ (dovolená pozice, bez překážky) a 1 (zakázaná pozice, s překážkou). Při počáteční inicializaci je tato matice nulová, mimo spodní podstavy. Podložka je totiž jediná známá překážka. Informace o dalších možných překážkách jsou postupně zjišťovány a zpřesňují vnitřní model prostředí, který je využit k plánování trajektorie. Každý stav lze expandovat do 26 následníků (3 klouby s možností zůstat ve své poloze nebo se pohnout o krok jedním nebo druhým směrem, je vynechán stav beze změny alespoň jednoho kloubu).
4.3. Plánování cesty Pro nalezení cesty z bodu B do bodu A je využit algoritmus uspořádaného prohledávání stavového prostoru A* (viz [5]). Algoritmus A* používá heuristickou funkci ve tvaru:
f ( n) = g ( n) + h ( n) , kde g (n) je odhad délky nejkratší cesty z aktuálního uzlu délky z.aktuálního uzlu do koncového.
n
a
h (n) je odhad
Tento algoritmus je přípustný, pokud platí tyto dvě nerovnosti:
h ( n) ≤ h( n) ,
(14)
g ( n) ≥ g ( n) , kde h(n) je skutečná vzdálenost k cíli a g (n) je optimální cesta do bodu n. Funkce h(n) je zvolena jako euklidovská vzdálenost aktuálního bodu n od koncového a funkce g (n) jako nejkratší euklidovská cesta z počátku do bodu n, která byla po krocích dosud zjištěna. Hodnota g (n) ) je rovna součtu euklidovských vzdáleností bodů v prošlé cestě.
Obr.6a: trajektorie robota (pohled z boku)
Obr.6b: trajektorie robota (pohled z vrchu) 7
Simulace v prostředí Matlab demonstruje praktickou použitelnost navrženého algoritmu. Tvar trajektorie pohybu v prostředí s překážkami (viz. Obr.6a a Obr.6b), který není po objetí překážky přímý, je dán skutečností, že kloub umožňující robotu otáčení v horizontální rovině má omezený rozsah otáčení. Použitá heuristická funkce, hledající nejkratší projetou cestu, proto vede na tuto trajektorii. Ta je sice delší než přímá, je však nejkratší vzhledem k omezeným možnostem pohybu robota.
5. Závěr V prvním kroku byla ze zadaného obrázku vypočítána matice homografie. Pomocí této matice se následně určily souřadnice počátečního a cílového bodu trajektorie. Přesnost získaných souřadnic se liší maximálně o 1 jednotku v každém směru, což je přesnost, která postačuje pro další navazující metody. Pomocí IKU lze získat z bodů zadaných v souřadnicích pracovního prostoru manipulátoru jednotlivá natočení kloubů E, F a G. Prostřednictvím PKU lze naopak přecházet z kloubových souřadnic do souřadnic pracovního prostoru. Experimentálně byla správnost IKU a PKU ověřena na dvou různých bodech pracovního prostoru. Plánování nejkratší cesty mezi dvěma body s neznámými překážkami je založeno na.algoritmu A* a při detekci překážky dojde k přeplánování trajektorie. Algoritmus A* dokáže najít cestu v jakémkoliv stavovém prostoru s kladně ohodnocenými hranami, pokud nějaká existuje. Všechny početní části této úlohy byly zpracovány v programu Matlab.
6. Literatura [1] Matoušek, M.: 33IRO – Inteligentní robotika (zima 2004) [online]. Poslední revize 2004-11-03 [cit.2004-10-21]. < http://cmp.felk.cvut.cz/cmp/courses/IRO/2004Z/iro2004/index.html > [2] Matoušek, M.: 33IRO – Inteligentní robotika: Zadání samostatné práce [online]. Poslední revize 2002-09-27 [cit.2004-10-22]. < http://cmp.felk.cvut.cz/cmp/courses/IRO/2004Z/iro2004-zadani/index.html > [3] Krajník, E.: Maticový počet. ČVUT, Praha, 1998. [4] Smutný, V.: Kinematika: Přednáška k předmětu 33ROB [online]. Poslední revize 2000-11-22 [cit.2004-10-27]. < http://cmp.felk.cvut.cz/cmp/courses/roblec/kinematika.pdf > [5] Pajdla, T.: 33IRO – Přednášky z předmětu Inteligentní robotika [online]. Poslední revize 2000 [cit.2004-11-02]. < http://cmp.felk.cvut.cz/cmp/courses/IRO/2000Z/iro2000L/index.html >
8
