0036442833 roko romic osnove robotike 1 lab

7/21/2019 0036442833 Roko Romic Osnove Robotike 1 Lab

1/14

Osnove Robotike Grupa C - RRTR

ROMI ROKO1_DAUT

AUTOMATIKA

0036442833

FAKULTET ELEKTROTEHNIKE I RAUNARSTVA

ZAGREB

ZAVOD ZA AUTOMATIKU I RAUNALNO

INENJERSTVO 27.10.2011.

OSNOVE ROBOTIKE

Vjeba br. 1:

Direktna i inverzna kinematika

manipulator

Uvod

Direktna kinematika

Robotski manipulator se sastoji od lanaka koji su povezani preko zglobova. Prvi zglobtj., nulti je postavljen u nepominu bazu oko koje se ostatak zglobova/lanaka prostorno(u tri

dimenzije) giba. Tako moemo definirati problem direktne kinematike:

Ako je zadan vektor varijabli zglobova robotskog manipulatora, tada treba odrediti

poloaj i orijentaciju alata u odnosu prema koordinatnom sustavu pridruenom bazi robota.

Za rjeavanje problema direktne kinematike potrebno nam je poznavanje rotacijsko i

translacijsko gibanje. Rotacija(transformacija koordinata) je potrebna za odreivanje orijentacije

alata. Kad pokretni sustav rotira okojedininog vektora nepokretnog sustava dobije se matrica

osnovne rotacije, tako postoje tri osnovne matrice rotacije dobivene rotirajui oko svaka od tri

jedinina vektora nepominog sustava. Takve matrice su definirane na slijedei nain:

R()=

332313

322212

312111

mfmfmf

mfmfmf

mfmfmf

(1)

Zaprikaz poloaja alata s obzirom na bazu manipulatora koristimo se translacijom.Razlika izmeu translacije i rotacije je to se kod translacije ishodite koordinatnog sustava

pomie du jedininog vektora, dok kod rotacije ishodite ostajeu istoj toci.

Za definiranje translacije potrebno je prijei u etverodimenzionalan prostor homogenih

koordinata. Kako bi se opisao poloaj i orijentacija vrha alata, definira se matrica homogene

transformacije:


2/14


R pT =

n (2)

pri emu matrica Rpredstavlja matrice rotacija, vektor ppredstavlja vektor translacije(poloaj

alata s obzirom na bazu sustava) ,n

je vektor perspektive0 vektor, sigma koja je najee 1.

Kinematiki parametri odreuju se prema sljedeem postupku:1. Pridruiti zglobovima brojeve od 1 do n, pri emu je poetni zglob 1 vezan uz

bazu, a zglobovi n-2, n-1, n vezani su uz orijentaciju alata.

2. Bazi robota pridruiti desno orijentirani ortonormirani koordinatni sustavL0 takoda se os z0podudara s osi zgloba 1 te poveati vrijednost k za 1.

3. Os zkpostaviti u smjeru osi zgloba k+1.4. Ishodite koordinatnog sustavaLkpostaviti u presjek osi zk i zk-1, a ako se one ne

sijeku, tada se treba koristiti presjekom osi zk i zajednike okomice na osi zk i zk-1.5. Postaviti os xk tako da bude okomita na osi zk i zk-1, a ako su te dvije osi paralelne,

tada postaviti os xk da pokazuje od zk-1prema zk.6. Os ykpostaviti tako da sa osima xk i zk ini desno orijentirani ortonormirani

koordinatni sustavLk.7. Vrijednost kpoveati za 1 te ukoliko je k


3/14


Ova transformacija radi na principu da svaki k-ti pokretni sustav preslika u pokretni

sustav k-1, mnoenjem takvih n(broj zglobova sustava) matrica dobije se matrica sloene

homogene transformacije koja se naziva matrica manipulatora ili ruke.

)()()...( 0111

0 qTqTqTT n

n

n

n

alat

baza (4)

Iz matrice manipulatora je mogue odrediti orijentaciju alata te poloaj alatas obzirom na bazusustava.

Inverzna kinematika

Vektor konfiguracije alata je definiran na slijedei nain:

, (5)

pri emu vektor p oznaava poloaj alata s obzirom na bazu, a orijentaciju vrha alata, priemu je kut rotacije alata. Vektor w ukupno ima est komponenti, vektor su spremljenekoordinate vrha alata (x,y,z) koordinate te orijentacija alata.

Problem inverzne kinematike moe se definirati na sljedei nain:

Ako su zadane vrijednosti poloaja p i orijentacije alata R, tada treba pronai vrijednosti

varijabli u prostoru zglobova Rn (qi, 1in) koje zadovoljavaju jednadbumanipulatora.

Inverzni kinematiki problem se posebno rjeava za svakog, odnosno za svaku grupu

slinih robota.

Dva najea postupka rjeavanja inverznog kinematikog problema su:

1. iterativni numeriki postupci,

2. analitika rjeenja.

Kod numerikih postupaka sadrana je mogunost izrade univerzalnog programa za razliite

konfiguracije robota, ali se zato javljaju problemi singulariteta.

Kod analitikih rjeenja se omoguava izravnan proraun vrijednosti varijabli zglobova, ali je

postupak nalaenja rjeenja puno sloeniji nego kod direktne kinematike koritenjem DH

metode. Problem kod inverzne kinematike je mogunost pojave vieznanih rjeenja, tj., dolazak

u krajnju toku kreui se razliitim trajektorijama.


4/14


1. Zadatak Implementiratiprogramski rjeava (solver) direktne kinematike

Da bi se uspjeno rjeio solver za ulazne parametre uzimaju se vektori q,d,a,alfa.Vektor q sadri informacije o rotaciji triju osi,d sadi informacije o translaciji jedne osi,za robot RRTR. Kao izlazni vektor dobije se vektor konfiguracije alata.Parametri q, d,a i alfa su definirani za robot RRTR na slijedei nain:

1. os theta1= q1 d1=d1 a1=0 alfa1=-pi/2

2. os theta2=q2 d2=0 a2=0 alfa2=-pi/23. os theta3=0 d3=q3 a3=0 alfa3=0

4. os theta4=q4 d4=d4 a4=0 alfa4=0

Solver se pokree pozivom m-skripte direktna_kinematika(q,d,a,alfa), pri emu suq,d,a,alfa zadani kao vektori, sa vrijednostima poetnih uvjeta. Npr., poetni uvjeti zavektor q glase q=[pi/2 pi/2 0 -pi] , d=[1 0 1 1] (kod parametra d, d(3) je promjenjiv doksu d(1) i d(4) proizvoljne konstantne), te su vektori a i alfa konstantni i iznose

a=[0 0 0 0], alfa=[-pi/2pi/2 0 0 ].

function[ w ] = direktna_kinematika(q,d,a,alfa)

T1=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1];

fori=1:4T= [cos(q(i)) -cos(alfa(i))*sin(q(i)) sin(alfa(i))*sin(q(i)) a(i)*cos(q(i));

sin(q(i)) cos(alfa(i))*cos(q(i)) -sin(alfa(i))*cos(q(i)) a(i)*sin(q(i));0 sin(alfa(i)) cos(alfa(i)) d(i);


5/14


0 0 0 1];T1=T1*T;

endT_alat=T1;w=[T_alat(1,4);T_alat(2,4);T_alat(3,4);exp(q(4)/pi)*T_alat(1,3);exp(q(4)/pi)*

T_alat(2,3);exp(q(4)/pi)*T_alat(3,3)] ;end

1.Programski kod za direktnu kinematiku

2. Zadatak Provjeri ti ispravnost rada solvera di rektne kinematike iscrtavanjem

manipulatora u najmanje osam pozicija (ukljuujui poziciju u kojoj je robot nacrtan u

zadatku 1. domae zadae); Za svaku poziciju navesti pripadajue vrijednosti varijablizglobova

Za provjeru ispravnosti solvera direktne kinematike iscrtavanjem manipulatora dobije se

koritenjem matlab toolboxa- robotics.

L(1) = Link([ pi/2 1 0 -pi/2 0], 'standard');L(2) = Link([ pi 0 0 -pi/2 0], 'standard');L(3) = Link([ 0 1 0 0 1], 'standard');L(4) = Link([ pi/2 1 0 0 0], 'standard');rrtr = SerialLink(L, 'name', 'RRTR');q=[-pi/2 -pi/2 0 pi/2];% mijenjaju se stanja prema elji

plot(rrtr,q)

2.Kod za iscrtavanje manipulatora


6/14


Slika 1.q1=, q2=

, q3=1, q4=

,

Slika 2. q1=, q2=, q3=1, q4=

,


7/14


Slika 3. q1=,, q2=, q3=1, q4=

,

Slika 4. q1=, q2=-, q3=1, q4=

,


8/14


Slika 5.q1=, q2=-, q3=3, q4=

,

Slika 6.q1=-//3, q2=/3, q3=1, q4=0


9/14


Slika 7. q1=,, q2=

,, q3=1, q4=0

Slika 8. q1= -

, q2=

, q3=0, q4=0


10/14


3. ZadatakImplementirati programski rjeava(solver) inverzne kinematke

Solver je raen analitikim nainom pronalaska rjeenja inverzne kinematike. Solver je tako realiziran da kao ulazne parametre prima vektore w koji je dobiven

prethodno koristei solver direktne kinematike, te vektora d, te se poziva preko

inverz_kinematika. Kao izlaz funkcije je vektor q, tj parametri q1, q2, q3, q4. Do izraza

koji raunaju parametre qpreko w sloenije je doi nego kod direktne kinematike, to se

vidi u slijedeim izrazima:

)2cos(

)2sin()1sin(

)2sin()1cos(

)2cos()34(1

)2sin()1sin()34(

)2sin()1cos()34(

4

4

4

6

5

4

3

2

1

q

qq

qq

qqdd

qqqd

qqqd

w

w

w

w

ww

e

e

e

q

q

q

(6)

iz prethodnog izraza dolazimo do konanih:

q1=arctgq2=arctg2(

(7)

q3=-d(4)-sin(q2)*(cos(q1)*w(1)+sin(q1)*w(2))-cos(q2)*(w(3)-d(1))

q4= 262

5

2

4ln www

function[q1 q2 q3 q4] =inverzna_kinematika (w,d)

q1=atan2(-w(2),-w(1));

if(w(2)==0 && w(1)==0)q1=pi+atan2(w(2),w(1));

elseif(w(2)


11/14


q1=atan2(-w(2),-w(1));elseif(w(2)>0 && w(1)0 && w(1)>0)q1=atan2(w(2),w(1));

end

q2=atan2(-(w(4)*cos(q1)+sin(q1)*w(5)),-w(6));q3=-d(4)-sin(q2)*(cos(q1)*w(1)+sin(q1)*w(2))-cos(q2)*(w(3)-d(1));q4=pi*log(sqrt(w(4)^2+w(5)^2+w(6)^2));end

3. Programski kod za inverznu kinematiku

4. Zadatak Provjeri ti ispravnost rada solvera inverzne kinematike na sljedei nain:

odabrati poetne vrijednosti varijabli zglobova, uvrstiti ih u solver direktne kinematike,

rezultat uvrstiti u solver inverzne kinematike i usporediti ga s poetnim

vrijednostima.Treba odabrati toliko poetnih vrijednosti , da svaki rotacij ski zglob bude

prikazan u sva 4 kvadranta, u svim moguim kombinacijama.

Rezultate prikaite grafiki.

Princip na kojemu se temelji provjera ispravnosti je da se za neki vektor q definiran u svaetiri kvadranta odrede sve mogue kombinacije poloaja robota u prostoru, te se za svakukombinaciju poetnih uvjeta dobije iz direktne kinematike vektor w. W se dalje uvrsti ufunkciju koja rauna inverz kinematike tako to se dobiju rjeenja vektora q '. Usporedbomq' sa poetnim uvjetima na poetku zadatke rauna se pogreka.

rand_q = [-3*pi/4, -pi/4, pi/4, 3*pi/4];

q3_0=[0 0 0 0];[a1 a2 a3 a4]= ndgrid(rand_q,rand_q,q3_0,rand_q);

z = [a1(:) a2(:) a3(:) a4(:)];pogres=0;fork=1:256

q=z(k,:);%parametri ulaznid=[1 0 1 1];alfa=[-pi/2 -pi/2 0 0];a=[0 0 0 0];T1=[1,0,0,0;0,1,0,0;0,0,1,0;0,0,0,1];


12/14


fori=1:4% racunanje direktne kinematikeT= [cos(q(i)) -cos(alfa(i))*sin(q(i)) sin(alfa(i))*sin(q(i))a(i)*cos(q(i));

sin(q(i)) cos(alfa(i))*cos(q(i)) -sin(alfa(i))*cos(q(i))a(i)*sin(q(i));

0 sin(alfa(i)) cos(alfa(i)) d(i);0 0 0 1];

T1=T1*T;

end

T_alat=T1;

w=[T_alat(1,4);T_alat(2,4);T_alat(3,4);exp(q(4)/pi)*T_alat(1,3);exp(q(4)/pi)*T_alat(2,3);exp(q(4)/pi)*T_alat(3,3)] ;

% racunanje inverz kinematike

q1=atan2(-w(2),-w(1));%{if (w(2)==0 && w(1)==0)

q1=pi+atan2(w(2),w(1));

elseif(w(2)0)

q1=atan2(w(2),w(1));

end

%}

q2=atan2(-(w(4)*cos(q1)+sin(q1)*w(5)),-w(6));

q3=-d(4)-sin(q2)*(cos(q1)*w(1)+sin(q1)*w(2))-cos(q2)*(w(3)-d(1));q4=pi*log(sqrt(w(4)^2+w(5)^2+w(6)^2));%racunanje pogreskeerr=(abs(q(2))-abs(q2));err1=(abs(q(1))-abs(q1));err3=(abs(q(4))-abs(q4));err2=(abs(q(3))-abs(q3));if( err>0.1 )

pogres=pogres+1;

plot(k,err,'*'), axis([0 256 0 1*pi]);hold on

endif( err1>0.1 )


13/14


pogres=pogres+1;plot(k,err1,'*'),axis([0 256 0 1*pi]);hold on

endif( err2>0.1 )

pogres=pogres+1;plot(k,err2,'*'),axis([0 256 0 1*pi]);

hold onendif( err3>0.1 )

pogres=pogres+1;plot(k,err3,'*'),axis([0 256 0 1*pi]);hold on

end

end

4.Programski kod za provjeru ispravnosti solvera

Slika 9. grafiki prikaz pogreaka solvera

Zakljuak

Provjera solvera direktne kinematike prikazano na slikama (slika1.-slika8.) grafiki daje

tone rezultate, dok sam solver daje pogrenekutove. Ti pogrenikutovi rezultat su funkcije

atan2(-w(2),-w(1)), koja za (w(2) i w(1) ) daje pogrene kvadrante za 0


14/14


rjeenja. Uporabom izraza (7) manipulator nema singularnih toaka, to u stvarnom sustavu ne

mora znaiti, direktno vezano za ovaj sluaj kada drugi zglob ne moe rotirati za 360 stupnjeva

oko vlastita zgloba.

0036442833 roko romic osnove robotike 1 lab

Documents