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:
7/21/2019 0036442833 Roko Romic Osnove Robotike 1 Lab
2/14
Osnove Robotike Grupa C - RRTR
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
7/21/2019 0036442833 Roko Romic Osnove Robotike 1 Lab
3/14
Osnove Robotike Grupa C - RRTR
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.
7/21/2019 0036442833 Roko Romic Osnove Robotike 1 Lab
4/14
Osnove Robotike Grupa C - RRTR
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);
7/21/2019 0036442833 Roko Romic Osnove Robotike 1 Lab
5/14
Osnove Robotike Grupa C - RRTR
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
7/21/2019 0036442833 Roko Romic Osnove Robotike 1 Lab
6/14
Osnove Robotike Grupa C - RRTR
Slika 1.q1=, q2=
, q3=1, q4=
,
Slika 2. q1=, q2=, q3=1, q4=
,
7/21/2019 0036442833 Roko Romic Osnove Robotike 1 Lab
7/14
Osnove Robotike Grupa C - RRTR
Slika 3. q1=,, q2=, q3=1, q4=
,
Slika 4. q1=, q2=-, q3=1, q4=
,
7/21/2019 0036442833 Roko Romic Osnove Robotike 1 Lab
8/14
Osnove Robotike Grupa C - RRTR
Slika 5.q1=, q2=-, q3=3, q4=
,
Slika 6.q1=-//3, q2=/3, q3=1, q4=0
7/21/2019 0036442833 Roko Romic Osnove Robotike 1 Lab
9/14
Osnove Robotike Grupa C - RRTR
Slika 7. q1=,, q2=
,, q3=1, q4=0
Slika 8. q1= -
, q2=
, q3=0, q4=0
7/21/2019 0036442833 Roko Romic Osnove Robotike 1 Lab
10/14
Osnove Robotike Grupa C - RRTR
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
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)
7/21/2019 0036442833 Roko Romic Osnove Robotike 1 Lab
11/14
Osnove Robotike Grupa C - RRTR
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];
7/21/2019 0036442833 Roko Romic Osnove Robotike 1 Lab
12/14
Osnove Robotike Grupa C - RRTR
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 )
7/21/2019 0036442833 Roko Romic Osnove Robotike 1 Lab
13/14
Osnove Robotike Grupa C - RRTR
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
7/21/2019 0036442833 Roko Romic Osnove Robotike 1 Lab
14/14
Osnove Robotike Grupa C - RRTR
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.