SOFTWARE PRO ŘÍZENÍ HUMANOIDNÍHO ROBOTU TITLE

VYSOKÉ UČENÍ TECHNICKÉ V BRNĚ BRNO UNIVERSITY OF TECHNOLOGY

FAKULTA STROJNÍHO INŽENÝRSTVÍ ÚSTAV AUTOMATIZACE A INFORMATIKY FACULTY OF MECHANICAL ENGINEERING INSTITUTE OF AUTOMATION AND COMPUTER SCIENCE

SOFTWARE PRO ŘÍZENÍ HUMANOIDNÍHO ROBOTU TITLE

DIPLOMOVÁ PRÁCE DIPLOMA THESIS

AUTOR PRÁCE

KLEMENT ŠŤASTNÝ

AUTHOR

VEDOUCÍ PRÁCE SUPERVISOR

BRNO 2007

DOC. RNDR. ING. JIŘÍ ŠŤASTNÝ, CSC.

Strana 3

ZADÁNÍ ZÁVĚREČNÉ PRÁCE (na místo tohoto listu vložte originál a nebo kopii zadání Vaší práce)

Strana 5

LICENČNÍ SMLOUVA (na místo tohoto listu vložte vyplněný a podepsaný list formuláře licenčního ujednání)

Strana 7

ABSTRAKT Diplomová práce se zabývá řešením analýzy pohybu humanoidního robotu. Realizací matematického modelu končetiny se šesti stupni volnosti robota. Ověřením matematického modelu na řídícím programu. Vytvořeným s použitím objektové technologie při návrhu a implementaci datové struktury, v programovacím jazyku C++. Program je dále ověřen při simulaci řízení konkrétních pohybů.

ABSTRACT This diploma thesis concerns with the solution of the analysis motions of the humanoid robot by the realization of the matemathic model of the limb with six degrees of freedom and by the verification of the mathemathic model on the control programm. This constructed programm use the object of technology at the design and at the intemplementation of the data structure in programming language C++. This programm is futher verified by the simulation of control concrete motions.

KLÍČOVÁ SLOVA Robot, manipulátor, končetina, vazba, koncový efektor, matice,objekt, třída.

KEYWORDS Robot, manipulator, limb, joint, end effector, matrix, object, class.

Strana 9

PODĚKOVÁNÍ Děkuji vedoucímu diplomové práce Doc. RNDr. Ing. Jiřímu Štastnému, Csc. za vedení a poskytnuté konzultace. Děkuji Doc. Ing. Pavlovi Bělohoubkovi, Csc. za cenné rady v oblasti robotiky.

Strana 11

Obsah: Zadání závěrečné práce...................................................................................................3 Licenční smlouva.............................................................................................................5 Abstrakt............................................................................................................................7 1 Úvod................................................................................................................................13 2 Historie...........................................................................................................................15 3 Hloubková analýza........................................................................................................19 3.1 Matematický aparát pro kinematickou analýzu robotů.................................................19 3.1.1 3.1.2 3.1.3 3.1.4

3.2

Homogenní souřadnice..........................................................................................................19 Transformace souřadnic........................................................................................................20 Transformační Matice translačního pohybu..........................................................................20 Transformační Matice rotačního pohybu...............................................................................21

Modelování kinematického řetězce...............................................................................22

3.2.1 3.2.2 3.2.3 3.2.4

3.3

Denavit – Hartenberg notace.................................................................................................22 Přímá úloha kinematiky.........................................................................................................24 Inverzní úloha kinematiky.....................................................................................................24 Kinematická Analýza Humanoidního Robotu.......................................................................25

Řízení Robotů................................................................................................................29

3.3.1 Pohyb Efektoru do požadovaného bodu................................................................................29 3.3.2 Změna Efektoru a počátku.....................................................................................................30

4 4.1 4.2 4.3

Realizace matematického modelu robota ...................................................................33 Kinematický model.......................................................................................................33 Nalezení DH parametrů.................................................................................................34 Rekonfigurovatelnost....................................................................................................36

4.3.1 Dopředné konfigurace...........................................................................................................36 4.3.2 Zpětné konfigurace................................................................................................................38

5 5.1

Realizace řídícího programu........................................................................................41 Použité nástroje a knihovny..........................................................................................41

5.1.1 5.1.2 5.1.3 5.1.4 5.1.5 5.1.6

5.2

Objektově orientovaná technologie.......................................................................................41 Jazyk C++.............................................................................................................................42 Vývojové Prostředí................................................................................................................43 Microsoft Fundamental Class ...............................................................................................44 Knihovna NewMat................................................................................................................44 Knihovna Roboop .................................................................................................................46

Dialogy a ovládání programu........................................................................................48

5.2.1 Nastavovací dialog................................................................................................................48 5.2.2 Výpočtový dialog.................................................................................................................48 5.2.3 Simulační dialog..................................................................................................................51

5.3

Datová a programová struktura.....................................................................................53

5.3.1 5.3.2 5.3.3

5.4 6

Rozdělení zdrojových kódů..................................................................................................53 Popis vytvořených a použitých tříd......................................................................................54 Ukázky alokace a uvolňování paměti...................................................................................59

Otestování programu na datech.....................................................................................61 Závěr...............................................................................................................................63 Seznam použité literatury.............................................................................................65

Strana 13

1

ÚVOD

Robot je inteligentní stroj, který je schopen autonomně ovlivňovat své okolí podle programu. Roboty, lze rozdělit podle mnoha hledisek například podle konstrukce nebo podle činnosti. Řada lidských činností je již dnes úspěšně realizována za pomocí průmyslových robotů. Člověk však stále hledá způsob ulehčení své práce za pomocí jediného univerzálního robota. Slovo humanoidní znamená lidský. Humanoidní robot, je potom robot, který se podobá člověku. Podobnost humanoidního robota a člověka nemusí nutně spočívat ve věrné kopii člověka, ale spíše v napodobení různorodých lidských činností. Vytvoření humanoidního robota je velice složitá úloha, na jejímž řešení se musí podílet velký kolektiv spolupracujících lidí. Tato diplomová práce přispívá k vyvinutí softwaru pro řízení humanoidního robota. Vzhledem k velké náročnosti tématu se tato diplomová práce zaměřuje na jeho část, která je pro pohyb nejdůležitější. Touto částí je končetina. Podobně jako u člověka, tak i humanoidní robot disponuje končetinami, kterými jsou ruce a nohy. Po provedení kinematické analýzy jednotlivých typů lidských končetin (ruce, nohy) zjistíme velkou podobnost (např. počet kloubů a možnosti jejich pohybu). Údaje získané z provedené analýzy je možné zpětně využít ke konstrukci libovolné končetiny humanoidního robota. V první fázi je nutné najít principy pro matematický popis. Na základě nalezených principů definovaných vstupů a výstupů je možné vytvoření matematického modelu. Z definovaného matematického modelu se provede objektová analýza, která se stane podkladem k vytvoření programu. Je zde popsána realizace softwaru pro řízení této končetiny včetně popisů nástrojů a technologií použitých při této realizaci. Druhá kapitola seznamuje čtenáře z historií a současným stavem v oblasti robotiky. Mimo jiné jsou tu uvedeny různé definice slova robot. Dále se zde pojednává o oblastech, v kterých se dnes roboti nejvíce uplatňují včetně několika příkladů robotů z praxe. V další kapitole se pojednává o problematice a způsobech řešení této problematiky. Jedná se o homogenní souřadnice, pomocí kterých se popisuje poloha a orientace v prostoru. Dalším potřebným nástrojem je Denavit – Hartenbergův princip pro popis kinematického řetězce. Je zde také vysvětlen význam a princip přímé úlohy kinematiky, pomocí které se zjišťuje poloha a natočení. Naproti tomu, inverzní úloha kinematiky, která je zde také uvedená, řeší výpočet parametrů jednotlivých pohyblivých spojení robota. Dále zde rozebereme kinematiku člověka. Jsou zde popsány jednotlivé typy kloubů a pohyb který realizují. Na základě toho pak stanovíme, jak by mohla vypadat konstrukce humanoidního robota. Probereme metody řízení (PTP, CP) a jednotlivé konfigurace končetiny. Ve čtvrté kapitole se zaměříme na realizaci matematického modelu končetiny, k čemuž využijeme znalosti nabyté v kapitole předchozí. Sestavíme kinematický model končetiny a definujeme jednotlivé souřadné systémy. Pak už lze naleznout odpovídající Denavit-Hartnebergovy parametry, pomocí kterých lze sestavit transformační matice. Tyto matice pak sestavíme i pro další konfigurace. Ještě před popisem návrhu a realizací softwaru, je vhodné stručné seznámení s technologiemi a nástroji, pomocí kterých je tento program vyvinut. Především se jedná o popis objektové technologie a programovacího jazyka. Pro uživatele programu je důležitá část, která se věnuje ovládaní a chování programu. Po této části následuje návrh a implementace datové struktury. Seznámení s použitými třídami a provázanost jednotlivých objektů. Nechybí některé části zdrojových kódů. Nakonec se provede a vyhodnotí ukázkový příklad, na kterém se ověří funkčnost softwaru.

Strana 15

2

HISTORIE

Slovo robot vymyslel Josef Čapek. Poprvé jej použil jeho bratr Karel Čapek v roce 1920 ve své divadelní hře RUR. Robot zde představoval uměle vytvořenou inteligenci, která měla nahradit lidskou práci.[7] Slovo robot, představující umělou inteligenci, která by nahradila lidskou práci, se ve světě velice rychle rozšířilo. Robot s lidskou podobou a jednoduchými pohyby byl poprvé zkonstruovaný v roce 1927. Tehdy jej představil Televox na světové výstavě v New Yorku. O rok později se konala výstava britské asociace inženýrů nesoucí název Erik. Britové zde ukázali světu robota, který byl již schopen řeči. V roce1931 se uskutečnila radiotechnická výstava v Londýně, na této výstavě byl představen robot schopný mluvit a číst. Robotika svých základů dosáhla teprve v roce 1942, kdy Norbert Wiener definoval kybernetiku, ustanovil základy řízení a propracoval teorii zpětné vazby. V roce 1954 byla robotika rozšířena Georgem Devolem, který podal patent na řízení programem s děrnými štítky a Josephem Endelbergem, který se zasloužil o strojní část robotu. Devol spolu s Endelbergem založili společnost Unimation, ze které v roce 1958 vzešel první robot nesoucí název Unimate 1900. O čtyři roky později byl sestrojen druhý robot, tentokráte konkurenční společností Versatran. V šedesátých letech dosáhla obecně robotika svého rozšíření ve Švédsku a Japonsku. V sedmdesátých letech již roboti nahrazovali lidskou práci například ve svařovací a montážní sféře a jejich výroba se rozšířila i do Asie a Severní Ameriky. V následující tabulce vidíme nasazování průmyslových robotů. 1972 1978 1980

1980 (DEF)

1984 (BRA)

Japonsko

1500

7000

8400

4600

34000

USA

850

3500

6000

2200

13000

Západní evropa

300

2500

4000

-

21 000

SRN

-

-

-

-

6600

Tab. 1 Nasazování průmyslových robotů ve světě Přesná definice robota nebyla od počátku zřejmá, vyvíjela se postupně se samotným robotem. V roce 1984 byl průmyslový robot definován jako: „automatický stroj obsahující výrobní mechanismus/manipulátor se dvěma a více stupni volnosti a přeprogramovatelným řídicím systémem k uskutečnění pohybů a řídicích funkcí. [8] O čtyři roky později jsme se setkali s upřesňující definicí: „Průmyslový robot je stacionární nebo pohyblivý automatický stroj skládající se z manipulátoru a přeprogramovatelného řídicího systému na provedení pohybových a řídicích funkcí při výrobním procesu.“[8] Definice robota, kterou máme dnes, pochází z roku 1990: „Průmyslový robot je automaticky řízený reprogramovatelný víceúčelový manipulační stroj, stacionární nebo umístěný na pojezdu, přičemž je určen na použití v průmyslové automatizaci.  Reprogramovatelný - schopnost změnit naprogramované pohyby nebo pomocné funkce bez fyzických změn. 

Fyzická změna - změna v mechanické konstrukci - např. výměna vaček, nebo ŘS, netýká se výměny karet, disket ap.



Víceúčelový - lze přizpůsobit k různým aplikacím.“ [8]

Strana 16

2 Historie

Robot je počítačem řízený integrovaný systém, schopný autonomní cílově orientované iterace s reálným prostředím v souladu s instrukcemi člověka. V současnosti je terminologie v robotice velice různorodá, a proto se můžeme setkat s různými odlišnostmi jak v definici robota, tak v robotice jako celku. Robotika spojuje obory jako mechanika, automatizace, elektronika, informatika, matematika, kybernetika, biologie a mnohé další.[3] Cílem mnoha konstruktérů a pracovišť ve světě je sestrojit robota, který by byl schopen nahradit člověka. V případě specifických úkolů, kdy robot vykonává stále se opakující činnost ve stejnorodém prostředí, je tato snaha na vysoké úrovni. Příkladem mohou být nejrůznější vyspělé továrny a výrobní linky v automobilovém průmyslu. Dá se říci, že čím dražší je cena lidské práce, tím větší je úroveň nasazení automatizace a průmyslových robotů. Pokud do toho zahrneme ještě úroveň oborů, na kterých robotika závisí, pak nikoho nepřekvapí, že mezi největší producenty robotů patří především země jako USA, Japonsko nebo Německo.

Obr. 1 Montážní linka v automobilce TPCA Ze statistik posledních let vyplývá, že objem výroby průmyslových robotů po výrazné depresi let 1991 - 1994, začal v roce 1995 opět pozvolna růst a tento trend pravděpodobně potrvá i v příštích letech.[3] Také ztráty na životech v mnoha posledních vojenských konfliktech a při teroristických útocích, vedou k stále většímu nasazování robotů ve vojenství a v pyrotechnických týmech. Zde se dálkově řízený roboti používají pro zneškodňování náloží a bomb již poměrně delší dobu. Ve vojenské robotice jsou pak nejdále Spojené Státy Americké. Ty už vyvinuli nebo vyvíjejí celou řadu vojenských robotů, počínaje velkými bezpilotními letadly a konče malými špionážními robůtky. Některé roboti jsou už i prakticky nasazováni např. bezpilotní stroje Global Hawk, Predator nebo pásový roboti.

Obr. 2 Bezpilotní letoun Predator.

2 Historie

Strana 17

Další oblastí kde jsou roboti hojně nasazovány je kosmonautika. Zde je toto nasazeni velice výhodné, neboť použití lidských osádek je velice problematické z důvodů drsných podmínek, které panují ve vesmíru nebo na jiných planetách či měsících. Dalším důvodem je pak časová náročnost jednotlivých vesmírných misí, která může trvat i desítky let. Vzhledem k tomu, že při těchto misích se už promítá, z důvodu velké vzdálenosti i zpoždění signálu, je tu snaha o co největší umělou inteligenci. To je nezbytné, pokud si robot musí rychle poradit s nějakým nepředvídaným problémem autonomně, neboť při dálkovém řízení by už mohlo být pozdě.

Obr. 3 Vozítko Mars Pathfider. V současnosti se pracuje na vývoji robotů určených pro lékařství tzv. robochirurgové. To je dáno především schopností dosáhnout daleko větší přesnosti než dosahují lidští chirurgové. Tato disciplína je však hodně náročná. Vzhledem k fatálním následkům při selhání tyto roboti zatím nepracují autonomně.

Strana 19

3

HLOUBKOVÁ ANALÝZA

Při řízení humanoidního robota je možné robota rozdělit na jednotlivé části (ruce, nohy, atd.). Jednotlivé části lze potom řešit jako samostatného robota. Nad tímto stupněm řízení je vyšší vrstva řízení, která koordinuje pohyb jednotlivých částí.

3.1

Matematický aparát pro kinematickou analýzu robotů

Pro řízení těchto částic, je potřeba umět zjistit polohu a orientaci v prostoru v závislosti na vstupních proměnných a obráceně, pokud chceme přesunout robota do požadované pozice. Což provedeme nastavením právě těchto vstupních proměnných. 3.1.1

Homogenní souřadnice

V kinematice mechanismů, kromě třírozměrných souřadnic, se s výhodou používá homogenních souřadnic, které jsou čtyřrozměrné. Kde čtvrtá souřadnice, která je zavedena z formálních důvodů, nám říká, jestli se jedná o souřadnice vektoru nebo bodu. Pro souřadnice bodu má velikost 1, pro souřadnice vektoru je 0. První tři souřadnice jsou stejné jako souřadnice ortogonální. Souřadnice bodu P a vektoru v, jsou tedy vyjádřeny jako sloupcové matice

[] []

Px P P= y Pz 1

Rov. 1

vx v v = y vz 0

Rov. 2

Pokud chceme znát kromě polohy bodu také jeho natočení, můžeme tento bod popsat maticí o rozměru 4x4. Matice pak bude obsahovat tři vektory, které budou tvořit souřadný systém a určovat natočení bodu, čtvrtý sloupec pak bude určovat polohu od vztažného souřadného systému.V případě koncového bodu robota (manipulátoru) se tato matice značí NSAP.

[

N x S x Ax Px N S y Ay P y T= y N z S z Az P z 0 0 0 1

]

Rov. 3

Strana 20 Kde

3 Hloubková analýza :

N je normálový vektor S je vektor stisku A je přibližovací vektor P je polohový vektor určující vzdálenost od počátku souřadného systému začátku robota

Obr. 4 Koncový Efektor

. 3.1.2

Transformace souřadnic

Pro přepočet (transformaci) souřadnic mezi lokálními souřadnými systémy, které jsou spojeny s daným článkem a globálním souřadným systémem, který je spojen s pracovním prostorem, je s výhodou využíváno maticového počtu. Je výhodné transformaci rozdělit na více elementárních transformací. Výsledná transformační matice se poté získá vynásobením jednotlivých elementárních matic. Obecné a translační matice se většinou značí velkým T nebo A a rotační matice značíme velkým R. T =T 1⋅T 2⋅⋅T n Rov. 4 3.1.3

Transformační Matice translačního pohybu

Při translačním pohybu nedochází ke změně natočení, což se v matici projeví tak, že vektory natočení budou stejné pro translaci v libovolném směru.

Translace ve směru osy x:

[ ]

1 0 T x= 0 0

0 1 0 0

0 x 0 0 1 0 0 1

Rov. 5

3 Hloubková analýza

3.1.4

Strana 21

[ ] [ ]

Translace ve směru osy y:

1 T y= 0 0 0

0 1 0 0

0 0 0 y 1 0 0 1

Rov. 6

Translace ve směru osy z:

1 0 T z= 0 0

0 1 0 0

0 0 0 0 1 z 0 1

Rov. 7

Transformační Matice rotačního pohybu Při rotačním pohybu nedochází ke změně vektoru polohy, ale mění se vektory natočení.

[ [ [

Rotace kolem osy x:

1 0 0 0 cos  x  −sin  x  R x= 0 sin  x  cos x  0 0 0

Rotace kolem osy y:

cos  y  0 R y= sin y  0

Rotace kolem osy z:

cos  z  −sin  z  R z= sin  z  cos z  0 0 0 0

0 sin y  1 0 0 cos  y  0 0

0 0 1 0

0 0 0 1

0 0 0 1

]

] ]

0 0 0 1

Rov. 8

Rov. 9

Rov. 10

Rotace kolem obecné osy r [ rx , ry , rz ]T o úhel φ:

[

r 2x 1−cos cos  r x r y 1−cos −r z sin  r x r z 1−cos −r y sin  2 Rr = r x r y 1−cos −r z sin  r y 1−cos cos  r y r z 1−cos −r x sin  2 r x r z 1−cos −r y sin  r y r z 1−cos −r x sin  r z 1−cos cos  Rov. 11

]

Strana 22

3.2


Modelování kinematického řetězce

Na obr. vidíme otevřený kinematický řetězec. Řetězec je tvořen jednotlivými linky, které jsou spojeny vazbami jointy. Link nula je spojen s rámem a má souřadný systém x0,y0,z0,, konec řetězce

Obr. 5 Otevřený kinematický řetězec. je v linku n. Tento konec se také nazývá Koncový Efektor.

3.2.1

Denavit – Hartenberg notace

Při modelování složitějších struktur je výhodné přiřazovat souřadnicové systémy pro jednotlivé články kinematického řetězce nějakým systémem. Denavit a Hartenberg v roce 1955 navrhli maticovou metodu systematického přiřazení souřadnicových systémů každému z článků kinematického systému. Metoda používá reprezentaci pomocí matice 4x4 homogenní transformace pro každý článek. My se budeme dále tomuto systému podrobně věnovat Denavit-Hartenbergův systém (zkráceně D-H). Postup pro nalezení transformačních rovnic metodou Denavitovou–Hartenbergovou (Denavitova–Hartenbergova notace). Popisujeme kloub i. 1. Nalezneme osy otáčení kloubů i − 1, i, i + 1. 2. Nalezneme přícku (společnou normálu) os kloubů i − 1 a i a os kloubů i a i + 1. 3. Nalezneme body Oi−1, Hi, Oi. 4. Osu zi položme do osy kloubu i + 1. 5. Osu xi položme do prodloužení příčky HiOi. 6. Osa yi tvoří s ostatními pravotočivou soustavu. 7. Označme vzdálenost bodu |Oi−1, Hi| = di. 8. Označme vzdálenost bodu |Hi, Oi| = ai. 9. Označme úhel mezi příčkami θi. 10. Označme úhel mezi osami kloubů i, i + 1, αi . 11. Pro rám je možné zvolit polohu bodu Oo kdekoliv na ose kloubu a osu x0 orientovat libovolně. Například tak, aby d1 = 0. 12. Pro chapadlo je možné opět zvolit bod On a orientaci osy zn při dodržení ostatních pravidel. 13. Jsou-li osy dvou po sobe jdoucích kloubů rovnoběžné, je možné polohu příčky zvolit, například tak, že di = 0. 14. Pro posuvné klouby lze polohu osy kloubu zvolit. 15. Transformace v kloubu je zcela popsána čtyřmi parametry ai, di, θi, αi. Parametry ai,


Strana 23

αi jsou konstanty, jeden z parametrů di, θi se mění s pohybem kloubu.[4] 

θi – je úhel kloubu od xi-1 osy k xi ose okolo osy zi-1 (v pravotočivém systému)



di – je vzdálenost z počátku (i-1)-té soustavy do průsečíku zi-1 osy s xi osou podél osy zi-1



ai – je offset posunutí z průsečíku osy zi-1 s osou xi do počátku i-té soustavy podél xi osy (nebo nejmenší rozdíl mezi osami zi-1 a zi).



αi – je offset úhlu od osy zi-1 k ose zi okolo osy xi (v pravotočivém systému)

Obr. 6 Dva sousední linky.

Strana 24


Obr. 7 Sousední souřadné systémy v DH. Účelem Denavit-Hartenbergova principu je tedy vytvoření transformační matice mezi dvěma sousedními lokálními souřadnými systémy v univerzálním tvaru tak, aby bylo možno provést snadnou algoritmizaci a naprogramování výpočtu přímé úlohy kinematiky. 3.2.2

Přímá úloha kinematiky

Jedná se o přepočet pozice koncového efektoru a jeho natočení v jeho souřadném systému do základního souřadného systému, při znalosti úhlů natočení v případě rotační vazby nebo posuvů v případě posuvné vazby. Vstupními parametry jsou natočení nebo posuv v jednotlivých vazbách a znalost řešeného kinematického řetězce. Výstupem je transformační matice, která transformuje polohu a natočení efektoru do základního souřadného systému.

T 0n q1 , q2 ,⋯, q n= A0b⋅A10  q1 ⋅A21  q2 ⋅⋯ Ann−1  qn 

[]

[]

xb yb =T 0n q 1 , q 2 ,⋯, q n ⋅ zb 1

3.2.3

xn yn zn 1

Rov. 12

Rov. 13

Inverzní úloha kinematiky

Inverzní úloha kinematiky je základním výpočtem kinematických mechanizmů. Je nezbytná pro polohování a pro orientaci jednotlivých článků mechanizmu v prostoru. Přímý výpočet kinematiky je relativně snadný při známé transformační matici. Naproti tomu je inverzní úloha kinematiky relativně složitý výpočet, který je stále v popředí zájmu mnoha pracovišť ve světě. Algoritmy výpočtů inverzní úlohy kinematiky jsou stále rychlejší a univerzálnější. Kinematická syntéza řeší problém, jak získat, ze známé polohy a orientace koncového efektoru, natočení nebo posuvy jednotlivých servopohonů robota. Základním kritériem, kterým se hodnotí metody inverzní transformace, je jejich univerzálnost, tj. možnost použití stejného algoritmu pro libovolnou kinematickou strukturu a počet stupňů volnosti mechanizmu


Strana 25

Obecný přehled metod inverzní transformace, dle jejich principu: ●

konkrétní struktura vektorová metoda

○ ●

univerzální metody aproximační

○

■

Taylorovým rozvojem transformační matice

■

Newton-Raphson metoda

optimalizační

○

■

heuristické

■

gradientní

Popis metody inverzní kinematiky pomocí aproximace použitím Taylorova rozvoje: n

T nb q1 q1, q 2 q2, ... , qn q n=T nb q 1, q2, ... , qn ∑ i =1

∂T nb q 1, q 2,  , q n ⋅ q i Rov. 14 ∂q i

kde n

i

∂T b ∂ A q  = A0b⋅A1b  q1 ⋅A21 q 2  i−1 1  A nn-1 q n=U ni q ∂ qi ∂ qi

Rov. 15

maticová rovnice pak má následující tvar n

T nb q q=T bn q∑ U ni q⋅ q 1

Rov. 16

32 33 t d1,4 = t v1,4 + u 31 1,4⋅ q1 + u 1,4⋅ q2 + u1,4⋅ q3

Rov. 17

32 33 t d2,4 = t v2,4 + u31 2,4⋅ q 1 + u 2,4⋅ q 2 + u 2,4⋅ q3

Rov. 18

i =1

d

v

31

32

33

t 3,4 = t 3,4 + u3,4⋅ q1 + u3,4⋅ q2 + u 3,4⋅ q 3 3.2.4

Rov. 19

Kinematická Analýza Humanoidního Robotu Pokud bychom chtěli sestrojit humanoidního robota, který by vykonával pohyby ekvivalentní

Strana 26


pohybům člověka, je inspirující podívat se na kosterní systém člověka. Člověku umožňuje pohyb kosterní systém, na který se upínají svaly. Kosterní systém je tvořen z 206 kostí, které jsou vzájemně pospojovány prostřednictvím kloubů. Kosterní spojení neboli klouby se dělí na tři hlavní: volně pohyblivá spojení, částečně pohyblivá spojení a nepohyblivá spojení.

Obr. 8 Kosterní systém člověka.

Obr. 9 Kladkový kloub (vpravo) a čepový kloub (vlevo) Na obrázcích nahoře vidíme kladkový kloub a čepový kloub. Kladkový kloub umožňuje natažení a stáhnutí kostí. Nalezneme jej na loktu, prstech u ruky, koleni, kotníku a palci u nohy. Kloub


Strana 27

tedy vykonává pohyb kolem jedné osy otáčení, která je kolmá k oběma osám připojených kostí. Kloub čepový se nachází na krku a na loktu, způsobuje otáčení jedné kosti kolem druhé. Tedy vykonává rotační pohyb, jehož osa je rovnoběžná s osou kosti.

Obr. 10 Sedlovitý kloub (vpravo) a kulovitý kloub (vlevo). Sedlovitý kloub zapříčiňuje poměrně volný pohyb kostí. Vyskytuje se například na palci u ruky. Kulovitý kloub je umístěn pouze na rameni a na kyčli. Tento kloub umožňuje nejvíce volný pohyb.

Obr. 11 Částečně pohyblivá spojení. Částečně pohyblivá spojení se nachází na páteři. Jedná se o pohyby mezi obratlovými kostmi, které jsou velmi omezené. Celková vysoká ohebnost páteře je ale dána znásobením jednotlivých pohybů malých kostí. Pohyb páteře je možný prostřednictvím tahu za kostní výběžky. Pohyb páteře se uskutečňuje čtyřmi způsoby, pohybem dopředu, dozadu a pohybem ze strany na stranu. Pohyb ohnutí dozadu způsobují svaly, které táhnou kostní výběžky na zadní straně páteře. Pohyb dopředu je dán uvolněním svalů. Při ohnutí vpravo táhnou svaly malé kostní výběžky, které jsou umístěny na pravé straně každého obratle. V případě ohybu vlevo jsou taženy kostní výběžky na straně levé. Humanoidního robota lze zrealizovat pouze s použitím rotačních vazeb, kde dvě rotační vazby nahradí jeden kloub. Hlava humanoidního robota bude obdobně jako u člověka platformou pro připojení nejrůznějších senzorů, především pak pro vidění. Její připojení ke zbytku robota lze realizovat dvěma rotačními vazbami. Pokud budou tyto vazby na sobě kolmé, tak pracovní prostor natáčení hlavy bude tvořen kuloplochou. Hlavu tak bude tvořit manipulátor s dvěma stupni volnosti.

Strana 28


Další částí je oblast hrudi a břicha. Pohyb této části je dost komplikovaný, protože je realizován pomocí páteře. My ho nicméně můžeme zjednodušit a nahradit také klouby. Nejlépe je nahradit ho dvěma klouby, jedním v oblasti hrudi pro natáčení ramen a druhým v oblasti pánve pro natáčení boků. Při ještě větším zjednodušení by také šlo použít pouze jeden kloub, kde by se jednotlivé rotační vazby rozdělily, jedna do oblasti hrudi, druhá do oblasti pánve. Toto nahrazení by však bylo příliš zjednodušené a mohlo by přinést řadu omezení v pohybu. Nejdůležitější částí lidského těla z hlediska kinematické analýzy jsou končetiny, kterými člověk nejvíce ovlivňuje své okolí. Člověk má čtyři končetiny, dvě ruce a dvě nohy. Z hlediska kinematiky jsou si velice podobné. Díky tomu je můžeme považovat za stejné a nahradit je jedním typem. Pokud opomeneme prsty jak na nohou tak na rukou, které lze realizovat přidáním vhodného nástroje na koncový efektor končetiny, je končetina složena ze tří kloubů. Kvůli tomu je realizace končetiny také nejsložitější. My se budeme dále v této práci zabývat končetinami, protože právě končetiny jsou pro humanoidního robota klíčovou částí.


3.3

Strana 29

Řízení Robotů

Co všechno musí mít robot, který bude schopen v některých oblastech nahrazovat člověka? Především se musí umět pohybovat a fyzicky ovlivňovat prostředí. Toho robot dosahuje Efektory, které jsou přemisťovány pomocí Realizátoru plánů. Tento subsystém se pak nazývá Motorický subsystém. Robot dále potřebuje zpětnou vazbu s prostředím, což se realizuje za pomoci soustavy snímačů a senzorů v Senzorickém subsystému. Poslední subsystém z níže uvedeného schématu je Kognitivní, ve kterém probíhá rozhodovací činnost robota a plánování k dosažení cílu.[14]

Obr. 12 Schéma obecného řízení robota 3.3.1

Pohyb Efektoru do požadovaného bodu

Při pohybu efektoru robota mezi dvěma uzlovými body, je potřeba vědět, zda je třeba brát ohled na trajektorii efektoru, popřípadě koncového bodu nástroje robotu nebo ne. S tohoto hlediska se řízení pohybu dělí na řízení metodou CP a metodu PTP. Při pohybu mezi uzlovými body se může měnit nejen poloha koncového efektoru, ale i jeho orientace. Tato změna může obecně probíhat podél jedné z os tvořících základní souřadný systém robota, ve kterém se mění poloha koncového efektoru nebo podél obecné prostorové osy, která je definována bodem (průvodič od počátku) a úhlem rotace kolem ní. V případě metody PTP (Point To Point) nezáleží na trajektorii, po které se efektor přesune do požadované pozice, ale záleží na pozici a orientaci v požadovaném bodě. Průběh rychlosti není konstantní, ale při startu je fáze náběhu, kdy pohony v jednotlivých kloubech zrychlují na maximální rychlost, potom se touto rychlostí přemisťují až do fáze brzdění. Náběh rychlosti a brzdění může být lineární nebo nemusí. Změna úhlů nebo posuvů podle toho jestli se jedná o vazbu rotační nebo posuvnou se mohou provádět na všech vazbách současně. Naproti tomu při řízení metodou CP (Continuous Point) se bere v úvahu také vztah mezi polohou koncového efektoru s ohledem na orientaci, rychlost a zrychlení a to nejlépe pro každý bod ležící na trajektorii přesunu koncového efektoru. Toho bohužel dosáhnout nelze, protože těchto bodů je nekonečně mnoho. Proto se spočítají jenom parametry pouze na některých bodech. Takto spočtené body se potom nazývají interpolační body a jejich počet je funkcí polohovací vzdálenosti, rychlosti přesunu, zrychlení a samozřejmě taktu regulační smyčky na jednotlivých servopohonech robotu. Pohyb servopohonů se potom děje dle časové posloupnosti podle spočtených natočení v závislosti na regulačním cyklu servopohonů. Takt jednoho cyklu regulační smyčky na všech servopohonech robotu může být stejný nebo nemusí.

Strana 30


Obr. 13 Interpolace trajektorie Výhodou metody CP oproti PTP spočívá v tom, že je známá trajektorie přesunu včetně natočení a díky tomu, je možné zjistit, jestli robot při přesunu koncového efektoru nezavadí o nějakou překážku prostředí, v kterém se pohybuje. Naproti tomu nevýhodou je nutnost naplánovat trajektorii přesunu koncového efektoru a vypočítat příslušné parametry pro všechny interpolační body tak, aby bylo možné řídít příslušné servopohony. Největší nevýhodou však je pomalost přesunu koncového efektoru, způsobená tím, že jednotlivé klouby se natáčejí postupně tak, aby bylo splněno dosažení jednotlivých parametrů ve všech interpolačních bodech. 3.3.2

Změna Efektoru a počátku

V případě našeho humanoidního robotu předpokládáme v kloubech pouze rotační vazby. Pohyb robotu se zajišťuje postupným natáčením jednotlivých vazeb v končetinách robotu podle plánu vytvořeným na vyšší úrovni řízení, které není součástí této práce. Každou končetinu je tak možné brát a řídit podobně jako průmyslový manipulátor.


Strana 31

Obr. 14 Průmyslový manipulátor Na rozdíl od průmyslového manipulátoru na obrázku, jehož počátek je pevně daný a manipulátor je tímto počátkem svázán se svým okolím. V případě končetiny je končetina takto svázána se zbytkem robota, který se ovšem může vůči svému okolí pohybovat a to právě pomocí končetin. Aby bylo možné realizovat tento pohyb pomocí končetin, je potřeba zajistit, aby především v případě končetin pomocí kterých se robot pohybuje, bylo možno libovolně určit koncový efektor z množiny jednotlivých článku končetiny robota. A stejně tak bod, vůči kterému se bude končetina pohybovat.

Strana 33

4

REALIZACE MATEMATICKÉHO MODELU ROBOTA .

4.1

Kinematický model

Končetina humanoidního robota je realizována pomocí šesti rotačních vazeb a tří ramen. Tato struktura je zobrazena na následujícím obrázku. Každá dvojice rotačních vazeb tak nahrazuje kloub člověka. Na každou dvojici pak připadá jedno rameno, které s touto dvojicí tvoří tzv. článek. Končetina pak obsahuje tři články. Každá rotační vazba má uhel q, který určuje natočení a ramena jsou určeny délkou d.

d1 q1 q2

q3 q4

d2 d3

Koncový Efektor

q5 q6 Obr. 15 Náčrt kinematiky končetiny. Díky 6 rotačním vazbám má končetina 6 stupňů volnosti. Končetina je schopná nahradit jak ruku, tak nohu. Na obrázku níže je vymodelován kinematický řetězec podle náčrtu, který je zobrazen výše.

Strana 34

4 Realizace matematického modelu robota

Obr. 16 Model Manipulátoru dle náčrtu končetiny. Jednotlivé osy otáčení se postupně mění o 45 stupňů nebo 135 stupňů tak, aby se článek byl schopen natáčet okolo dvou na sebe kolmých os a aby vazby měly stejný směr otáčení. Tím pracovní prostor článku vytváří kulovou plochu o poloměru délky ramena. Pracovní prostor celé končetiny je potom omezen délkou ramen. Pokud chce robot dosáhnout na pozici, která leží mimo tento prostor, musí přesunout počátek končetiny.

4.2

Nalezení DH parametrů

Na obrázku níže máme náčrt kinematického řetězce, který představuje manipulátor se šesti stupni volnosti. Jednotlivé klouby jsou znázorněny tak, aby se mohly natáčet podle os rotace, které jsou popsány písmenem q a číslem rotační vazby. Ke každé vazbě je pak přiřazen souřadný systém.

q2

q4

z0,y1

z1

q3 y0

d3

d2

d1

q1

q6

z2,y3

z2

q5 y2

z4,y5

z5

y4

z6

y6 x0,x1,x2 x3,x4,x5 x6


Strana 35

Obr. 17 Kinematický schéma s přiřazenými souřadnými systémy. Ze schématu výše lze potom snadno odvodit jednotlivé DH-parametry. Postupovat budeme tak, že vezmeme souřadný systém s nejmenším číslem porovnávaného se sousedním souřadným systémem. Úhel posunutí těchto souřadných systémů je potom parametr α. Parametr q je proměnná, jejíž počáteční hodnotu nastavíme na nulu. Délka d bude také všude nulová. Poslední parametr a potom bude pouze u vazeb, ze kterých vychází rameno a jeho hodnota bude rovna tomuto ramenu. Hodnoty všech DH-parametrů jsou pak vypracovány v následující tabulce.

Link

q

α

d

a

1

0

π/2

0

0

2

0

3*π/2

0

d1

3

0

π/2

0

0

4

0

3*π/2

0

d2

5

0

π/2

0

0

6

0

3*π/2

0

d3

Tab. 2 DH-parametry Manipulátoru končetiny. Zde jsou jednotlivé transformační matice pro převod do sousedního souřadnicového systému.

T 10=R q1 , z0⋅R / 2, x0=R / 2, x0⋅R q1 , y1

[

cos q1  −sin q 1 T 10= sin q1  cos q1  0 0 0 0

0 0 1 0

][

0 1 0⋅0 0 0 1 0

Rov. 20

][

cos q 1 0 0 0 0 −1 0 = sin q 1 1 0 0 0 0 0 1 0

[ [ [

0 sin q 1 0 −cos q 1 1 0 0 0

T 21=R q2,z1⋅R3∗ / 2,x1⋅T d1,x2

cos q 2 0 sin q 2 d 1 T 21= sin q 2 0 cos q 2 0 0 −1 0 0 0 0 0 1

T 32=R q3 , z2⋅R / 2, x3

cos q3  T 32= sin q3  0 0

T 43=R q4,z3⋅R3∗ / 2, x3⋅T d2,x4

cos q 4 0 sin q 4 d 2 T 43= sin q 4 0 cos q 4 0 0 −1 0 0 0 0 0 1

0 sin q3  0 −cos q3  1 0 0 0

] 0 0 0 1

]

0 0 0 1

]

Rov. 21

]

Rov. 22

Rov. 23

Strana 36


[ [

T 54=R q5 , z4⋅R  /2, x4

cos q5  T 54= sin q5  0 0

0 sin q5  0 −cos q5  1 0 0 0

T 65=Rq6,z5⋅R 3∗ /2, x5⋅T d3,x6

cos q 6 0 sin q6 d 3 T 65= sin q6 0 cos q6 0 0 −1 0 0 0 0 0 1

0 0 0 1

]

]

Rov. 24

Rov. 25

Výsledná matice pro transformaci z konce končetiny do počátku vznikne postupným vynásobením jednotlivých transformačních matic a bude pak vypadat takto:

T 60=T 21⋅T 32⋅T 43⋅T 54⋅T 65 4.3

Rov. 26

Rekonfigurovatelnost

Pokud chceme přesunout do určité pozice koncový efektor a vztažný bod nebude na začátku. Tudíž nechceme, aby se při tomto přesunu měnily rotační vazby na místech, které leží před vztažným budeme kolem kterého se končetina bude natáčet. Je potřeba změnit konfiguraci robota, to znamená, že je potřeba k výpočtu použít jiný matematický model. Tedy náš robot bude složen z jiného kinematického řetězce. Konfigurace lze rozdělit na dvě skupiny dopřednou a zpětnou. Přičemž zavedeme pojmy Start Link a End Link. Start Link se nachází vždy na počátku linku, tedy ve vazbě, která je připojena k tomuto linku. End Link se pak nachází na konci linku, jedná se tedy o efektor respektive koncový efektor . 4.3.1

Dopředné konfigurace

Pokud efektor (End Link) bude před vztažným bodem, tzn. (Start Link < = End Link), potom k výpočtu použijeme transformace odvozené z robotu které budou vymodelovány podle DH-parametrů v tabulce. Modelování probíhá tak, že postupně odebíráme jeden stupeň volnosti, tedy jednu vazbu. Odebíraná vazba je vždy ta první v konfiguraci. Tím je zajištěno, že hodnota Start Linku označuje vazbu (kloub), vůči kterému se bude robot natáčet.


Konfigurace Start Link 1

1

2

2

3

3

4

4

5

5

6

6

Označení Rotační vazby q1 q2 q3 q4 q5 q6 q2 q3 q4 q5 q6 q3 q4 q5 q6 q4 q5 q6 q5 q6 q6

Strana 37

DH - parametry q α 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0

d π/2 3*π/2 π/2 3*π/2 π/2 3*π/2 3*π/2 π/2 3*π/2 π/2 3*π/2 π/2 3*π/2 π/2 3*π/2 3*π/2 π/2 3*π/2 π/2 3*π/2 3*π/2

a 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0

0 d1 0 d2 0 d3 d1 0 d2 0 d3 0 d2 0 d3 d2 0 d3 0 d3 d3

Tab. 3 DH-parametry jednotlivých dopředných konfigurací. Výsledné transformační matice do koncového efektoru pak budou při jednotlivých konfigurací vypadat takto, kde horní index označuje konfiguraci, dolní indexy jsou čísla souřadných systémů pro které platí daná transformace. : 1

Konfigurace 1 (StartLink=1,EndLink=6)

T 60=T 60


T 60=T 10⋅T 61

Rov. 28


T 60=T 120⋅T 362

Rov. 29


4 T 60=T 130⋅T 63

Rov. 30


T 60=T 140⋅T 564

Rov. 31


T 60=T 150⋅T 665

Rov. 32

1

Rov. 27 2

Strana 38 4.3.2


Zpětné konfigurace

V případě, že vybraný efektor je menší než-li vztažná vazba, (EndLink<StartLink) musíme jednotlivé sériové manipulátory pro různé konfigurace modelovat opačným směrem podle parametrů uvedených v tabulce. V tomto případě bude mít robot pouze dvě ramena, neboť na konci třetího ramene není žádná vazba. Postup modelování bude následující. Souřadný systém šest modelovat nebudeme, protože tento souřadný systém je určen koncovému efektoru dopředného sériového manipulátoru, není tedy spřažen s žádnou rotační vazbou. První vazba je v tomto případě vazba šest, jejíž efektor (konec linku 6 ve zpětném režimu a počátek linku 6 v dopředném režimu) se nachází v souřadném systému pět. Dále najdeme D-H parametry které transformují souřadný systém na systém čtyři, kde se nachází pátá vazba. Takto budeme pokračovat, až do souřadného systému nula. Přehled jednotlivých D-H parametrů je v tabulce.

Konfigurace Start Link 1

1

2

2

3

3

4

4

5

5

6

6

Označení Rotační vazby q6 q5 q4 q3 q2 q1 q5 q4 q3 q2 q1 q4 q3 q2 q1 q3 q2 q1 q2 q1 q1

DH - parametry q α 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0

d -π/2 -3*π/2 -π/2 -3*π/2 -π/2 0 -3*π/2 -π/2 -3*π/2 -π/2 0 -π/2 -3*π/2 -π/2 0 -3*π/2 -π/2 0 -π/2 0 0

a 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0

0 -d2 0 -d1 0 0 -d2 0 -d1 0 0 0 -d1 0 0 -d1 0 0 0 0 0

Tab. 4 DH-parametry při konfigurací pro zpětný chod Pokud se použijí tyto konfigurace je potřeba nejprve vypočítat počátek robotu. Zjistíme ho tak, že spočteme konec robotu při dopředné konfiguraci. Tento počátek však není počátkem robota při zpětné konfiguraci vymodelovaným dle DH parametrů v tabulce, ale počátek ramene tři, které v těchto konfiguracích chybí. 2

T 00=1T 60⋅T E

Rov. 33


Strana 39

Kde horní indexy na levé straně označují chod (1 je dopředný a 2 zpětný) a TE se spočte jako: 1

−1 1

T E = T 60 ⋅ T 50

Rov. 34

Dále pak pokračujeme obdobně jako v předchozím případě. Konfigurace 1 (StartLink=6,EndLink=1)

2

2

2

1

T 60= T 00⋅ T 60

Rov. 35


2

T 60= T 00⋅ T 10⋅ T 61

2

2

1

2

2

Rov. 36


2

T 60= T 00⋅ T 20⋅ T 62

2

2

1

2

3

Rov. 37


2

T 60= T 00⋅ T 30⋅ T 63

2

2

1

2

4

Rov. 38


2

T 60= T 00⋅ T 40⋅ T 64

2

2

1

2

5

Rov. 39


2

T 60= T 00⋅ T 50⋅ T 65

2

2

1

2

6

Rov. 40

Strana 41

5

REALIZACE ŘÍDÍCÍHO PROGRAMU

Program nese název Simrob a slouží k ověření matematického modelu popsaným v předchozích kapitolách. Umožňuje analýzu pohybu končetiny humanoidního robota. Program je vytvořen jako dialogová aplikace. Obsahuje tři poddialogy, kde do prvního se zadávají parametry robota, druhý slouží pro výpočet a zobrazení číselných výsledků a poslední k provedení simulace a vizualizace průběhu pohybu.

5.1

Použité nástroje a knihovny

Program byl vytvořen ve vývojovém prostředí Microsoft Visual Studio 6.0, respektive v jeho části Visual C++ 6.0 se service packem 6. V jazyku C++ za použití standardní knihovny, MFC (Microsoft Fundamental Class), dále byly použity knihovna NewMat a knihovna Roboop. 5.1.1

Objektově orientovaná technologie

Začátek vzniku objektově orientovaného programování má již za sebou několik desítek let bohatou tradici. Objektově orientované programování ve zkratce OOP představuje nový směr programátorského přístupu na zápis zdrojových kódů. Princip OOP spočívá v myšlence sloučit data a operace nad nimi do jednoho celku „objektu“. Komunikace mezi jednotlivými objekty spočívá ve vzájemném zasílání zpráv. Každý z programovacích jazyků, umožňující objektově orientovaný přístup, má svůj specifický způsob implementace objektů a zasílání zpráv. Jak již bylo zmíněno, OOP spočívá v objektech a ve vzájemném zasílání zpráv mezi nimi. Programovací jazyk C++ navíc umožňuje sloučit OOP se strukturálním způsobem programování do jednoho zdrojového kódu. Detailní popis spolu s návodem jak vytvořit program s využití OOP není v této publikaci možný poskytnout. V seznamu použité literatury je přehled několika publikací, které se věnují OOP. Na následující řádcích této kapitoly lze nalézt jen krátké shrnutí. Jak již bylo zmíněno, objekty jsou hlavními prvky OOP. Objekty představují (spíše měly by představovat) reálné objekty, které vidíme kolem sebe, ať už živé (pes, zvíře, člověk, ...), nebo neživé (dům, robot, kombajn, ...). Spojitost mezi objekty v OOP a těmi reálnými je ta, že oba objekty mají vlastnosti a reagují na různé události, které jsou evokovány z okolních objektů. Objekty pak mohou vyvolat události u jiných objektů. Zápis objektů v C++ je reprezentován objektovou třídou (class), která definuje, jaké vlastnosti a události budou objekty od ní odvozené vykazovat. Objektová třída, která je vlastně jakýmsi vzorem pro vytváření objektů, začíná klíčovým slovem class, nebo struct. Vlastnosti objektu reprezentují jeho jednotlivé proměnné, které mohou být buď již zabudovaného typu (int, char, …), nebo jimi mohou být jiné objekty, případně ukazatele na tyto objekty. Chování objektů je definováno vnitřními metodami, což jsou v podstatě klasické funkce tak, jak je známe z procedurálního programování. Jelikož jsou svázány k objetu, tak je nazýváme metodami (někdy také členskými funkcemi) . Základní vlastnosti objektů jsou: – dědičnost – zapouzdření – polymorfismus Dědičnost umožňuje objektům přejímat vlastnosti a metody z jiných objektů. Objekt, který vzniká děděním, nazýváme potomkem a objekt, ze kterého vznikl, nazýváme předkem. Potomkovský objekt může být vytvořen děděním od několika předků. Tato vlastnost umožňuje progresivní vyváření objektů s relativně přehledným kódem. Zapouzdření je vlastnost, která objekt uzavírá před ostatními objekty. Ostatní objekty pak mohou přistupovat k objektu jen přes metody, který objekt nadefinuje jako veřejné. Díky zapouzdření získává objekt kontrolu nad svými daty. Polymorfismus je vlastnost objektů, která umožňuje na stejnou událost reagovat různě. Tato

Strana 42

5 Realizace řídícího programu

vlastnost bývá také popisována jako mnohoznačnost. Mezi vlastnosti také patří možnost přiřadit objekt do objektu třídy jeho předka. Objekty, které pracují s předkem, pak aniž by cokoliv tušily, v podstatě pracují s potomkem a mohou za jistých okolností volat jeho metody. Pro pochopení významu polymorfismu je nutná hlubší znalost objektů a celého OOP. Jak již bylo zmíněno výše, objekt je tvořen z objektového vzoru. Vytváření objektu může být dvojí, buď statické nebo dynamické. V obou případech vytváření je řízení běhu programu předáno konstruktoru. Konstruktor je v podstatě metodou objektu, která je definována v objektovém vzoru. V případě statického vytváření objektu, je objekt vytvořen již v době překladu a konstruktor tohoto objektu, je volán ještě před voláním hlavní funkce programu main. Dynamické vytváření objektu, je prováděno za chodu programu. V místě, kde chceme, aby došlo k vytvoření objektu, pak za pomoci operátoru new a identifikátoru objektového vzoru (class) vytvoříme nový objekt. V této době je řízení běhu programu předáno taktéž konstruktoru. Toto vytvoření je podmíněno tím, zdali operační systém umožní alokovat potřebnou velikost operační paměti. Každý objektový vzor musí obsahovat konstruktor. V případě, že není konstruktor definován, pak jej překladač vytvoří automaticky. Pro další informace opět odkazuji na použitou literaturu [6]. Opakem k vytváření objektů je jejich ničení. Na rozdíl od způsobu vytváření statický a dynamický je ničení pouze dynamické. Objekty vzniklé staticky jsou zničeny operačním systémem automaticky spolu s ukončením programu. Objekty vzniklé dynamicky, lze taktéž dynamiky zničit (uvolnit z paměti). Samotné ničení je provedeno operátorem delete a ukazatelem na objekt. Destruktor je metoda objektu, která je volána těsně před zničením objektu. I v případě destruktoru platí, že není-li explicitně definován, pak je definován automaticky překladačem. 5.1.2

Jazyk C++

Jazyk C++ vychází z jazyka C. Jazyk C má svůj počátek ve firmě AT&T v roce 1972, kde hlavním vývojářem byl Dennise Retchie. Původní platformou pro jazyk C byl Unix. Dalším významným milníkem byl rok 1978. V tomto roce D. Retchie vydal spolu s dalším kolegou verzi nazvanou „Kernighan-Retchie C“. V následujícím období byly snahy vydat jednotnou normu, která by definovala nutné standardy nově se rozvíjejícího jazyka C. Tyto snahy byly naplněny roku 1984, kdy vyšla norma ANSI C. V tomto období dochází k vyvíjení nových programovacích jazyků založených na jazyku C (např. Concurrent C, Objective-C). V této době již zde byly i jiné programovací jazyky využívající objektový přístup Smalltalk vycházející z jazyka Simula. Sloučením principů jazyka C a Simula vzniká v roce 1980 jazyk C with Classes. V červenci 1983 již vychází C++. Od této chvíle se již začíná plně pracovat na zlepšování a standardizaci jazyka C++. V roce 1998 vychází norma C++ ANSI/ISO. Jazyk C++ je v současnosti mezi programátory velmi rozšířen a oblíben. Za velkou popularitou jazyka C++ stojí množství vydávané dokumentace, silná podpora od producentů vyvíjejících překladače a podpora prakticky pro všechny možné platformy. Mezi významné společnosti, které se podílejí na vývoji překladačů, jsou GNU Free Software foundation, vydala a udržuje překladač G++, který kromě jazyka C++ zahruneje i další (např. Objective C, ADA, JAVA, ...). Společnost Borland vydala C++ Builder, její hlavní vývoj však směřuje na podporu jazyka Pascal. Na překlad zdrojových kódů pod OS Windows je jednoznačným favoritem překladač od společnosti Microsoft, který kromě samotného překladače má nejlépe propracované celé vývojové prostředí. Vydávaný překladač včetně IDE (Integrated Development Enviroment) nese v současnosti označení Visual Studio 2007. Následující text krátce popisuje standard jazyka C ++. Každý program napsaný v C++ lze rozdělit na jednotlivé jazykové prvky. Základními stavebními prvky jazyka C++ jsou: – identifikátory – klíčová slova – operátory – komentáře – oddělovače


Strana 43

Napsaný zdrojový kód programu lze sice rozdělit na výše uvedené prvky, ale pro jeho pochopení, nebo napsání to ani zdaleka nestačí. K napsání zdrojového kódu jsou nutnou podmínkou znalosti jazykové syntaxe, knihovních funkcí a cílové architektury. Pro zápis zdrojového kódu, který implementuje požadovanou úlohu, jsou nutné mimo jiné i schopnosti a znalosti, které umožní převést řešenou úlohu na algoritmus, který je již možný aplikovat v konkrétním jazyku, v našem případě C++. Pro obecný popis úloh se používá velmi rozšířený UML (Unified Model Language). Hlavní silnou zbraní jazyka C++ je velmi bohatý repositář knihovních funkcí. Knihovní funkce lze rozdělit do dvou kategorií, standardní a specifické. Standardní knihovnou se rozumí soubor knihoven, které musí být zahrnuté, aby bylo možno považovat překladač za standart ANSI/ISO. Standardní knihovna zahrnuje základní prostředky pro napsání programu, který je již spustitelný pod OS, který podporuje normu POSIX. Specifické knihovny definují prostředky pro přístup k různým ovladačům HW, nebo poskytují funkce pro řešení různých úloh, které nejsou definovány standardem ANSI/ISO. Knihovny kromě prostředků pro přístup k OS, také nabízejí širokou škálu všech možných algoritmů. Samotný program se textově píše do souboru a ten se předá do překladače, který z tohoto a dalších nutných souborů vytvoří spustitelný program. Záležitostí konvence je, že zdrojový kód má příponu .cpp pro jazyk C++ a .c pro jazyk C. Pro zvýšení přehlednosti lze zdrojový kód rozložit do několika souborů. Pro oddělení implementace od deklarace a možnost uchování implementaci, jako know how, se část kódu ukládá do hlavičkových souborů, které mají příponu .h. Samotná činnost překladu zdrojového kódu je pro názornost zobrazena na obr. 18. *.obj

*.lib

*.obj

Zdrojový kód *.cpp *.h

*.lib

Preprocesor

Compiler

linker

*.i

*.obj

*.exe *.lib *.dll

Obr. 18 Schéma činnosti překladače. Preprocesor je část překladače, jehož činnost je při překladu spuštěna jako první. Vstupem je překládaný soubor, v něm jsou rozbaleny veškerá makra. V této části nemůže překladač nahlásit žádnou chybu. Výstup z preprocesoru je automaticky předán do samotného překladače (Compiler), který převede zdrojový kód do strojového kódu. Takto vytvořený strojový kód je pak následně spojen s jinými strojovými kódy (knihovnami), čímž vznikne spustitelný program. 5.1.3

Vývojové Prostředí

Jako vývojové prostředí pro tvorbu programu jsem zvolil nástroj Microsoft Visual Studio 6.0, jehož součástí je překladač Visual C++ 6.0, resource editor, a spousty dalších nástrojů. Důvodem volby je dostupnost ve školních počítačových učebnách Ústavu Automatizace a Informatiky. Dalším důvodem je rozšířenost tohoto prostředí a díky tomu existence spousta literatury a návodů pro toto prostředí.

Strana 44 5.1.4


Microsoft Fundamental Class

MFC je zkratkou pro Microsoft Foundation Class. MFC je knihovnou, která je framworkem pro aplikace běžící pod OS WIN napsaných v C++. MFC zajišťuje mnoho nezbytného kódu pro řízení oken, menu, dialogu, obstarává základní vstupy a výstupy, ukládání datových objektů, atp. Vše, co musí programátor učinit, je přidat svůj specifický kód do tohoto frameworku. Díky přirozené vlastnosti C++, je velmi snadné přetížit nebo rozšířit základní vlastnosti, které jsou tímto frameworkem zajišťovány. MFC je součást programovacího nástroje MSVS, který obsahuje mnoho grafických nástrojů pro vytváření kódu, ikon, menu, dialogů, atd. Základem MFC je třída CObject, od které dědí všechny ostatní. Tato třída definuje základní prostředky pro ukládání a otevírání souborů, dynamické vytváření a odstraňování poděděných tříd, zjišťování správnosti proměnných, atd. Třída CObject není určena na přímé použití, ale na to, aby se od ní dědilo. Pro vytvoření programu spustitelného pod OS WIN je nutné vytvořit statický objekt ze třídy CWinApp nebo lépe vlastní třídy, které od CWinApp dědí. Přesný způsob jak CwinApp použít, lze nalézt v [6]. Samotná třída CWinApp nám ještě neumožní to, aby aplikace měla vizuální podobu, toho lze docílit objektem, v jehož základu se nachází třída CFrameWnd. CFrameWnd obsahuje množství proměnných pro nastavení okna např. vzhled, chování, atd. Tyto vlastnosti lze prakticky libovolně na sobě nezávisle aplikovat, čímž lze docílit veliké variability. 5.1.5

Knihovna NewMat

Knihovna je určena pro zjednodušení práce s maticemi a operací s nimi. Je napsaná v jazyku C ++ a muže se tedy bez potíží v tomto jazyku používat. Jejím autorem je Robert Davies z Nového Zélandu. Je uvolněná jako freeware uživatel ji může používat ve svém jak komerčním, tak nekomerčním programu a také ji může modifikovat. V tomto programu je požita v tuto dobu nejnovější verze, a to je verze 11. Knihovna je vytvořena jako multiplatformní a lze ji zkompilovat mnoha kompilátory kromě kompilátoru ve Visual C++ také Gnu C++, Intel C++, Sun C++ atd. Z důležitých hlavičkových souborů je potřeba uvést tyto: include.h - zde jsou zpřístupněny nastavení kompilátoru newmat.h - pro přístup k hlavní maticové knihovně newmatap.h - pro přístup k rozšířeným maticovým rutinám jako Choleského dekompozice, QR triangularizace atd. ● newmatio.h - pro přístup k výstupním rutinám. Dá se použít pouze s kompilátorem, který podporuje standartní vstupně/výstupní rutiny zahrnující manipulátory. ● newmatnl.h - pro přístup k nelineárním optimalizačním rutinám. ● ● ●

Jednotlivé prvky matice jsou reálného typu mohou být buď typu real, nebo double ,který z těchto typů bude používán, se nastavuje v hlavičkovém souboru include.h Matice je tvořena objektem třídy Matrix. Dále jsou k dispozici odvozené třídy ColumnVector, RowVector pro vektory a několik tříd pro speciální druhy matic např: SquareMatrix, UpperTriangularMatrix, LowerTriangularMatrix, SymmetricMatrix a DiagonalMatrix. Konstruktor se pak dá volat takto: Matrix A(m,n); kde A je název matice a m,n které jsou celá čísla jsou rozměry matice. Konstruktor neinicializuje prvky matice. Pro nastavení všech prvku na 0 muže vypadat viz níže Matrix A(m, n); A = 0.0; Přistupovat k jednotlivým prvkům matice se dělá buď A(i,j) nebo A.element(i,j). Pokud aktivujeme #define SETUP_C_SUBSCRIPTS v souboru include.h, můžeme pak používat tradiční C styl tedy A[i][j].


Strana 45

Pro kopírování matice z jedné do druhé lze buď použít operátorů = nebo << nebo metody Inject. A.inject(D); Pro uložení lze také použít metodu swap A.swap(B); nebo swap(A,B); Vložení hodnot do matice lze také několika způsoby například: Matrix A(3,2); Real a[] = { 11,12,21,22,31,33 }; A << a; nebo Matrix A(3,2); A << 11 << 12 << 21 << 22 << 31 << 32; a ještě lze naplnit matici po jednotlivých řádcích nebo sloupcích. Matrix A(3,2); A.row(1) << 11 << 12; A.row(2) << 21 << 22; A.row(3) << 31 << 32; Třída Matrix má také spoustu užitečných metod, které lze rozdělit na oblasti podle použití, jsou to funkce pro přístup k prvkům, kopírování, skalární pro zjišťování rozměrů, skalární funkce výpočtový, skalární maxima-minima, funkce pro změnu typu, funkce pro řízení paměti, a změnu dimenzí např.: Matrix A(3,2); A.t(); // transponuje matici A.i(); // vytvoří inverzní matici Vlastnosti

Maticová knihovna

Pole

Výrazy

* násobení, inverzní funkce

-

Přístup k prvkům

Přístup k prvkům přes pozice.

Přístup k prvkům přes iterace.

Základní funkce

např: determinant, trace

Násobení matic jako funkce.

Pokročilé funkce

např. eigenvalue analýza

-

Typy Prvků

Real a complex

real,integer,string, atd.

Typy

čtvercová,symetrická,diagonální jedno, dvě, tří rozměrná pole. Tab. 5 Srovnání vlastností Newmat a pole

Strana 46


Knihovna je navržena tak aby optimálně zvládala malé a středně velké matice, u velkých matic je lepší vzhledem k rychlosti implementovat matice pomocí pole. V tomto programu se používají malé matice, ale zato se s nimi provádí mnoho operací jako násobení nebo hledání inverzní matice. Což spolu se závislostí jiné knihovny na této vedlo k rozhodnutí a jejím použití v tomto programu. 5.1.6

Knihovna Roboop

Jedná se programovou knihovnu určenou k syntéze a simulaci modelů robotických manipulátorů. Knihovna je psaná v jazyce C++ a uvolněna je pod licencí GPL. jednotlivé funkce hlavní třídy Robot lze rozdělit na tři části. Kinematické, Dynamické a Lineární dynamické pro sériové robotické manipulátory. Knihovna dále obsahuje funkce pro homogenní transformace a spousty dalších. Pro homogenní transformaci jsou k dispozici funkce jako trans, rotx, roty, rotz, rotd, tyto funkce vrací typ ReturnMatrix, který je popsán v předchozí kapitole. Jako příklad si můžeme uvézt funkci pro translaci, jejíž syntax je následující. ReturnMatrix trans(const ColumnVector & a);. Jako parametr si funkce bere vektor, který udává posunutí. Funkce pak vrátí matici, která umožní přepočet do hlavního souřadného systému.

[ ]

1 0 trans a 1 , a 2 , a 3 = 0 0

0 1 0 0

0 a1 0 a2 1 a3 0 1

Hlavní třídou knihovny je třída Robot. Tato třída reprezentuje sériový manipulátor popsaný pomocí Denavit-Hartenbergova principu. Kromě toho existuje i třída mRobot, která používá modifikovaný D-H princip. Robot je realizován pomocí pole prvků třídy Link. Počet prvků tohoto typu je daný počtem stupňů volnosti robota. Pokud zavoláme konstruktor bez parametrů vytvoří se model s jedním stupněm volnosti. Jako parametr lze také použít matici typu Matrix. Struktura této matice je potom následující, robot má tolik stupňů volnosti, kolik má matice řádků. Význam jednotlivých prvků v řádku je pak popsán v následující tabulce, která je přejata a přeložena z dokumentace knihovny Roboop.[10]


Strana 47

sloupec

jméno

popis

1

σ

typ vazby (0 je rotační, 1 je posuvná)

2

q

Denavit-Hartenberg parametr

3

d


4

a


5

α


6

q_min

minimální hodnota ve vazbě

7

q_max

maximální hodnota ve vazbě

8

q_off

offset vazby

9

m

10

c_x

těžiště hmotnosti linku okolo osy x

11

c_y

těžiště hmotnosti linku okolo osy y

12

c_z

těžiště hmotnosti linku okolo osy z

13

I_xx

prvek xx matice tenzoru setrvačnosti

14

I_xy

prvek xy matice tenzoru setrvačnosti

15

I_xz

prvek xz matice tenzoru setrvačnosti

16

I_yy

prvek yy matice tenzoru setrvačnosti

17

I_yz

prvek yz matice tenzoru setrvačnosti

18

I_zz

prvek zz matice tenzoru setrvačnosti

19

I_m

setrvačnost rotoru motoru

20

Gr

brzdící koeficient motoru

21

B

koeficient vizkozity tření motoru

22

Cf

koeficient columbovského tření motoru

23

Immobile

hmotnost linku

znak pro zamknutí vazby

Tab. 6 Parametry objektu třídy Robot Z prvků uvedených v tabulce jsou pro nás důležitý pouze prvky jedna až osm. Tyto prvky jsou parametry robota z hlediska kinematiky. Ostatní prvky pak popisují setrvačnost a pohon ve vazbě a v programu nejsou používány. Z hlediska metod třídy Robot používaných v programu se nejvíce využívají metody set_q, kine, inv_kine. Tyto metody si nyní popíšeme trochu více. Definice první metody vypadá následovně. void set_q(const ColumnVector & q); Funkce uloží do objektu třídy Robot hodnoty natočení v jednotlivých vazbách. Kde &q je reference na vektor, který obsahuje hodnoty, které představují žádané hodnoty natočení. Funkce kine provádí přímou úlohu kinematiky a vrací transformační matici z počátku do konce manipulátoru v případě defauldního parametru jinak do efektoru toho linku, který se zadá jako parametr. ReturnMatrix kine (Matrix & Rot, ColumnVector & pos) const Poslední důležitou funkcí je funkce inv_kine, která řeší inverzní úlohu kinematiky. Funkce vrací typ ReturnMatrix, který obsahuje hodnoty natočení jednotlivých vazeb. Jako vstup pak referenci na typ Matrix se žádanými souřadnicemi efektoru. ReturnMatrix inv_kine (const Matrix &Tobj, const int mj, const int endlink, bool &converge);

Strana 48

5.2


Dialogy a ovládání programu

Program má hlavní dialog, na kterém jsou umístěny formou záložek další tři dialogy. Kromě těchto dialogů, které jsou podrobně popsány v následujících kapitolách, program obsahuje několik standardních dialogů, které nabízí prostředí MFC jako jsou dialogy na otevření nebo uložení do souboru. Na hlavním dialogu se kromě poddialogů nachází tlačítka pro opuštění programu nebo zobrazení nápovědy. 5.2.1

Nastavovací dialog

Dialog se používá především pro zadání geometrických vlastností robota. Jednotlivé parametry se zadávají v horní části dialogu. Tato část je rozdělena na tři obdobné oblasti, každá pro jeden ze tří článků. Parametr d označuje délku ramene. Dále jsou k dispozici kolonky pro zadání minimálních a maximálních úhlů natočení v jednotlivých vazbách.

Obr. 19 Panel pro zobrazení a nastavení parametrů robota. Na dialogu jsou dále k dispozici tlačítka, kde tlačítko Create vytvoří robota se zadanými parametry. Tlačítka Otevri a Ulož slouží k nahrání a uložení parametrů do souboru. Tlačítko Parametry zobrazí všechny parametry robota. Jednotlivé parametry jsou popsány v předchozí kapitole o knihovně roboop. 5.2.2

Výpočtový dialog

Tento dialog se nazývá Kinematika a slouží k ověření jednotlivých přesunů kvůli zjištění dosažitelnosti nebo hodnotám ať už kloubů nebo NSAP matice.


Strana 49

Obr. 20 Výpočtový panel Dialog je opticky rozdělen do několika částí. Část Úhly v rotačních vazbách obsahuje šest editboxu. Zde se zadávají hodnoty natočení uhlů v jednotlivých vazbách. Pokud se počítá Inverzní úloha kinematiky, tak se sem vypíší vypočítané hodnoty. Hodnoty se zkrátí na tři desetinná místa.

Obr. 21 Oblast hodnot natočení vazeb Pro zadávání pozice a natočení efektoru slouží další část dialogu, a ta má název Koncový Efektor. Obsahuje šestnáct editboxů, které jsou rozmístěny do tvaru matice 4x4. První řádek představuje hodnoty souřadnice x, druhý hodnoty souřadnice y a třetí je pro souřadnice z, poslední řádek pak informuje, jestli se jedná o souřadnice vektoru nebo bodu. Zde se zadávají hodnoty pozice a natočení efektoru při inverzní úloze a vracejí vypočítané hodnoty při přímé úloze kinematiky. Při zadávání se zadávají hodnoty od počátku robota (počátek Linku 1) a vracejí se hodnoty pro transformaci do globálního souřadného systému.

Strana 50


Obr. 22 Oblast hodnot pozice efektoru Konfigurace robotu se zadává do části konfigurace, která obsahuje dva ComboBoxy. Jeden je pro Start Link, kde se určí vazba, která bude vůči okolí stacionární. V druhém End Link se pak nastaví koncový efektor, pro který budou platit hodnoty v předchozí popisované oblasti.

Obr. 23 Panel pro zadání konfigurace(vlevo), vyrolovaný Start Link(vpravo) Vizualizace stavu robota se zobrazuje ve dvou oblastech. První od shora představuje pohled v osách X-Y. Osa X je vodorovná a kladná část vede doprava. Osa Y je pak kladná ve směru dolu. Druhá pak představuje zobrazení v osách X-Z. Směr os je totožný s předchozím případem. Osa X je doprava kladná, osa Z je kladná dolu. Jednotlivé vazby jsou vykresleny kružnicí. Počínaje vazbou q1 na Linku 1 konče vazbou q6 ve směru k modrému čtverečku. Ten představuje konec končetiny. Úsečky pak představují jednotlivá ramena. První rameno je to, které je připojeno ke kružnici, z které vychází jen jedna úsečka, následuje druhé rameno a třetí je pak připojeno k modrému čtverečku.


Strana 51

Obr. 24 Oblast pro vizualizaci výsledků Dále je na dialogu okýnko pro výstupní informace. Tím může být třeba velikost zoomu nebo vypočtené hodnoty či parametry robotu. Tlačítko Získej pozici slouží k spuštění úlohy přímé kinematiky a tlačítko Získej úhly pak k inverzní úloze kinematiky. Tlačítko Parametry vypíše parametry končetiny. Poslední tlačítkem je Reset, to nastaví počáteční pozici robota. 5.2.3

Simulační dialog

Dialog slouží k simulaci pohybu, který je složen s více příkazů. Průběh pohybu je možné sledovat ve vizualizační části a tak ověřit správnost matematického modelu.

Obr. 25 Simulační dialog

Strana 52


Při zadávání pohybových příkazů je potřeba nejprve nastavit typ. Zadání lze zadávat buď natočení v jednotlivých úhlech nebo pozici efektoru přes NSAP matici. Výběr je realizován pomocí prvků Radiobutton. Při spuštění není vybrán žádný.

Obr. 26 Oblast pro zadání typu příkazu Poté se nastaví jednotlivé parametry buď v oblasti pro nastavení jednotlivých vazeb, nebo souřadnice efektoru. Nakonec se zadá konfigurace. Realizace je obdobná předchozímu dialogu jedná se o prveky ComboBox. Obr. 27 Zde se zadává konfigurace v simulačním panelu Když je příkaz nastaven, je potřeba ho vložit do seznamu příkazů, operace se seznamem se provádějí tlačítky. Tlačítku Add přidá příkaz do seznamu na poslední pozici seznamu. Remove odstraní poslední příkaz ze seznamu. Start spustí vykonávání příkazů. Tlačítka Open a Save slouží k uložení a nahrání příkazů do souboru. Poslední tlačítku Reset nastaví počáteční polohu robota.

Obr. 28 Tlačítka pro ovládání simulace Vizualizace pohybu pak probíhá formou animace, která se jako v předchozím případě výpočtového dialogu, zobrazuje do dvou oblastí jedna pro osy x-y a druhá pro osy x-z.


Strana 53

Obr. 29 Oblast pro vykreslování animace Dialog ještě obsahuje dva posuvníky, jeden pro nastavení zvětšení a druhy pro nastavení rychlosti. Animace posuvníky jsou realizovány pomocí prvků Slider, na které jsou namapovány instance třídy CSliderCtrl.

5.3

Datová a programová struktura

5.3.1

Rozdělení zdrojových kódů

Zdrojové kódy programu jsou rozděleny pro přehlednost do několika souborů. Zde uvádím popis těch nejdůležitějších.  simrob.cpp- Zde se vytvoří instance třídy CSimrobApp která zabaluje objekt třídy . CSimrobDlg 

simrobDlg.cpp – Obsahuje definici třídy CSimrobDlg.



MyTabCtrl.cpp – Obsahuje definici třídy CMyTabCtrl.



Dialog1.cpp – Obsahuje definici třídy CDialog1, která zapouzdřuje dialog pro zadávání nastavení robota.



Dialog2.cpp – Obsahuje definici třídy CDialog2, která zapouzdřuje výpočtový dialog.



Dialog3.cpp – Obsahuje definici třídy CDialog3, která zapouzdřuje simulační dialog.



ioroboop.cpp – Obsahuje definice tříd CMaticPrevod, CManipulator, CKoncetina.



simulace.cpp – Obsahuje definice tříd CKoncetinaSim, CPrikaz, CPrikazy.



kresli.cpp – Obsahuje definici třídy CKresli.

Používané hlavičkové soubory většinou korespondují se soubory cpp, dále následuje popis hlavičkových souborů.  simrob.h – Hlavičkový soubor s deklarací třídy CSimrobApp

Strana 54

5.3.2

5 Realizace řídícího programu 

simrobDlg.h –Hlavičkový soubor s deklarací třídy CSimrobDlg



mytabctrl.h –Hlavičkový soubor s deklarací třídy CMyTabCtrl.



Dialog1.h – Hlavičkový soubor s deklarací CDialog1.



Dialog2.h –Hlavičkový soubor s deklarací CDialog2.



Dialog3.h – Hlavičkový soubor s deklarací CDialog3.



ioroboop.h –Inkluduje knihovnu roboop a oobsahuje deklarace CManipulato, CKoncetina a CMaticPrevod.



simulace.h – Inkluduje ioroboop.h a absahuje deklarace tříd viz. simulace.cpp.



kresli.h- Hlavičkový soubor ke kresli.cpp obsahuje deklaraci třídy Ckresli.

Popis vytvořených a použitých tříd

Robot je tvořen instancí třídy CKoncetina pro potřeby výpočtového panelu a třídou CKoncetinaSim pro potřeby simulačního panelu, která je potomkem třídy Ckoncetina, rozšířená především o metodu pro řízení metodou PTP (Point To Point). Tyto třídy agregují dva objekty třídy CManipulator. Každý pro jeden ze dvou typů konfigurací.Každý příkaz zadaný v simulačním panelu je realizován instancí třídy CPrikaz, která dědí po třídě storage_data. Ukazatelé na tyto příkazy se pak nacházejí v instanci třídy CPrikazy. Instance třídy Ckresli slouží k vizualizaci jak ve výpočtovém tak simulačním panelu dědí po třídě CStatic.CMaticPrevod je třída navržena především pro převod typu Matrix na naformátovaný textový řetězec. Instance tříd CDialog1, CDialog2, CDialog3 a CSimrobDlg reprezentují jednotlivé dialogy programu a všechny jsou potomky třídy CDialog. Obsluha přepínaní mezi jednotlivými dialogy je realizována ve třídě CMyTabCtrl, která je potomkem třídy CTabCtrl. Celá aplikace je pak zabalena třídou CSimrobApp. Tato třída dědí po CWinApp. Na následujícím obrázku jsou zobrazeny jednotlivé třídy z prostředí Visual Studia


Strana 55

Obr. 30 Přehled použitých tříd Ukázka zdrojových kódů třídy CKoncetinaSim. Jedná se o rozšíření třídy CKoncetina pro potřeby simulace s implementací řízení typu PointToPoint. class CKoncetinaSim : public CKoncetina { private: int nasobic; double krok; ColumnVector pozadovane_uhly,aktualni_uhly; public: CKoncetinaSim(); //konstruktor defauldni DH-parametry ~CKoncetinaSim(); //destructor CKoncetinaSim(Matrix &aMat); //konstruktor s DH-parametry public: int Get_iterace(double akrok); double pohyb_uhel(double akrok, int ajoint); void pohyb_ptp(double akrok,bool &ahotovo); void set_end_efektor(int aend_efektor, int astart_link); void SetPrikaz(ColumnVector &aq,int aEndLink, int aStartLink); void SetPrikaz(Matrix &ansap,int aEndLink, int aStartLink); void SetPrikaz(CPrikaz* aprikaz); }; void CKoncetinaSim::pohyb_ptp(double akrok,bool &ahotovo)

Strana 56


{ int hot=0; for (int i=1;i<7;i++) { if (pozadovane_uhly(i)>aktualni_uhly(i)) { aktualni_uhly(i)=aktualni_uhly(i)+M_PI/akrok; if (pozadovane_uhly(i)<=aktualni_uhly(i)) { aktualni_uhly(i)=pozadovane_uhly(i); } } if (pozadovane_uhly(i)=aktualni_uhly(i)) { aktualni_uhly(i)=pozadovane_uhly(i); } } if (pozadovane_uhly(i)==aktualni_uhly(i)) hot++; if (hot==6) ahotovo=true; else ahotovo=false; } SetUhly(aktualni_uhly); } Na následujícím obr. 31 jsou znázrněny vztahy mezi objekty z hlediska dědičnosti


Strana 57

CStatic

CObject

(from Controls)

(from CObject Classes)

storage_prikazy

Ckresli

CPrikazy

CKoncetina Ckresli_posun

Ckreslisim

CPrikaz

CKoncetinaSim

CWinApp

CTabCtrl

(from Application Architecture)

(from Controls)

CSimrobApp

CMyTabCtrl

CManipulator

CMaticPrevod

CDialog (from Dialog Boxes)

CDialog3

CDialog2

CSimrobDlg

CDialog1

CAboutDlg

Obr. 31 Znázornění dědičnosti Dále si ukážeme provázanost mezi jednotlivými objekty bez dědičnosti, ta je zobrazena na obr. 32. Vzhledem k přehlednosti je diagram rozdělen do dvou. První zobrazuje návaznosti na objekt třídy CDialog2. Druhý potom zobrazuje návaznosti na objekt třídy CDialog3

Strana 58


CMy TabCtrl

Robot

Ckresli

(from roboop)

m_DialogID[3] : int m_nPageCount : int

Ckresli() Reset() Testov ani() SetSouradnice() SetBody () <> SetRobot() <> Posun() ...

CMy TabCtrl() <> ~CMy TabCtrl() InitDialogs() Activ ateTabDialogs() <> OnSelchange() <> OnGetobject() <> OnClick()

+m_obrazxz +m_tbCtrl

+m_obrazxy

CDialog2

CSimrobDlg m_hIcon : HICON

SetKoncetina() kreslixy () get_nacti_uhly () get_pozice_ef ektor() set_pozice_ef ektor() set_nacti_uhly () CDialog2() <> DoDataExchange() <> OnButton2() ...

Nov y robot() CSimrobDlg() <> DoDataExchange() <> OnInitDialog() <> OnSy sCommand() <> OnPaint() <> OnQuery DragIcon() <> OnOK() <> OnClickTab1() <> OnHelp()

Robot() Robot() Robot() Robot() Robot() <> ~Robot() <> robotTy pe_inv _kin() ... <> kine_pd() inv _kin() <> inv _kin() <> inv _kin_rhino() <> inv _kin_puma() <> inv _kin_schilling() ... <> jacobian() <> jacobian() <> jacobian_dot() ... <> dTdqi() <> dTdqi() <> torque() <> torque() ...

+robots[6]

+m_simrobdlg +my koncetina

CDialog1 GetKoncetinaSim() Setdata() GetKoncetina() Getdata() Getdata() CDialog1() <> DoDataExchange() <> OnCreate() <> OnParametry () <> OnUloz() <> OnOtev ri()

CKoncetina konv ergence : bool robot_1 : CManipulator robot_2 : CManipulator

+m_koncetina

Get_v y pis_pozice() GetEndLink() GetStartLink() Reset() CKoncetina() CKoncetina() ...

CManipulator konv ergence : bool ... dof : int v y brany robot : int CManipulator() CManipulator() AktualizujUhly () SetPoziceRet() AktualizujDT() SetUhly () SetUhly () ...

Obr. 32 Provázanost objektů z hlediska objektu třídy CDialog2.


Ckresli_posun

Strana 59

CDialog3

SetKoncetina() Ukladani() Nacitani() +m_obraz_xy Napisprikazy() +m_obraz_xz get_nacti_uhly() set_nacti_uhly() CDialog3() <> DoDataExchange() +m_addpole Pocitej() set_nacti_nsap() <> OnAdd() <> OnSelchangeStart() <> OnSelchangeEnd() CPrikaz <> OnRemove() <> OnSave() typ_q : int <> OnOpen() <> OnStart() CPrikaz() <> OnHScroll() SetPrikaz() <> OnReset() SetPrikaz() <> OnInitDialog() Getprikaz() Get_typ() <> Serialize() <> Posun() <> SetRobot()

+ppole

Novyrobot() CSimrobDlg() <> DoDataExchange() <> OnInitDialog() <> OnSysCommand() <> OnPaint() <> OnQueryDragIcon() ... +m_simrobdlg CDialog1 GetKoncetinaSim() Setdata() GetKoncetina() Getdata() Getdata() CDialog1() <> DoDataExchange() <> OnCreate() <> OnParametry() <> OnUloz() <> OnOtevri() +koncetina

CPrikazy m_addpolecount : int RemovePrikaz() AddPrikaz() <> Serialize()

CSimrobDlg m_hIcon : HICON

+m_prikazy

+m_koncetinasim

CKoncetinaSim nasobic : int krok : double CKoncetinaSim() CKoncetinaSim() Get_iterace() pohyb_uhel() pohyb_ptp() set_end_efektor() ...

Obr. 33 Provázanost objektů z hlediska objektu třídy CDialog3 5.3.3

Ukázky alokace a uvolňování paměti

V případě změny parametrů robota. To se provádí v dialogu. Parametry je potřeba vytvořit nový objekt třídy CKoncetinaSim. Tento objekt se vytvoří po stisknutí na tlačítko Create. Nejprve je potřeba zjistit, jestli už existuje předchozí objekt. Pokud existuje, pak je potřeba ho smazat, a tím uvolnit jím alokované místo v paměti. Toto zjištění se provede pomocí větvení, kde se testuje ukazatel na tento objekt. Pokud ukazatel není NULL, pak dojde k zavolání operátoru delete, který daný objekt smaže z paměti. Tím se ukončí tělo větvení. Dále se provede alokace a vytvoření nového objektu. K tomu slouží operátor new, adresa paměti se pak uloží do proměnné typu ukazatel na CKoncetinaSim. Nakonec se zavolá metoda Novyrobot v objektu, na který ukazuje ukazatel m_simrobdlg. Tato metoda pak rozdistribuuje adresu objektu robota do ostatních dialogů. void CDialog1::OnCreate() {

Strana 60

5 Realizace řídícího programu // TODO: Add your control notification handler code here Matrix B(12,23); Matrix C(6,23); Matrix D(6,23); //B=Getdata(); //C=B.rows(1,6); //D=B.rows(7,12); if (m_koncetina!=NULL) { delete m_koncetina; m_koncetina=NULL; } m_koncetina= new CKoncetinaSim(Getdata()); m_simrobdlg->Novyrobot();

} Dále si ukážeme správu paměti při vytváření příkazu v simulačním panelu. V tomto panelu je potřeba při stisku tlačítka Add, vytvořit nový příkaz a při stisku tlačítka Remove smazat posledně přidaný příkaz. Ukážeme si části kódu pro obsluhu těchto tlačítek. void CDialog3::OnAdd() { // TODO: Add your control notification handler code here UpdateData(); OnSelchangeStart(); OnSelchangeEnd(); ColumnVector q(6); q=set_nacti_uhly(); if (m_radio==0) //provede se v pripade vybraneho q { ColumnVector q(6); q=set_nacti_uhly(); s1.Format("StartLink=%d,EndLink=%d,q\r\n",StartLink,EndLink); m_addpolecount++; sprikaz[m_addpolecount]= new CString(); *sprikaz[m_addpolecount]=s1; m_prikazy.m_addpole[m_addpolecount]= new CPrikaz(); //alokace noveho prikazu m_prikazy.m_addpole[m_addpolecount]->SetPrikaz(q,EndLink,StartLink); } if (m_radio==1) //provede se v pripade vybraneho nsap { Matrix nsap(4,4); nsap=set_nacti_nsap(); s1.Format("StartLink=%d,EndLink=%d,NSAP\r\n",StartLink,EndLink); m_addpolecount++; sprikaz[m_addpolecount]= new CString(); *sprikaz[m_addpolecount]=s1; m_prikazy.m_addpole[m_addpolecount]= new CPrikaz(); m_prikazy.m_addpole[m_addpolecount]-SetPrikaz(nsap,EndLink,StartLink); } m_console.SetWindowText(Napisprikazy()); }


Strana 61

void CDialog3::OnRemove() { // TODO: Add your control notification handler code here if (m_addpolecount>=0) { delete m_prikazy.m_addpole[m_addpolecount]; delete sprikaz[m_addpolecount]; m_addpolecount--; m_console.SetWindowText(Napisprikazy()); } }

5.4

Otestování programu na datech

Otestování programu bylo provedeno na příkladu, který představuje pohyb nohy při chůzi. Pohyb nohy je složen ze šesti příkazů. Všechny příkazy jsou typu q tzn., že jsou zadány pomocí úhlů natočení v jednotlivých vazbách. Popis příkazů je zobrazen v následující tabulce. Příkaz

Start Link

End Link

Změna úhlů [rad]

1

1

6

q1=1,57;q5=-1,57

2

5

1

q5=-1,8

3

1

6

q3=2;q5=-2,2

4

1

6

q1=-1,23;q3=2;q5=-2,2

5

1

6

q1=-1,23;q3=1;q5=-1,57

6

5

1

q1=1,57;q5=-1,57

Tab. 7: Řídící příkazy pro simulaci chůze. První příkaz zafixuje první vazbu jako počátek fixovaný k okolí. Koncový efekor je nastaven na konec končetiny. V této konfiguraci se nastaví úhly q1 na 1,57 rad. a q5 na -1,57 rad. Dále dojde ke změně konfigurace na Start Link 5 a EndLink 1. Vazba q5 se nastaví na -1,8rad. Tím dojde k posunutí počátku končetiny. Pokud by byla končetina součástí kompletního humanoidního robota, došlo by k posunutí celého robota. Následuje opět změna konfigurace a nastavení vazeb q3 na 2 rad. a q5 na -2.2 rad. Končetina se tak zvedne, toto je nezbytné, pokud by robot překračoval překážku. Následuje přesun končetiny dopředu a její položení do výchozí konfigurace.

Obr. 34 Vizualizace jednotlivých fází pohybu Ze simulace si můžeme všimnout menšího stoupání, které končetina vykonává. Toto je způsobeno absencí druhé končetiny, která dodá potřebnou stabilitu. Při provádění poslední fáze bude druhá končetina provádět fázi dva. To zapříčiní posunutí těžiště celého robota dolu včetně první končetiny. Ta se tak dostane do stejné výškové pozice, jako byla na začátku.

Strana 63

6

ZÁVĚR

V diplomové práci byly probrány základní principy robotiky. Jako je Denavit-Hartnbergův princip, pomocí něhož byly sestaveny transformační matice. Z těchto transformačních matic je možno vypočítat přímou úlohu kinematiky. Další důležité výpočty řeší inverzní úloha kinematiky. Byla uvedena numerická metoda pro výpočet inverzní úlohy kinematiky aproximací transformační matice pomocí taylorova rozvoje. Podobnost humanoidního robotu s lidmi vedla k analýze pohybového aparátu lidí. Tato analýza byla použita k inspiraci při plánování kinematické struktury humanoidního robota, čímž byl naplněn cíl analýza pohybu humanoidního robota. Nejdůležitější částí takového robota jsou končetiny, což vedlo k jejich hlubšímu rozpracování. Výsledkem toho je vytvořený matematický model končetiny, jehož největším přínosem je možnost volby stacionárního bodu, vůči kterému se robot pohybuje a efektoru, jehož polohu sledujeme. Tato funkcionalita je hlavní odlišností od klasického manipulátoru typu RRRRRR, tedy manipulátoru se šesti stupni volnosti realizovaných pomocí rotačních vazeb, který je fixován ke svému okolí. Díky tomu lze simulovat a analyzovat různé pohyby končetiny vůči okolí, aniž by byl k dispozici matematický model celého humanoidního robota. Tímto byl dosažen cíl na vytvoření matematického modelu. Dosažení žádané polohy se provádí postupným natočením jednotlivých vazeb. Natáčení se řídí některou z metod řízení. Zmíněny byly dva způsoby řízení: metoda PTP a CP. Z těchto dvou byla implementována metoda PTP (Point To Point). Výhodou tohoto řízení je rychlost přesunu, neboť dochází ke změnám všech vazeb současně. Nevýhodu, která spočívá v neznalosti dráhy přesunu, lze minimalizovat rozfázováním pohybu na více jednotlivých pohybů. Pro dosažení cíle vytvoření softwaru pro řízení humanoidního robotu, byl vytvořen program Simrob. Jehož výhodou je implementace pomocí objektové technologie v jazyku C++. Tato konstrukce pak vytváří modulární strukturu, která umožňuje snadné rozšíření a údržbu. Například o metodu řízení CP nebo o nový vizualizační modul pro trojrozměrné zobrazení animace pohybu. Cíl ověření programu na konkrétních datech byl naplněn při realizaci konkrétního pohybu končetiny, který představuje pohyb končetiny při chůzi.

Strana 65

SEZNAM POUŽITÉ LITERATURY [1] Poliščuk, R.: Titulní strana závěrečné práce, , [2] Poliščuk, R.: Instrukce pro autory závěrečných prací, 2006, http://autnt.fme.vutbr.cz/doc/SZZ2006_Instrukce.pdf [3] Tesař, M.: Software pro řízení pohybové trajektorie průmyslových robotu, Diplomová práce, VUT-FSI, Brno, 2004. [4] Mostýn,V., Skařupa, J.:Teorie průmyslových robotů,2000, http://robot.vsb.cz/elekskripta/Teorie%20prumyslovych%20robotu.pdf [5] Očenášek, R.: Software pro řízení paralelní kinematické struktury, Diplomová práce ,VUT-FSI, Brno, 2006. [6] Function X.: Visual C++ and MFC Programming. 2th edition, 2004. [7] Cenek S.- Kučera, V.: Literatura pro III. Ročník středních škol. Pracovní antologie textů, Praha 1989, s. 223. [8] Katedra robotechniky VŠB -TUO. Co je robot. 2007. [cit. 2007-17-09]. Dostupné z [9] Davies, R.,B. Documentation for newmat 11, a matrix library in C++ . [online]. 2006, 16th of April 1996 [cit. 2007-17-08]. Dostupné z [10]Gourdeau, Richard. ROBOOP - A robotics object oriented package in C++. [HTML dokument] 10th October 2007 Dostupný z [11]Bělohoubek P.: Matematický aparát pro řízení 6-ti osých robotů. Výzkumná zpráva v rámci HS 380 208, VUT-FSI, Brno, 1999. [12]Prata, S.: Mistrovství v C++, Computer Press, Praha, 2001. [13]Bělohoubek, P.: Design and control of industrial robots optimization, Brno 2005. [14]Šolc, F.- Žalud, L.: Robotika, Brno 2002.

SOFTWARE PRO ŘÍZENÍ HUMANOIDNÍHO ROBOTU TITLE

Recommend Documents