BROB Základy robotiky. Ing. František Burian, Ph.D. Jan Macháček VUT ID: Martin Pavelka VUT ID:

Transkript

1 Předmět: BROB Základy robotiky Rok vypracování: 2018 Název projektu: Vedoucí práce: Realizace inverzní kinematiky manipulátoru Ing. František Burian, Ph.D. Autoři projektu: František Majvald VUT ID: Zadání projektu: Jan Macháček VUT ID: Martin Pavelka VUT ID: Pro planární manipulátor typu RPRP (viz obrázek): 1. Spočtěte přímou kinematickou úlohu (analyticky) 2. Spočtěte inverzní kinematickou úlohu (numericky) 3. Manipulátor realizujte v modelovém provedení 4. Demonstrujte funkčnost inverzní kinematické úlohy na modelu.

2 Přímá úloha kinematiky Schematický návrh manipulátoru: H02 = Rotace(z, q1) Translace(L1,0,0) Rotace(z, q2) Translace(L2,0,0) = cos(q1) sin(q2) L1 cos(q2) sin(q2) L2 [ sin(q2) cos(q1) 0] [ ] [ sin(q2) cos(q2) 0] [ ] = Po roznásobení a úpravě dojdeme k výsledku: cos (q1 + q2) sin (q1 + q2) cos(q1 + q2) L2 + cos(q1) L1 H02 = [ sin (q1 + q2) cos (q1 + q2) sin(q1 + q2) L2 + sin(q1) L1 ] výsledná orientace - výsledná pozice Rovnice Přímé úlohy kinematiky z matice H02: x = cos(q1 + q2) L2 + cos(q1) L1 y = sin(q1 + q2) L2 + sin(q1) L1 φ = q1 + q2 Kvůli zvětšení pracovního prostoru jsme nakonec pozměnili orientaci ramen, což po přepočtu vedlo na pozměnění výsledku přímé úlohy kinematiky. Výsledná matice: Z té vyjde rovnice: cos (q1 q2) sin (q1 q2) cos(q1 q2) L2 + cos(q1) L1 H02 = [ sin (q1 q2) cos (q1 q2) sin(q1 q2) L2 + sin(q1) L1 ] x = cos(q1 q2) L2 + cos(q1) L1 y = sin(q1 q2) L2 + sin(q1) L1 φ = q1 q2

3 Inverzní úloha kinematiky Přímá úloha kinematiky je nelineární, a proto je složité najít inverzní funkci. Možným řešením je tedy řešení numerické. K výsledku dojdeme díky znalosti počáteční pozice kloubů a místa, kam se chceme koncovým bodem dostat. Vypočítáme směrnici tečny v současné pozici natočení kloubů. Z ní vypočítáme o kolik máme změnit natočení kloubů. Pohneme s klouby. Ve výsledné pozici opět vypočítáme směrnici tečny. Znovu pootočíme klouby a takto pokračujeme, dokud nejsme dostatečně blízko žádané pozici. Dostatečně blízko znamená, že odchylka od žádané pozice je menší přesnost stroje. V našem případě je to 1 mm. K tomuto výpočtu potřebujeme znát Jakobián (vypočítáno z rovnic pro novou orientaci ramen) Jakobián: x q1 y J = q1 φ [ q1 x q2 y L2 sin(q1 q2) L1 sin (q1) L2 sin (q1 q2) = [ L2 cos(q1 q2) + L1 cos (q1) L2 cos (q1 q2) ] q2 1 1 φ q2] Spodní řádek nemusíme uvažovat. Z toho plyne: Výpočet změn natočení kloubů: L2 sin(q1 q2) L1 sin (q1) L2 sin (q1 q2) J = [ L2 cos(q1 q2) + L1 cos (q1) L2 cos (q1 q2) ] ΔP = J Δq -> Δq = J 1 ΔP ΔP rozdíl současné pozice od pozice žádané Δq změna natočení kloubů Δq = [ Δq1 Δq2 ] = [ 0 J J 1 J J ] Δq = L2 (ΔPx cos(q1 q2) + ΔPy sin(q1 q2)) L1 L2 sin (q2) L1 (ΔPy sin (q1) + ΔPx cos (q1)) L2(ΔPy sin(q1 q2) + ΔPx cos (q1 q2) [ L1 L2 sin (q2) ] Natočení kloubů: q n+1 = q n + Δq (všechno jsou to sloupcové vektory) Pozice při novém natočení kloubů: P n+1 = Přímá úloha kinematiky(q n+1 ) ΔP n+1 = P žádaná P n Tento výpočet opakujeme dokud ΔP n+1 není menší než přesnost stroje.

4 Testování správnosti výpočtu v Matlabu Funkce Přímé úlohy kinematiky: Funkce Jakobiánu: Skript na otestování správnosti výpočtu s detekcí singularity: :

5 Vypočtené hodnoty: Žádaná cílová pozice Pk = [130;150] Pouze pro zajímavost počet kroků iterace Detekce singularity Konečné natočení kloubů V případě, kdy nastane singularita, se vyhlásí chyba a klouby se natočí do výchozí polohy. Kontroluje se, jestli aktuální vypočtená souřadnice se přibližuje k cílové pozici či nikoliv. V případě singularity se vypočtená souřadnice od žádaného bodu vzdaluje. Detekce singularity je nezbytná kvůli případnému zničení stroje, popřípadě ublížení na zdraví. Z výsledků je zřejmé, že výpočty jsou vyřešeny správně. Výpočty byly testovány na více kombinací počátečního natočení kloubů a cílové pozice.

6 Implementace matematiky do C# aplikace Funkce Přímé úlohy kinematiky: Funkce Jakobiánu: Výpočet inverzní matice podle vzorce: => Konkrétně: Funkce pro přepočet radiánů na byte pro serva:

7 Funkce pro výpočet požadovaného natočení serv v radiánech: Kde parametry ax a ay jsou souřadnice cílového bodu, parametry ainitangle1 a ainitangle2 jsou výchozí úhly natočení serv. Návratová hodnota je pole o třech prvcích první a druhý prvek je úhel natočení serv a třetí prvek je detekce singularity. V případě, že singularita byla detekována tak se 3. prvek nastaví na 1 a úhly se nastaví na výchozí hodnotu. Vychozí hodnota úhlů (v programu pojmenováno jako q_vych) znamená home position manipulátoru, počáteční hodnota úhlů (v programu pojmenováno jako q0) znamená aktuální hodnotu úhlů manipulátoru.

8 Ukázka jednoduchého obslužného programu pro kreslení kruhu: Obslužný program volá funkci Wait_ms(), která na chvíli uspí vlákno, aby serva měla čas se pohnout na vypočtenou pozici. Tento program tedy nemá zpětnou vazbu, kdy by ze serv vyčítal jejich aktuální polohu a podle toho jim posílal instrukce. Konektivita serv: Digitální serva od firmy Dynamixel vyžadují napájecí napětí 12-18V, což zajišťuje spínaný zdroj 230V 12V/2,5A. Serva komunikují po sériové lince RS 485. Zde bylo využito převodníku USB/RS 485.

9 Vyrobené díly (výkresová dokumentace přiložena v elektronické podobě) Sestava nohy ramene:

10 Sestava kleštiny pro tužku:

11 Pohled na sestavený manipulátor: Závěr: Projekt se podařilo úspěšně dokončit. Při kreslení objektů dochází k pravidelným záškubům, jejichž příčinou nejspíše bude chybějící regulace, kdy první servo hýbe s větší váhou než druhé, a tak se do cílové pozice dostane později, popřípadě chybějící vyhlazování (smoothing). Po menší úpravě programu, lze pozorovat jednotlivé iterace matematického aparátu přímo na manipulátoru. Z důvodu ukotvení manipulátoru v levém dolním rohu psací desky musela být přepočtena matematika pro kloub na druhou stranu (pryč z desky) pro větší aktivní prostor manipulátoru. Původní statická kleština musela být předělána na pohyblivou kleštinu s pružinkami z důvodu nerovnoměrné výšky hrotu tužky nad psací deskou někdy docházelo k tomu, že byl hrot ve vzduchu a jindy trhal papír. Před vlastním použitím manipulátoru je nezbytné nejprve inicializovat serva (přiřadit ID, výchozí rychlost, baudrate ).